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;
}
}
}
}
}
void SingleSLAM::saveFeatureTracks(const char* filePath, int minLen,
int startFrame) {
FILE* fp = fopen(filePath, "w");
if (!fp)
repErr("SingleSLAM::saveCamPoses -- cannot open '%s'!", filePath);
for (int i = 0; i < m_tracker.m_nMaxCorners; i++) {
Track2D& tk = m_tracker.m_tks[i];
if (tk.empty() || tk.length() < minLen)
continue;
if (tk.tail->pt->type == TYPE_FEATPOINT_DYNAMIC)
continue;
Track2DNode* node = tk.tail;
MapPoint* mpt = node->pt->mpt;
if (mpt && mpt->isCertainStatic())
fprintf(fp, "1 %g %g %g\n", mpt->M[0], mpt->M[1], mpt->M[2]);
else
fprintf(fp, "0 0 0 0\n");
//output the feature points
std::vector<FeaturePoint*> tmpFeatPts;
for (FeaturePoint* featPt = node->pt; featPt && featPt->f >= startFrame;
featPt = featPt->preFrame) {
tmpFeatPts.push_back(featPt);
}
for (std::vector<FeaturePoint*>::reverse_iterator iter =
tmpFeatPts.rbegin(); iter != tmpFeatPts.rend(); iter++) {
FeaturePoint* featPt = *iter;
fprintf(fp, "%d %g %g ", featPt->f, featPt->x, featPt->y);
}
fprintf(fp, "\n");
}
fclose(fp);
}
