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; }