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;
}
void GLImagePane::mouseDown(wxMouseEvent& event) {
int x = event.GetX();
int y = event.GetY();
double fm[2] = { imgWidth * x * 1.0 / getWidth(), imgHeight * y * 1.0
/ getHeight() };
FeaturePoint* pFeatPt = searchNearestFeatPt(
m_pSLAM->slam[m_camId].m_featPts, m_pSLAM->curFrame, fm, 7.0);
if (pFeatPt) {
s_clicked_camid = m_camId;
s_clicked_featPt = pFeatPt;
s_clicked_mappt = pFeatPt->mpt;
if (pFeatPt->mpt && pFeatPt->mpt->state == STATE_MAPPOINT_CURRENT) {
replaceMapPointFlag(m_pSLAM->curMapPts, FLAG_MAPPOINT_TEST4,
FLAG_MAPPOINT_NORMAL);
pFeatPt->mpt->flag = FLAG_MAPPOINT_TEST4;
// //test
pFeatPt->mpt->print();
double M[3], cov[9];
// isDynamicPoint(2,pFeatPt->mpt,2.0,M,cov);
//print reprojection errors
MapPoint* p = pFeatPt->mpt;
logInfo("newpt : %s\n", p->bNewPt ? "yes" : "no");
for (int i = 0; i < SLAM_MAX_NUM; i++) {
FeaturePoint* fp = p->pFeatures[i];
if (fp) {
CamPoseItem* cam = fp->cam;
double err = reprojErrorSingle(m_pSLAM->slam[i].K.data,
cam->R, cam->t, p->M, fp->m);
logInfo("[view#%d]error:%lf ", i, err);
double rm[2], var[4], ivar[4];
project(fp->K, fp->cam->R, fp->cam->t, p->M, rm);
getProjectionCovMat(fp->K, fp->cam->R, fp->cam->t, p->M,
p->cov, var, Const::PIXEL_ERR_VAR);
mat22Inv(var, ivar);
double err2 = mahaDist2(rm, fp->m, ivar);
logInfo("maha error:%lf\n", err2);
printMat(3, 3, cam->R);
printMat(3, 1, cam->t);
logInfo(
"is at the cam#%d's back : '%s'\n----------------------------------------\n",
i,
isAtCameraBack(cam->R, cam->t, p->M) ?
"yes" : "no");
//get the number of previous frames
// int num = 0;
// while (fp != 0) {
// num++;
// fp = fp->preFrame;
// }
// logInfo("[%d]number :%d\n", i, num);
}
}
if (isStaticPoint(m_pSLAM->numCams, p, 6.0, M, cov, 10)) {
//test
printf("static\n");
}
if (isDynamicPoint(m_pSLAM->numCams, p, 6.0, M, cov)) {
//test
printf("dynamic\n");
}
printf("static frame num:%d\n", p->staticFrameNum);
//test map registration
// Mat_d camDist;
// m_pSLAM->getCurrentCamDist(camDist);
// MyApp::bDebug = true;
// int groupId = m_pSLAM->m_groupId[m_camId];
// bool bReg = m_pSLAM->currentStaticPointRegisterInGroup(m_pSLAM->m_groups[groupId], camDist, pFeatPt->mpt,
// 10.0);
// logInfo("register:%d\n", bReg ? 1 : 0);
MyApp::redrawViews();
}
MyApp::redrawViews();
} else {
s_clicked_camid = -1;
s_clicked_featPt = 0;
s_clicked_mappt = 0;
}
}
