MRPT API 学习（二）——特征点获取，匹配，保存与上一次特征点相匹配的特征点

//过滤超出2D图像范围的特征点

void FiltInvalidFeats(CFeatureList&feats)

{CFeatureList::iterator itFeat = feats.begin();while (itFeat!=feats.end()){const TFeatureTrackStatus status = (*itFeat)->track_status;bool eras = (status_TRACKED!=status && status_IDLE!=status);if (!eras){// Also, check if it's too close to the image border:const float x= (*itFeat)->x;const float y= (*itFeat)->y;static const float MIN_DIST_MARGIN_TO_STOP_TRACKING = 10;if (x<MIN_DIST_MARGIN_TO_STOP_TRACKING  || y<MIN_DIST_MARGIN_TO_STOP_TRACKING ||x>(COL-MIN_DIST_MARGIN_TO_STOP_TRACKING) ||y>(ROW-MIN_DIST_MARGIN_TO_STOP_TRACKING)){eras = true;}}if (eras)// Erase or keep?itFeat = feats.erase(itFeat);else ++itFeat;}}

//获得当前帧的3D特征点map<TFeatureID, TPoint3D> GetCur3DFeats(CFeatureList feats,CObservation3DRangeScanPtr obs){map<TFeatureID, TPoint3D> curVisibleFeats;for (CFeatureList::iterator itFeat = feats.begin();itFeat!=feats.end();++itFeat){// Pixel coordinates in the intensity image:const int int_x= (*itFeat)->x;const int int_y= (*itFeat)->y;// Convert to pixel coords in the range image://  APPROXIMATION: Assume coordinates are equal (that's not exact!!)const int x = int_x;const int y = int_y;// Does this (x,y) have valid range data?const float d = obs->rangeImage(y,x);

//过滤深度不在范围内的点if (d>0.05 && d<10.0){ASSERT_( size_t(obs->rangeImage.cols()*obs->rangeImage.rows()) == obs->points3D_x.size() )const size_t nPt = obs->rangeImage.cols() * y + x;curVisibleFeats[(*itFeat)->ID] = TPoint3D( obs->points3D_x[nPt],obs->points3D_y[nPt],obs->points3D_z[nPt] );}}return curVisibleFeats;}

//将与上一次特征点相匹配的特征点到历史特征点，由于隔帧处理，相匹配的特征点可以认为是静止的，也可以看作是landmarkvoid AddValidFeaturesToHistory(map<TFeatureID, TPoint3D>& historyVisibleFeats,map<TFeatureID, TPoint3D>  preVisibleFeats,map<TFeatureID, TPoint3D>  curVisibleFeats){static TFeatureID featureID = 0;for (map<TFeatureID, TPoint3D>::const_iterator itCur= curVisibleFeats.begin();itCur!=curVisibleFeats.end();++itCur){map<TFeatureID, TPoint3D>::const_iterator itFound = preVisibleFeats.find(itCur->first);if (itFound!=preVisibleFeats.end()){historyVisibleFeats.insert(make_pair(featureID,itFound->second));featureID++;}}}

//特征点获取，匹配，保存与上一次特征点相匹配的特征点全过程，隔帧处理void ImageProcess(ImageData &imagedata){CFeatureListtrackedFeats;map<TFeatureID, TPoint3D>  preVisibleFeats,curVisibleFeats,historyVisibleFeats;CObservation3DRangeScanPtr obs;int frm_index = 0;CGenericFeatureTrackerAutoPtr  tracker = CGenericFeatureTrackerAutoPtr( new CFeatureTracker_KL );CImage preImg;tracker->extra_params["add_new_features"] = 1;while(imagedata.isRun){if(imagedata.is_obs){if (frm_index%FRAMERATE_5==0){obs = imagedata.obs;if (frm_index>0){tracker->trackFeatures(preImg,imagedata.image,trackedFeats);FiltInvalidFeats(trackedFeats);curVisibleFeats = GetCur3DFeats(trackedFeats,obs);if (!preVisibleFeats.empty()){AddValidFeaturesToHistory(historyVisibleFeats,preVisibleFeats,curVisibleFeats);}preVisibleFeats  = curVisibleFeats;}else{preImg = imagedata.image;COL = obs->cameraParamsIntensity.ncols;ROW = obs->cameraParamsIntensity.nrows;}}frm_index++;}}}