int PointRegister::process(double pixelErrVar) {
/*==============(critical section)=================*/
enterBACriticalSection();
bool empty = slam.actMapPts.empty();
leaveBACriticalSection();
if (empty)
return 0;
/*------------------------------------------------*/
vector<MapPoint*> vecActMapPts;
vecActMapPts.reserve(slam.actMapPts.size() * 2);
/*==============(critical section)=================*/
enterBACriticalSection();
typedef std::list<MapPoint*>::iterator MapPointListIter;
for (MapPointListIter iter = slam.actMapPts.begin();
iter != slam.actMapPts.end(); iter++) {
MapPoint* p = *iter;
vecActMapPts.push_back(p);
}
slam.actMapPts.clear();
slam.nActMapPts = (int) vecActMapPts.size();
leaveBACriticalSection();
/*------------------------------------------------*/
/*get the mask and unmapped feature points for registration*/
genMappedMask();
getUnMappedFeatPoints();
vector<bool> flagReged;
flagReged.resize((size_t) slam.nActMapPts, false);
/*==============(critical section)=================*/
enterBACriticalSection();
nRegTried = 0;
nRegDeepTried = 0;
for (int i = 0; i < slam.nActMapPts; i++) {
MapPoint* p = vecActMapPts[i];
if (!p->isFalse()) {
flagReged[i] = regOnePoint(p, pixelErrVar);
}
}
leaveBACriticalSection();
/*------------------------------------------------*/
/*==============(critical section)=================*/
enterBACriticalSection();
int nReg = 0;
for (size_t i = 0; i < slam.nActMapPts; i++) {
MapPoint* p = vecActMapPts[i];
if (flagReged[i] == false) {
slam.actMapPts.push_back(p);
} else {
p->lastFrame = slam.curFrame;
p->state = STATE_MAPPOINT_CURRENT;
slam.curMapPts.push_back(p);
nReg++;
}
}
leaveBACriticalSection();
return nReg;
}
bool Relocalizer::tryToRecover() {
//CameraTracker& tracker = slam.tracker;
int camId = _camId;
SingleSLAM& tracker = _coSLAM->slam[camId];
enterBACriticalSection();
KeyPose* kf = searchKeyPosebyThumbImage(SLAMParam::MAX_COST_KEY_FRM);
leaveBACriticalSection();
if (!kf)
return false;
vector<MapPoint*> matchedMapPts;
Mat_d matched2DPts, matched2DPts_dist;
cout << "key frame matched!" << endl;
bool bReg = registerToKeyFrame(camId, kf, tracker.m_img, matchedMapPts,
matched2DPts);
if (!bReg)
return false;
cout << "success in registering to the key pose!" << endl;
tracker.getDistortedCoord(matched2DPts, matched2DPts_dist);
//add to feature point list
/*==============(critical section)=================*/
enterBACriticalSection();
vector<FeaturePoint*> vecFeatPts;
int frameId = _coSLAM->curFrame;
// Remove the tracked feature points in current frame
tracker.m_featPts.removeFrame(frameId);
for (int i = 0; i < matched2DPts.m; i++) {
FeaturePoint* fp = tracker.m_featPts.add(frameId, camId,
matched2DPts_dist[2 * i], matched2DPts_dist[2 * i + 1],
matched2DPts[2 * i], matched2DPts[2 * i + 1]);
assert (matchedMapPts[i]);
fp->mpt = matchedMapPts[i];
fp->mpt->lastFrame = _coSLAM->curFrame;
fp->mpt->state = STATE_MAPPOINT_CURRENT;
fp->mpt->addFeature(_camId, fp);
_coSLAM->curMapPts.add(fp->mpt);
vecFeatPts.push_back(fp);
}
leaveBACriticalSection();
tracker.m_tracker.reset(tracker.m_img, vecFeatPts, _coSLAM->slam[_camId].m_featPts);
double R[9], t[3];
return tracker.poseUpdate3D(kf->cam->R, kf->cam->t, R, t,false);
}
//#define DEBUG_MODE
int SingleSLAM::poseUpdate3D(bool largeErr) {
propagateFeatureStates();
//get the feature points corresponding to the map points
std::vector<Track2DNode*> nodes;
int num = getStaticMappedTrackNodes(nodes);
if (num < 1) {
double* cR = m_camPos.current()->R;
double* cT = m_camPos.current()->t;
CamPoseItem* camPos = m_camPos.add(currentFrame(), camId,cR, cT);
updateCamParamForFeatPts(K, camPos);
warn(
"[camera id:%d]intra-camera pose update failed! less than five static map points (%d)",
camId, num);
//leaveBACriticalSection();
//CoSLAM::ptr->pause();
//enterBACriticalSection();
return -1;
}
//choose the feature points for pose estimation
std::vector<FeaturePoint*> featPts;
chooseStaticFeatPts(featPts);
std::vector<FeaturePoint*> mappedFeatPts;
mappedFeatPts.reserve(nRowBlk * nColBlk * 2);
for (size_t i = 0; i < featPts.size(); i++) {
if (featPts[i]->mpt)
mappedFeatPts.push_back(featPts[i]);
}
//get the 2D-3D corresponding points
int n3D2Ds = mappedFeatPts.size();
Mat_d ms(n3D2Ds, 2), Ms(n3D2Ds, 3), covs(n3D2Ds, 9);
for (int i = 0; i < n3D2Ds; i++) {
FeaturePoint* fp = mappedFeatPts[i];
ms.data[2 * i] = fp->x;
ms.data[2 * i + 1] = fp->y;
Ms.data[3 * i] = fp->mpt->x;
Ms.data[3 * i + 1] = fp->mpt->y;
Ms.data[3 * i + 2] = fp->mpt->z;
memcpy(covs.data, fp->mpt->cov, sizeof(double) * 9);
}
Mat_d R(3, 3), t(3, 1);
double* cR = m_camPos.current()->R;
double* cT = m_camPos.current()->t;
IntraCamPoseOption opt;
//test
Mat_d old_errs(n3D2Ds, 1);
//get reprojection error beform pose update
for (int i = 0; i < n3D2Ds; i++) {
FeaturePoint* fp = mappedFeatPts[i];
double m[2];
project(K.data, cR, cT, fp->mpt->M, m);
old_errs[i] = dist2(m, fp->m);
}
//end of test
intraCamEstimate(K.data, cR, cT, Ms.rows, 0, Ms.data, ms.data,
Param::maxErr, R.data, t.data, &opt);
Mat_d new_errs(n3D2Ds, 1);
for (int i = 0; i < n3D2Ds; i++) {
FeaturePoint* fp = mappedFeatPts[i];
double m[2];
project(K.data, R, t, fp->mpt->M, m);
new_errs[i] = dist2(m, fp->m);
}
//find outliers
int numOut = 0;
double errThres = largeErr ? 6.0 : 2.0;
double rm[2], var[4], ivar[4];
for (int i = 0; i < num; i++) {
double* pM = nodes[i]->pt->mpt->M;
double* pCov = nodes[i]->pt->mpt->cov;
project(K, R, t, pM, rm);
getProjectionCovMat(K, R, t, pM, pCov, var, Const::PIXEL_ERR_VAR);
mat22Inv(var, ivar);
double err = mahaDist2(rm, nodes[i]->pt->m, ivar);
if (err < errThres) { //inlier
nodes[i]->pt->reprojErr = err;
seqTriangulate(K, R, t, nodes[i]->pt->m, pM, pCov,
Const::PIXEL_ERR_VAR);
project(K, R, t, pM, rm);
getProjectionCovMat(K, R, t, pM, pCov, var, Const::PIXEL_ERR_VAR);
mat22Inv(var, ivar);
err = mahaDist2(rm, nodes[i]->pt->m, ivar);
// if (err >= 1) {
// nodes[i]->pt->mpt->setUncertain();
// //test
// printf("poseUpdate:1\n");
// }
} else {
//outliers
numOut++;
double repErr = dist2(rm, nodes[i]->pt->m);
nodes[i]->pt->reprojErr = repErr;
nodes[i]->pt->mpt->setUncertain();
}
}
CamPoseItem* camPos = m_camPos.add(currentFrame(), camId, R.data, t.data);
updateCamParamForFeatPts(K, camPos);
// if (currentFrame() > 9)
// MyApp::bStop = true;
#ifdef DEBUG_MODE
//if the number of outliers are too much (may due to some distant points)
if (currentFrame() >= 76) {
//test
printf("f:%d,cam:%d : n3d2d:%d, num:%d, numOut:%d\n", currentFrame(),
camId, n3D2Ds, num, numOut);
char dirPath[1024];
sprintf(dirPath, "/home/tsou/slam_posefailed/%s", MyApp::timeStr);
mkdir(dirPath, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
savePGM(m_img, "/home/tsou/slam_posefailed/%s/%d_img_%d.pgm",
MyApp::timeStr, currentFrame(), camId);
writeMat(Ms, "/home/tsou/slam_posefailed/%s/%d_pts3d_%d.txt",
MyApp::timeStr, currentFrame(), camId);
writeMat(ms, "/home/tsou/slam_posefailed/%s/%d_pts2d_%d.txt",
MyApp::timeStr, currentFrame(), camId);
writeMat(covs, "/home/tsou/slam_posefailed/%s/%d_cov3d_%d.txt",
MyApp::timeStr, currentFrame(), camId);
writeMat(3, 3, K, "/home/tsou/slam_posefailed/%s/%d_K_%d.txt",
MyApp::timeStr, currentFrame(), camId);
writeMat(old_errs, "/home/tsou/slam_posefailed/%s/%d_old_errs_%d.txt",
MyApp::timeStr, currentFrame(), camId);
writeMat(new_errs, "/home/tsou/slam_posefailed/%s/%d_new_errs_%d.txt",
MyApp::timeStr, currentFrame(), camId);
writeMat(3, 3, cR, "/home/tsou/slam_posefailed/%s/%d_oldR_%d.txt",
MyApp::timeStr, currentFrame(), camId);
writeMat(3, 1, cT, "/home/tsou/slam_posefailed/%s/%d_oldT_%d.txt",
MyApp::timeStr, currentFrame(), camId);
writeMat(R, "/home/tsou/slam_posefailed/%s/%d_R_%d.txt", MyApp::timeStr,
currentFrame(), camId);
writeMat(t, "/home/tsou/slam_posefailed/%s/%dT_%d.txt", MyApp::timeStr,
currentFrame(), camId);
//test
logInfo("program paused for debug at camera %d!\n", currentFrame(), camId);
leaveBACriticalSection();
CoSLAM::ptr->pause();
enterBACriticalSection();
}
#endif
return num;
}
