bool isRegistable(MapPoint* mp, FeaturePoint* new_fp, double pixelVar) { if (mp->featPts.size() == 0) return false; size_t nview = mp->featPts.size(); Mat_d Ks(nview + 1, 9), Rs(nview + 1, 9), ts(nview + 1, 3), ms(nview + 1, 2), nms(nview + 1, 2); double iK[9]; for (size_t c = 0; c < nview; c++) { const FeaturePoint* fp = mp->featPts[c]; assert(fp->K && fp->cam); const double* K = fp->K; const double* R = fp->cam->R; const double* t = fp->cam->t; const double* m = fp->m; doubleArrCopy(Ks, c, K, 9); doubleArrCopy(Rs, c, R, 9); doubleArrCopy(ts, c, t, 3); doubleArrCopy(ms, c, m, 2); getInvK(K, iK); normPoint(iK, m, nms + 2 * c); } doubleArrCopy(Ks, nview, new_fp->K, 9); doubleArrCopy(Rs, nview, new_fp->cam->R, 9); doubleArrCopy(ts, nview, new_fp->cam->t, 3); doubleArrCopy(ms, nview, new_fp->m, 2); normPoint(iK, new_fp->m, nms + 2 * nview); double M[3], m[2]; triangulateMultiView((int) (nview + 1), Rs, ts, nms, M); //check the re-projection error for (size_t c = 0; c < nview; c++) { const FeaturePoint* fp = mp->featPts[c]; assert(fp->K && fp->cam); project(fp->K, fp->cam->R, fp->cam->t, M, m); if (dist2(m, fp->m) > pixelVar) return false; } project(new_fp->K, new_fp->cam->R, new_fp->cam->t, M, m); if (dist2(m, new_fp->m) > pixelVar) return false; return true; }
void SingleSLAM::refineTriangulation(const FeaturePoint* fp, double M[3], double cov[9]) { double Ks[18], Rs[18], ts[6], ms[4], nms[4]; double iK[9]; getInvK(K.data, iK); const double* R0 = fp->cam->R; const double* t0 = fp->cam->t; doubleArrCopy(Ks, 0, K.data, 9); doubleArrCopy(Rs, 0, R0, 9); doubleArrCopy(ts, 0, t0, 3); doubleArrCopy(ms, 0, fp->m, 2); normPoint(iK, fp->m, nms); double C0[3]; getCameraCenter(R0, t0, C0); const FeaturePoint* maxFp = 0; double maxAngle = 0; //search a camera pose that has the largest difference from the view direction (R0,t0) fp = fp->preFrame; while (fp && fp->cam) { double C[3]; getCameraCenter(fp->cam->R, fp->cam->t, C); double angle = getAbsRadiansBetween(M, C0, C); if (angle > maxAngle) { maxAngle = angle; maxFp = fp; } fp = fp->preFrame; } if (maxFp) { doubleArrCopy(Ks, 1, K.data, 9); doubleArrCopy(Rs, 1, maxFp->cam->R, 9); doubleArrCopy(ts, 1, maxFp->cam->t, 3); doubleArrCopy(ms, 1, maxFp->m, 2); normPoint(iK, maxFp->m, nms + 2); triangulateMultiView(2, Rs, ts, nms, M); getTriangulateCovMat(2, Ks, Rs, ts, M, cov, Const::PIXEL_ERR_VAR); } }