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

最新推荐文章于 2020-09-23 23:36:49 发布

ry19890418

最新推荐文章于 2020-09-23 23:36:49 发布

阅读量810

点赞数 1

分类专栏：毕设SLAM

本文链接：https://blog.csdn.net/ry19890418/article/details/18043941

版权

毕设SLAM 专栏收录该内容

1 篇文章 0 订阅

订阅专栏

//过滤超出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;
}

//将与上一次特征点相匹配的特征点到历史特征点，由于隔帧处理，相匹配的特征点可以认为是静止的，也可以看作是landmark
void 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)
{
	CFeatureList	trackedFeats;

	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++;
		}
	}
}