双目摄像头基于SGBM算法的3d重建+立体标定+相机内外参获取

这是在下用的摄像头，还算便宜，如果是无畸变的摄像头可以直接跳过立体标定。也可以直接用两个相同的USB单目摄像头，不过进行立体标定的过程可能有点麻烦。

下面在下介绍在下实现3d重建的整个步骤。

1.先获取摄像头内参，外参

2.进行双目摄像头立体标定

3.进行立体匹配获得视差图（SGBM）

4.根据Z=b*f/d得到深度图

5.根据深度图绘制点云图

下面，在下按照各个步骤给大家进行讲解：

1.先获取摄像头内参，外参

 https://blog.csdn.net/u012319493/article/details/77622053

获得以下矩阵

Mat M1 = Mat::zeros(3,3,CV_64F);//左相机内参矩阵
Mat M2 = Mat::zeros(3,3,CV_64F);//右相机内参矩阵
Mat D1 = Mat::zeros(1,14,CV_64F);//左相机畸变系数向量
Mat D2 = Mat::zeros(1,14,CV_64F);//右相机畸变系数向量
Mat R = Mat::zeros(3,3,CV_64F);//两相机相对旋转矩阵
Mat T = Mat::zeros(3,1,CV_64F);//相对平移矩阵
Mat R1 = Mat::zeros(3,3,CV_64F);//第一个摄像机的校正变换矩阵（旋转变换）
Mat R2 = Mat::zeros(3,3,CV_64F);//第二个摄像机的校正变换矩阵（旋转变换）
Mat P1 = Mat::zeros(3,4,CV_64F);//第一个摄像机在新坐标系下的投影矩阵
Mat P2 = Mat::zeros(3,4,CV_64F);//第二个摄像机在新坐标系下的投影矩阵
Mat Q = Mat::zeros(4,4,CV_64F);//深度差异映射矩阵

2.进行双目摄像头立体标定

https://blog.csdn.net/qq_36537774/article/details/85005552

https://blog.csdn.net/qq_22424571/article/details/80836342

https://blog.csdn.net/u014652390/article/details/71169927

获得映射rmap 
initUndistortRectifyMap(M1, D1, R1, P1, imageSize, CV_16SC2, rmap[0][0], rmap[0][1]);
initUndistortRectifyMap(M2, D2, R2, P2, imageSize, CV_16SC2, rmap[1][0], rmap[1][1]);

进行畸变校正

remap(Img_left, Img_left_out, rmap[0][0], rmap[0][1], CV_INTER_LINEAR);
remap(Img_right, Img_right_out, rmap[1][0], rmap[1][1], CV_INTER_LINEAR);

查看校正的结果

看绿线处的特征是否对齐

获得校正之后的图

3.进行立体匹配获得视差图（SGBM）

4.根据Z=b*f/d得到深度图

https://www.cnblogs.com/riddick/p/8486223.html   //这篇博客很有参考价值，在下大致按照里面的步骤做的

https://www.cnblogs.com/riddick/p/8318997.html

https://blog.csdn.net/wwp2016/article/details/86080722

获得视差图，深度图，进行空洞填充。（第一篇博客里面有代码）

在下在实践的过程中获得一个经验，最好用右图减左图来获取视差图，这样效果更好一些。（不知为何）

sgbm->compute(imgR, imgL, disp);

不过通过以上设定生成的视差图与sgbm->compute(imgL, imgR, disp);生成的视差图是互补的，也就是说视差越大像素值越小，

所以要进行一个求补的运算，如果你disp是u8类型，像素值=255-像素值，u16类型，像素值=65535-像素值，你可以用pic.type()查看图像像素点数据类型。

建一个Trackbar用来调节SGBM的各个参数，以让视差图获得最好的效果。什么是最好的效果，图像平滑，杂点少黑洞小。

用来生成视差图的图像纹理要多。

 

深度=基线×焦距/视差（至于SGBM生成的视差图单位是什么，在下心中有一个大大的问号）

在下生成的视差图和深度图效果

 上面的视差图是求补运算前的图，所以越远的地方越亮，生成的深度图黑压压一片，啥都看不到，在下专门进行了normalize()归一化让特征显示的明显一点

5.根据深度图绘制点云图

 下面是最精彩的表演，生成点云图

在下用的是pcl库

​

void draw_pcl()
{
  Eigen::Isometry3d pose = Eigen::Isometry3d::Identity();
  pose.pretranslate( Eigen::Vector3d( 0, 0, 0 ) );
    
  double cx = M1.at<double>(0,2);
  double cy = M1.at<double>(1,2);
  double fx = M1.at<double>(0,0);
  double fy = M1.at<double>(1,1);
  double depthScale = 1.0;//不确定
  
  cout<<"将图像装换为点云"<<endl;
  typedef pcl::PointXYZRGB PointT;
  typedef pcl::PointCloud<PointT> PointCloud;
  
  PointCloud::Ptr pointCloud( new PointCloud );

    cout<<"转换图像中："<<endl;
    cv::Mat color = img_right;
    cv::Mat depth = img_right_depth;
    Eigen::Isometry3d T = pose;
    for( int v=0; v<color.rows; v++ )
      for( int u=0; u<color.cols; u++ )
      {
	unsigned int d = depth.ptr<unsigned short> ( v )[u];//<unsigned short>
	if( d==0 ) continue;
        if(d>=500) continue; //把远处的噪点截掉
	Eigen::Vector3d point;
	point[2] = double(d)/depthScale;
	point[0] = (u-cx)*point[2]/fx;
	point[1] = (v-cy)*point[2]/fy;
	Eigen::Vector3d pointWorld = T*point;
	
	PointT p;
	p.x = pointWorld[0];
	p.y = pointWorld[1];
	p.z = pointWorld[2];
	p.b = color.data[ v*color.step+u*color.channels() ];
	p.g = color.data[ v*color.step+u*color.channels()+1 ];
	p.r = color.data[ v*color.step+u*color.channels()+2 ];
	pointCloud->points.push_back( p );
      }
  
  
  pointCloud->is_dense = false;
  cout<<"点云共有"<<pointCloud->size()<<"个点。"<<endl;
  pcl::io::savePCDFileBinary( "map.pcd", *pointCloud );  


  system("pcl_viewer map.pcd");
  
/*
  pcl::visualization::CloudViewer viewer("Cloud_Viewer");
  viewer.showCloud(pointCloud);
  while (!viewer.wasStopped()) 
   { ; }
*/
}

[Click and drag to move]
​

把rgb图的像素和深度图的像素转换为世界坐标系的一个点

 

天花板投影仪隐约可见

 

 

效果还不是很好，用更高分辨率的摄像头和更优秀的立体匹配算法应该可以获得更准确的深度图，再加上滤波算法。先告一段落，在下日后再尝试。大家有什么更好的双目实现办法可以告诉在下，多谢。