C++ Mat_d::resize方法代码示例

int NewMapPtsNCC::getSeedsBetween(MapPointList& mapPts, int iCam, int jCam,
		Mat_d& seed1, Mat_d& seed2) {
	MapPoint* pHead = mapPts.getHead();
	if (!pHead)
		repErr("SLAM failed - No map point can be found!\n");
	std::vector<FeaturePoint*> vecFeatPts1, vecFeatPts2;
	for (MapPoint* p = pHead; p; p = p->next) {
		//if ((p->iLocalType == TYPE_MAPPOINT_STATIC || p->iLocalType == TYPE_MAPPOINT_DYNAMIC) && p->numVisCam >= 2 && p->pFeatures[iCam] && p->pFeatures[iCam]->f == m_curFrame && p->pFeatures[jCam] && p->pFeatures[jCam]->f == m_curFrame) {
		if (!p->isFalse() && !p->isUncertain() && p->numVisCam >= 2
				&& p->pFeatures[iCam] && p->pFeatures[iCam]->f == m_curFrame
				&& p->pFeatures[jCam] && p->pFeatures[jCam]->f == m_curFrame) {
			vecFeatPts1.push_back(p->pFeatures[iCam]);
			vecFeatPts2.push_back(p->pFeatures[jCam]);
		}
	}
	if (vecFeatPts1.empty())
		return 0;

	seed1.resize(vecFeatPts1.size(), 2);
	seed2.resize(vecFeatPts2.size(), 2);

	for (size_t i = 0; i < vecFeatPts1.size(); i++) {
		seed1.data[2 * i] = vecFeatPts1[i]->x;
		seed1.data[2 * i + 1] = vecFeatPts1[i]->y;
		seed2.data[2 * i] = vecFeatPts2[i]->x;
		seed2.data[2 * i + 1] = vecFeatPts2[i]->y;
	}
	return seed1.rows;
}

int getMatchingFromTracks(int nTracks, const Track2D* vecPTracks, int f1, int f2, Mat_d& pts1, Mat_d& pts2) {
	if (f1 >= f2)
		repErr("getMatchingFrameTracks - f1 >= f2\n");

	pts1.resize(nTracks, 2);
	pts2.resize(nTracks, 2);

	int k = 0;
	for (int i = 0; i < nTracks; i++) {
		Track2DNode* p = vecPTracks[i].head.next;
		Track2DNode* tail = vecPTracks[i].tail->next;
		bool bFoundPts1 = false;
		while (p != tail) {
			if (p->f == f1) {
				pts1.data[2 * k] = p->x;
				pts1.data[2 * k + 1] = p->y;
				bFoundPts1 = true;
			}
			if (bFoundPts1 && p->f == f2) {
				pts2.data[2 * k] = p->x;
				pts2.data[2 * k + 1] = p->y;
				k++;
				break; //found a matching in the track, jump to the next track
			}
			p = p->next;
		}
	}
	pts1.rows = k;
	pts2.rows = k;
	return k;
}

void TracksToMeasurments(const double invK[][9], std::vector<const Track2D*> vecPTracks, int nviews, Mat_d& meas, Mat_c& vmask) {

	int nTrack = vecPTracks.size();
	int nMeas = 0;
	for (int i = 0; i < nTrack; i++) {
		nMeas += vecPTracks[i]->length();
	}

	meas.resize(nMeas, 2);
	vmask.resize(nTrack, nviews);
	vmask.fill(0);

	double tmpPt[2];
	int k = 0;
	for (int i = 0; i < nTrack; i++) {
		Track2DNode* p = vecPTracks[i]->head.next;
		Track2DNode* tail = vecPTracks[i]->tail->next;
		while (p != tail) {
			int f = p->f;
			tmpPt[0] = p->x;
			tmpPt[1] = p->y;
			normPoint(invK[f], tmpPt, meas + 2 * k);
			vmask.data[i * nviews + f] = true;
			k++;
			p = p->next;
		}
	}
}

