int NewMapPtsNCC::reconstructTracks(Track2D tracks[], int numTracks,
int curFrame, std::vector<MapPoint*>& mapPts, int minLen,
double maxRpErr) {
int num = 0;
for (int k = 0; k < numTracks; k++) {
if (tracks[k].length() >= minLen) {
Mat_d ms(numCams, 2);
Mat_d nms(numCams, 2);
Mat_d Ks(numCams, 9);
Mat_d Rs(numCams, 9);
Mat_d Ts(numCams, 3);
int npts = 0;
for (Track2DNode* pTkNode = tracks[k].head.next; pTkNode; pTkNode =
pTkNode->next) {
int camId = pTkNode->f;
ms.data[2 * npts] = pTkNode->x;
ms.data[2 * npts + 1] = pTkNode->y;
//normalize the image coordinates of the feature points
normPoint(m_invK[camId], ms.data + 2 * npts,
nms.data + 2 * npts);
memcpy(Ks.data + 9 * npts, m_K[camId], sizeof(double) * 9);
memcpy(Rs.data + 9 * npts, m_R[camId], sizeof(double) * 9);
memcpy(Ts.data + 3 * npts, m_t[camId], sizeof(double) * 3);
npts++;
}
double M[4];
triangulateMultiView(npts, Rs.data, Ts.data, nms.data, M);
bool outlier = false;
//check re-projection error
for (int i = 0; i < npts; i++) {
double rm[2];
project(Ks.data + 9 * i, Rs.data + 9 * i, Ts.data + 3 * i, M,
rm);
double err = dist2(ms.data + 2 * i, rm);
if (err > maxRpErr
|| isAtCameraBack(Rs.data + 9 * i, Ts.data + 3 * i, M)) {
outlier = true;
break;
}
}
//if it is a inlier, a new map point is generated
if (!outlier) {
MapPoint* pM = new MapPoint(M[0], M[1], M[2], curFrame);
mapPts.push_back(pM);
//get the triangulation covariance
getTriangulateCovMat(npts, Ks.data, Rs.data, Ts.data, M,
pM->cov, Const::PIXEL_ERR_VAR);
int nDyn = 0;
for (Track2DNode* pTkNode = tracks[k].head.next; pTkNode;
pTkNode = pTkNode->next) {
int iCam = m_camGroup.camIds[pTkNode->f];
FeaturePoint* fp = pTkNode->pt;
fp->reprojErr = reprojErrorSingle(fp->K, fp->cam->R,
fp->cam->t, M, fp->m);
pM->addFeature(iCam, fp);
if (fp->type == TYPE_FEATPOINT_DYNAMIC)
nDyn++;
}
if (nDyn > 1) {
pM->setLocalDynamic();
}
// else if (nDyn == 1)
// pM->type = TYPE_MAPPOINT_UNCERTAIN;
// else{
// pM->type = TYPE_MAPPOINT_UNCERTAIN;
// pM->newPt = true;
// }
else
pM->setUncertain();
num++;
}
}
}
return num;
}
int NewMapPtsSURF::reconstructTracks(Track2D tracks[], int numTracks, int curFrame, std::vector<MapPoint*>& mapPts, //new map points that are generated
int minLen,
double maxRpErr) {
std::vector<FeaturePoint*> reconFeatPts[SLAM_MAX_NUM];
int num = 0;
for (int k = 0; k < numTracks; k++) {
if (tracks[k].length() >= minLen) {
Mat_d ms(numCams, 2);
Mat_d nms(numCams, 2);
Mat_d Ks(numCams, 9);
Mat_d Rs(numCams, 9);
Mat_d Ts(numCams, 3);
int npts = 0;
for (Track2DNode* pTkNode = tracks[k].head.next; pTkNode; pTkNode = pTkNode->next) {
int camId = m_camGroup.camIds[pTkNode->f];
ms.data[2 * npts] = pTkNode->x;
ms.data[2 * npts + 1] = pTkNode->y;
//normalize the image coordinates of the feature points
normPoint(m_invK[camId], ms.data + 2 * npts, nms.data + 2 * npts);
memcpy(Ks.data + 9 * npts, m_K[camId], sizeof(double) * 9);
memcpy(Rs.data + 9 * npts, m_R[camId], sizeof(double) * 9);
memcpy(Ts.data + 3 * npts, m_t[camId], sizeof(double) * 3);
npts++;
}
double M[4];
triangulateMultiView(npts, Rs.data, Ts.data, nms.data, M);
bool outlier = false;
//check re-projection error
for (int i = 0; i < npts; i++) {
double rm[2];
project(Ks.data + 9 * i, Rs.data + 9 * i, Ts.data + 3 * i, M, rm);
double err = dist2(ms.data + 2 * i, rm);
if (err > maxRpErr || isAtCameraBack(Rs.data + 9 * i, Ts.data + 3 * i, M)) {
outlier = true;
break;
}
}
//if it is a inlier, a new map point is generated
if (!outlier) {
MapPoint* pM = new MapPoint(M[0], M[1], M[2], curFrame);
mapPts.push_back(pM);
//get the triangulation covariance
getTriangulateCovMat(npts, Ks.data, Rs.data, Ts.data, M, pM->cov, Const::PIXEL_ERR_VAR);
for (Track2DNode* pTkNode = tracks[k].head.next; pTkNode; pTkNode = pTkNode->next) {
int camId = m_camGroup.camIds[pTkNode->f];
FeaturePoint* fp = pTkNode->pt;
reconFeatPts[pTkNode->f].push_back(fp);
fp->reprojErr = reprojErrorSingle(fp->K, fp->cam->R, fp->cam->t, M, fp->m);
pM->addFeature(camId, fp);
}
pM->setUncertain();
num++;
}
}
}
//test
logInfo("%d new map points are generated!\n", num);
for (int c = 0; c < numCams; c++) {
int camId = m_camGroup.camIds[c];
pCoSLAM->slam[camId].feedExtraFeatPtsToTracker(reconFeatPts[camId]);
}
return num;
}
int SingleSLAM::newMapPoints(std::vector<MapPoint*>& mapPts, double maxEpiErr,
double maxNcc) {
std::vector<FeaturePoint*> vecFeatPts;
getUnMappedAndTrackedFeatPts(vecFeatPts, 0, Param::nMinFeatTrkLen);
mapPts.clear();
mapPts.reserve(4096);
double M[3], m1[2], m2[2];
//reconstruct 3D map points
int numRecons = 0;
for (size_t k = 0; k < vecFeatPts.size(); k++) {
FeaturePoint* cur_fp = vecFeatPts[k];
FeaturePoint* pre_fp = cur_fp;
while (pre_fp->preFrame && pre_fp->preFrame->cam) {
if (pre_fp->type == TYPE_FEATPOINT_DYNAMIC) {
break;
}
pre_fp = pre_fp->preFrame;
}
if (pre_fp->type == TYPE_FEATPOINT_DYNAMIC || !pre_fp->cam)
continue;
normPoint(iK.data, pre_fp->m, m1);
normPoint(iK.data, cur_fp->m, m2);
//triangulate the two feature points to get the 3D point
binTriangulate(pre_fp->cam->R, pre_fp->cam->t, cur_fp->cam->R,
cur_fp->cam->t, m1, m2, M);
if (isAtCameraBack(cur_fp->cam->R, cur_fp->cam->t, M))
continue;
double cov[9], org[3];
getBinTriangulateCovMat(K.data, pre_fp->cam->R, pre_fp->cam->t, K.data,
cur_fp->cam->R, cur_fp->cam->t, M, cov, Const::PIXEL_ERR_VAR);
getCameraCenter(cur_fp->cam->R, cur_fp->cam->t, org);
double s = fabs(cov[0] + cov[4] + cov[8]);
if (dist3(org, M) < sqrt(s))
continue;
//check the reprojection error
double err1 = reprojErrorSingle(K.data, pre_fp->cam->R, pre_fp->cam->t,
M, pre_fp->m);
double err2 = reprojErrorSingle(K.data, cur_fp->cam->R, cur_fp->cam->t,
M, cur_fp->m);
if (err1 < maxEpiErr && err2 < maxEpiErr) {
//a new map point is generated
refineTriangulation(cur_fp, M, cov);
err1 = reprojErrorSingle(K.data, pre_fp->cam->R, pre_fp->cam->t, M,
pre_fp->m);
err2 = reprojErrorSingle(K.data, cur_fp->cam->R, cur_fp->cam->t, M,
cur_fp->m);
if (isAtCameraBack(cur_fp->cam->R, cur_fp->cam->t, M)
|| isAtCameraBack(pre_fp->cam->R, pre_fp->cam->t, M))
continue;
if (err1 < maxEpiErr && err2 < maxEpiErr) {
MapPoint* pM = new MapPoint(M[0], M[1], M[2], pre_fp->f);
doubleArrCopy(pM->cov, 0, cov, 9);
mapPts.push_back(pM);
pM->lastFrame = cur_fp->f;
for (FeaturePoint* p = cur_fp; p; p = p->preFrame)
p->mpt = pM;
//add the feature point
pM->addFeature(camId, cur_fp);
pM->setLocalStatic();
//compute the NCC block
pM->nccBlks[camId].computeScaled(m_lastKeyPos->imgSmall,
m_lastKeyPos->imgScale, cur_fp->x, cur_fp->y);
int x = max(0, min((int) cur_fp->x, m_rgb.w - 1));
int y = max(0, min((int) cur_fp->y, m_rgb.h - 1));
pM->setColor(m_rgb(x, y));
numRecons++;
}
}
}
return numRecons;
}
