代码如下:
vector<KeyPoint> keypoints_1, keypoints_2;
vector<DMatch> matches;
find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;
for ( DMatch m:matches )
{
//提取特征点的深度值:
//很奇怪,下面代码得出的y,x是小数点的,要用int强制类型转换
//cout << "y=" << keypoints_1[m.queryIdx].pt.y << "x=" << keypoints_1[m.queryIdx].pt.x <<endl ;
ushort d = d1.ptr<unsigned short> ( int( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
if ( d == 0 ) // bad depth
continue;
float dd = d/5000.0; //dd就是相机坐标系下的z坐标,这就是所谓深度的概念?
Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K ); //像素坐标转相机归一化坐标
pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );//dd就是相机坐标系下的z坐标,