void featPoint2Mat(std::vector<FeaturePoint*>& pFeatPoints, Mat_d& matPts) {
	size_t num = pFeatPoints.size();
	matPts.resize(num, 2);
	for (size_t i = 0; i < num; i++) {
		matPts.data[2 * i] = pFeatPoints[i]->x;
		matPts.data[2 * i + 1] = pFeatPoints[i]->y;
	}
}

void KpVec2Mat(const KpVec& vecPts , Mat_d& matPts) {
	int numPts = vecPts.size();
	matPts.resize(numPts, 2);
	for (int i = 0; i < numPts; i++) {
		matPts.data[2 * i] = vecPts[i].pt.x;
		matPts.data[2 * i + 1] = vecPts[i].pt.y;
	}
}

void matEyes(int n, Mat_d& mat) {
	mat.resize(n, n);
	mat.fill(0);
	int i;
	for (i = 0; i < n; ++i) {
		mat.data[i * n + i] = 1;
	}
}

void matRand(int m, int n, Mat_d& mat) {
	mat.resize(m, n);
	int i, len = m * n;
	for (i = 0; i < len; ++i) {
		double rnd_val = double(rand()) / RAND_MAX - 0.5;
		mat.data[i] = rnd_val;
	}
}

void cvMat2MyMat(const cv::Mat& cvMat , Mat_d& mat) {
	mat.resize(cvMat.rows, cvMat.cols);
	if (cvMat.type() == CV_64F) {
		CvMat tmpMat = cvMat;
		memcpy(mat.data, tmpMat.data.db, sizeof(double) * cvMat.rows * cvMat.cols);
	} else {
		CvMat tmpMat = cvMat;
		int len = cvMat.rows * cvMat.cols;
		for (int i = 0; i < len; i++) {
			mat.data[i] = tmpMat.data.fl[i];
		}
	}
}

bool registerToKeyFrame(int camId, const KeyPose* pose, const ImgG& curImg,
		vector<MapPoint*>& matchedMapPts, Mat_d& matched2DPts) {

	assert(pose && !pose->img.empty());
	assert(!curImg.empty());

	vector<MapPoint*> mappts;
	pose->getStaticMapPoints(mappts);
	Mat_d pts3d;

	vecMapPt2Mat(mappts, pts3d);
	if( pts3d.m  == 0)
		return false;

	Mat_d keyPts(pts3d.m, 2);

	project(pose->K, pose->cam->R, pose->cam->t, pts3d.m,
			pts3d.data, keyPts.data);

	//2.find the correspondences of the projected feature points in the current image
	Mat_d curPts;
	Mat_uc flag;

	int nMatched = trackFeatureKLT(pose->img, curImg, keyPts, curPts, flag,
			23, 5, 0.001);

	if (nMatched < 0.6 * keyPts.m)
		return false;

	matched2DPts.resize(nMatched, 2);
	int k = 0;
	for (int i = 0; i < pts3d.m; i++) {
		if (flag[i] > 0) {
			matched2DPts[2 * k] = curPts[2 * i];
			matched2DPts[2 * k + 1] = curPts[2 * i + 1];
			matchedMapPts.push_back(mappts[i]);
			k++;
		}
	}
	return true;
}

/* get points at frame f*/
void getPointsFromTracks(std::vector<const Track2D*>& vecPTracks, int f, Mat_d& pts) {
	pts.clear();
	size_t nTracks = vecPTracks.size();

	pts.resize(nTracks, 2);

	int k = 0;
	for (size_t i = 0; i < nTracks; i++) {
		Track2DNode* p = vecPTracks[i]->head.next;
		Track2DNode* tail = vecPTracks[i]->tail->next;
		while (p != tail) {
			if (p->f == f) {
				pts.data[2 * k] = p->x;
				pts.data[2 * k + 1] = p->y;
				k++;
				break;
			}
			p = p->next;
		}
	}
	pts.rows = k;
}

void matZeros(int m, int n, Mat_d& mat) {
	mat.resize(m, n);
	mat.fill(0);
}

void matOnes(int m, int n, Mat_d& mat) {
	mat.resize(m, n);
	mat.fill(1);
}