void SingleSLAM::propagateFeatureStates() {
for (int i = 0; i < m_tracker.m_nMaxCorners; i++) {
Track2D& tk = m_tracker.m_tks[i];
if (tk.empty())
continue;
Track2DNode* node = tk.tail;
if (node->pre) {
//propagate the type of feature points
node->pt->type = node->pre->pt->type;
if (node->pre->pt->mpt) {
MapPoint* pMapPt = node->pre->pt->mpt;
if (pMapPt->state != STATE_MAPPOINT_CURRENT)
continue;
//map correspondence propagation
if (!pMapPt->isFalse()) {
pMapPt->pFeatures[camId] = node->pt;
node->pt->mpt = pMapPt;
pMapPt->lastFrame = currentFrame();
if (pMapPt->isCertainStatic())
pMapPt->staticFrameNum++;
node->pt->reprojErr = node->pre->pt->reprojErr;
}
}
}
}
}
int NewMapPtsNCC::getSeedsBetween(MapPointList& mapPts, int iCam, int jCam,
Mat_d& seed1, Mat_d& seed2) {
MapPoint* pHead = mapPts.getHead();
if (!pHead)
repErr("SLAM failed - No map point can be found!\n");
std::vector<FeaturePoint*> vecFeatPts1, vecFeatPts2;
for (MapPoint* p = pHead; p; p = p->next) {
//if ((p->iLocalType == TYPE_MAPPOINT_STATIC || p->iLocalType == TYPE_MAPPOINT_DYNAMIC) && p->numVisCam >= 2 && p->pFeatures[iCam] && p->pFeatures[iCam]->f == m_curFrame && p->pFeatures[jCam] && p->pFeatures[jCam]->f == m_curFrame) {
if (!p->isFalse() && !p->isUncertain() && p->numVisCam >= 2
&& p->pFeatures[iCam] && p->pFeatures[iCam]->f == m_curFrame
&& p->pFeatures[jCam] && p->pFeatures[jCam]->f == m_curFrame) {
vecFeatPts1.push_back(p->pFeatures[iCam]);
vecFeatPts2.push_back(p->pFeatures[jCam]);
}
}
if (vecFeatPts1.empty())
return 0;
seed1.resize(vecFeatPts1.size(), 2);
seed2.resize(vecFeatPts2.size(), 2);
for (size_t i = 0; i < vecFeatPts1.size(); i++) {
seed1.data[2 * i] = vecFeatPts1[i]->x;
seed1.data[2 * i + 1] = vecFeatPts1[i]->y;
seed2.data[2 * i] = vecFeatPts2[i]->x;
seed2.data[2 * i + 1] = vecFeatPts2[i]->y;
}
return seed1.rows;
}
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;
}
