OpenCV双目立体视觉重建

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