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);
}
}
void updatePointUncertainty(MapPoint* mp, FeaturePoint* new_fp,
double pixelVar) {
if (mp->featPts.size() == 0)
return;
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);
getTriangulateCovMat((int) (nview + 1), Ks, Rs, ts, mp->M, mp->cov,
pixelVar);
}
