bool compareFeaturePt(const ImgG& scaledImg1, double imgScale1,
const ImgG& scaledImg2, double imgScale2, const FeaturePoint* pt1,
const FeaturePoint* pt2) {
NCCBlock nccblk1, nccblk2;
getScaledNCCBlock(scaledImg1, imgScale1, pt1->xo, pt1->yo, nccblk1);
getScaledNCCBlock(scaledImg2, imgScale2, pt2->xo, pt2->yo, nccblk2);
double ncc = matchNCCBlock(&nccblk1, &nccblk2);
if (ncc >= 0.6)
return true;
return false;
}
int SingleSLAM::regMapPoints(std::vector<MapPoint*>& mapPts,
PtrVec<NCCBlock>& nccblks, double maxEpiErr, double maxNcc,
double scale) {
CamPoseItem* cam = m_camPos.current();
assert(cam);
Mat_d featPts;
std::vector<FeaturePoint*> featPtsPtr;
getUnMappedFeatPts(featPtsPtr, &featPts);
int numMapPoints = mapPts.size();
int nRegistered = 0;
ImgG img;
scaleDownAvg(m_img, img, scale);
for (int i = 0; i < numMapPoints; i++) {
double ms0[2], var[4];
project(K.data, cam->R, cam->t, mapPts[i]->M, ms0);
if (ms0[0] < 0 || ms0[0] >= videoReader->_w || ms0[1] < 0
|| ms0[1] >= videoReader->_h)
continue;
getProjectionCovMat(K.data, cam->R, cam->t, mapPts[i]->M,
mapPts[i]->cov, var, Const::PIXEL_ERR_VAR);
int iNearest = searchNearestPoint(featPts, ms0[0], ms0[1], var,
maxEpiErr, 0);
if (iNearest < 0)
continue;
//compare the image blocks
double fx = featPts.data[2 * iNearest];
double fy = featPts.data[2 * iNearest + 1];
NCCBlock nb;
getNCCBlock(img, fx * scale, fy * scale, nb);
double ncc = matchNCCBlock(&nb, nccblks[i]);
if (ncc >= maxNcc) {
//find a projection
mapPts[i]->addFeature(camId, featPtsPtr[iNearest]);
featPtsPtr[iNearest]->mpt = mapPts[i];
nRegistered++;
}
}
return nRegistered;
}
