void binTriangulateLin(const double K0[9], const double R0[9],
const double t0[3], const double K1[9], const double R1[9],
const double t1[3], const double p[2], const double q[2], double x[3]) {
double iK0[9], iK1[9], m0[2], m1[2];
mat33Inv(K0, iK0);
mat33Inv(K1, iK1);
normPoint(iK0, p, m0);
normPoint(iK1, q, m1);
binTriangulateLin(R0, t0, R1, t1, m0, m1, x);
}
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 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 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);
}
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;
}
