OpenCV立体匹配结果求三维坐标代码
在OpenCV源码的258-261行,代码是将输出输出转化为8位的图像输出,原来输出的disp是16位float类型的,是无法作为RGB图像输出的,现在转成8位单通道图像才能imwrite输出。 源码262-278行是输出对应视差图像。
最后主要是通过矩阵Q转化为三维坐标,所用的主要是reprojectImageTo3D,看下图中的第四个参数释义,如果是没有匹配上的点,也就是视差图中黑色的点它的Z值会被置成10000,当我们看点云的三维坐标时,就可以由此看到漏匹配点的个数。
之后我们可以得到xyz这个Mat矩阵,他是一个三通道的Mat,其中每一个像素点(姑且将其理解为类似RGB形式的Mat图像,只不过每个通道是16位)的值有三个通道,分别代表了该像素点的空间上X,Y,Z的坐标值,坐标原点是左摄像头的光心。xyz之后是输出为Point3D文件,在另外的地方要用的话,就读入它为Mat再用.at读数据,但这样用源码往往结果不对,下文会讲到如何修改。
但是在这里建议是自己再输出一个yml文件,这样子就能看到三维坐标和点云了。
我的代码如下:
if(!point_cloud_filename.empty())//点入点云数据
{
printf("storing the point cloud...");
fflush(stdout);//强制输出
Mat tmp;
// disp=depth_fill(disp);
disp.convertTo(tmp, CV_32FC1, 1 / 16.0);
reprojectImageTo3D(tmp, xyz, Q, true);
// cout<<endl<<xyz.type()<<endl;
FileStorage f("point3d.yml", FileStorage::WRITE);
// f.open("point3d.yml", FileStorage::WRITE);
if( f.isOpened() )
{
f<<"xyz"<<xyz;
f.release();
}
imshow("img1",img1);
// imwrite("obj.jpg",img1);
cout<<img1.cols<<" "<<img1.rows<<endl;
cout<<disp8.cols<<" "<<disp8.rows<<endl;
// cout<<"type: "<<disp8.type()<<endl;
saveXYZ(point_cloud_filename.c_str(), xyz);
printf("\n");
这里用filestorage输出yml文件,而最重要的是这一句:
disp.convertTo(tmp, CV_32FC1, 1 / 16.0);
将disp转成32位,同时除16,关键在于除16,具体原因我也不是特别清楚了,好像是数据往前移了4位,要移回来吧。但是就是要除上16才能得到正确值,如果你只是看看点云长啥样那可以不除,但你要是想算距离就必须要除上。
但是输出为yml文件有个坏处,就是你没办法用源码的筛选函数,你要自己重写写一个,或者可以和我一样,改写saveXYZ为输出yml格式文件,再或者就是,输出yml格式文件看效果,后续计算还是用saveXYZ出来的文件。
查看点云如图:
有了基本平面的样子,但矫正矩阵效果不好,还要重新标定,如果说得到的点云是一个很锥形的云,如果之前你已经除了16,那就是你标定矩阵不好,你要进行重新进行摄像机标定。计算面积和距离的函数可以自己写,我这边就讲一种计算空间直线距离的最简单方法。主要用到的函数是triangulatePoints,这里感谢一波学长。
官网源码释义,前面两个参数就是之前相机标定里extrinsics文件中的P1和P2,三四两个参数就是输入的左右图像上的一对对应特征点(此时的左右图要是极线对准的,参数是通过图像识别算得的像素坐标值),但是要注意,必须是以规定格式输入,我用的是vector,而且要求是float类型的,最后一个参数也要注意,输出的是对应点的齐次坐标,是一个四维向量,要先转换到标准坐标系下,这里可以用convertPointsFromHomogeneous函数,但是要注意的是两者格式不同,triangulatePoints函数得到的坐标中每个点的坐标是以列向量的形式表示的,而convertPointsFromHomogeneous是以行向量表示的,所以,在转换到标准坐标系之前要先转置一下结果矩阵,这里用transpose
我的计算代码是:
float cal_max_distance(vector<Point> obj_point1,vector<Point> obj_point2,Mat P1,Mat P2)
{
int num=4;
vector<Point2f> input1;
input1.resize(num);
vector<Point2f> input2;
input2.resize(num);
vector<Point> top_point1;
top_point1.resize(num);
vector<Point> top_point2;
top_point2.resize(num);
top_point1=get_max_distance_points(obj_point1);
top_point2=get_max_distance_points(obj_point2);
input1=points_int_to_float(top_point1);
input2=points_int_to_float(top_point2);
Mat Point3D;
cv::triangulatePoints(P1,P2,input1,input2,Point3D);
// for(int i=0;i<Point3D.rows;i++)
// {
// for(int j=0;j<Point3D.cols;j++)
// {
// float a=Point3D.at<float>(i,j);
// cout<<" "<<a<<" ";
// }
// cout<<endl;
// }
// cout<<Point3D.rows<<" "<<Point3D.cols<<endl;
Mat b;
Mat Point3D_transposition=Mat(4,4,CV_32F);
cv::transpose(Point3D,Point3D_transposition);
convertPointsFromHomogeneous(Point3D_transposition,b);
// cout<<b.rows<<" "<<b.cols<<endl<<b.type()<<endl;
Vec3f p1=b.at<Vec3f>(0,0);
Vec3f p2=b.at<Vec3f>(1,0);
double distances=sqrt(pow((p1.val[0]-p2.val[0]),2)+pow((p1.val[1]-p2.val[1]),2)+pow((p1.val[2]-p2.val[2]),2));
cout<<"distance= "<<distances<<endl;
return distances;
}
里面部分函数我是另外写的,作用和函数名一样。
效果图(点的对应在图右上角)
可以看到误差不大,基本准确。