OpenCV双目立体视觉重建

本篇文章主要给出使用opencv sgbm重建三维点云的代码，鉴于自身水平所限，如有错误，欢迎批评指正。

环境：vs2015 ，opencv3.4.6，pcl1.8.0

原始数据使用D455采集，图像已做完立体校正，如下图所示（欢迎进Q群交流：874653199）：

左图：

右图：

视差结果图：

彩色视差结果图：

点云结果：

#include <iostream>
#include <fstream>

#include <opencv2/opencv.hpp> 
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include<pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>

#define isStereoRectify 


void visualize(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
  pcl::visualization::PCLVisualizer viewer("3D Viewer");

  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(cloud, 255, 255, 255);

  viewer.setBackgroundColor(0, 0, 0);
  viewer.addPointCloud(cloud, src_h, "cloud");

  while (!viewer.wasStopped())
  {
    viewer.spinOnce(100);
    boost::this_thread::sleep(boost::posix_time::microseconds(100000));
  }

}


void recon3d(cv::Mat disparty, double f, double cx, double cy, double baseline) {

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());

  pcl::PointXYZ singlePoint;

  for (int i = 0; i < disparty.rows; i++) {
    for (int j = 0; j < disparty.cols; j++) {
      const double disp = disparty.at<float>(i, j);
      if (disp == 0) {
        continue;
      }
      else {
        singlePoint.z = f*baseline / disp;

        singlePoint.x = (i - cx) / f *singlePoint.z;

        singlePoint.y = (j - cy) / f *singlePoint.z;

        if (singlePoint.z >= -0.65 && singlePoint.z <= 0.3) {
          cloud->points.emplace_back(singlePoint);
        }
      }
    }
  }

  visualize(cloud);
  pcl::io::savePLYFileBinary("cloud.ply", *cloud);

}




int main(){

  cv::Mat imageL = cv::imread("E:/2_光学测量/6_数据/6_stereo/l0.jpg",0);
  cv::Mat imageR = cv::imread("E:/2_光学测量/6_数据/6_stereo/r0.jpg", 0);



  cv::Mat cameraMatrixL = (cv::Mat_<double>(3, 3) << 428.406, 0.000000, 420.335, 0.000000, 428.406, 238.037, 0.000000, 0.000000, 1.000000);
  cv::Mat distCoeffL = (cv::Mat_<double>(5, 1) << 0, 0, 0, 0, 0);


  cv::Mat cameraMatrixR = (cv::Mat_<double>(3, 3) << 428.406, 0.000000, 420.335, 0.000000, 428.406, 238.037, 0.000000, 0.000000, 1.000000);
  cv::Mat distCoeffR = (cv::Mat_<double>(5, 1) << 0, 0, 0, 0, 0);


  cv::Mat R = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);

  cv::Mat T = (cv::Mat_<double>(3, 1) << -0.0949472, 0, 0);
  

#ifdef isStereoRectify

  cv::Mat Rl, Rr, Pl, Pr, Q;
  cv::Rect validROIL, validROIR;

  cv::Size imageSize = imageL.size();
  cv::stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, cv::CALIB_ZERO_DISPARITY,
    0, imageSize, &validROIL, &validROIR);

  cv::Mat mapLx, mapLy, mapRx, mapRy;

  cv::initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, CV_32FC1, mapLx, mapLy);
  cv::initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);

  cv::Mat rectifyImageL, rectifyImageR;
  cv::remap(imageL, rectifyImageL, mapLx, mapLy, cv::INTER_LINEAR);
  cv::remap(imageR, rectifyImageR, mapRx, mapRy, cv::INTER_LINEAR);
  imageL = rectifyImageL;
  imageR = rectifyImageR;

#endif // stero

  cv::namedWindow("disparity", CV_WINDOW_NORMAL);

  int SADWindowSize =5, numberOfDisparities = 128;
  cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(0, numberOfDisparities, SADWindowSize);
  sgbm->setPreFilterCap(64);
  sgbm->setBlockSize(SADWindowSize);
  sgbm->setP1(8 * SADWindowSize* SADWindowSize);
  sgbm->setP2(64 * SADWindowSize* SADWindowSize);
  sgbm->setMinDisparity(0);
  sgbm->setNumDisparities(numberOfDisparities);
  sgbm->setUniquenessRatio(10);
  sgbm->setSpeckleWindowSize(200);
  sgbm->setSpeckleRange(64);
  sgbm->setDisp12MaxDiff(1);
  sgbm->setMode(cv::StereoSGBM::MODE_SGBM);

  cv::Mat disp, disp8, dispf;
  sgbm->compute(imageL, imageR, disp);

  disp.convertTo(disp, CV_32F, 1.0 / 16.0);//1.0/16.0 
  disp.convertTo(disp8, CV_8U, 1.0);
  imshow("disparity", disp8);
  cv::imwrite("disp_mono.png", disp8);
  cv::Mat disp8_color;
  cv::applyColorMap(disp8, disp8_color, cv::COLORMAP_JET);
  imshow("disparity_color", disp8_color);
  cv::imwrite("disp_color.png", disp8_color);

  recon3d(disp, cameraMatrixL.at<double>(0,0), cameraMatrixL.at<double>(0, 2), cameraMatrixL.at<double>(1, 2), T.at<double>(0));

  cv::waitKey(0);

  return 0;

}