长短焦异构双目立体匹配

已知条件：

左右长短焦相机的内参、外参、畸变系数。

#include <iostream>
#include <fstream>
#include <string>
#include <vector>

#include <opencv2/plot.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/viz.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv4/opencv2/ximgproc.hpp>

using namespace std;


cv::Mat rectifyImageL, rectifyImageR;
cv::Mat grayImageL, grayImageR;
cv::Mat R_L, R_R, P1, P2, Q;
cv::Mat mapLx, mapLy, mapRx, mapRy;
cv::Rect validROIL, validROIR;
int main()
{
      // 获取原始图像
      Mat ori_img_left = cv::imread("");
      Mat ori_img_right = cv::imread("");
      // 获取两个相机之间的内参 外参 和 畸变系数，自己根据实际情况读取
      // 注意：这里的相机外参是每个相机相对于车辆坐标系原点，如果是直接做双目标定，则可以
      // 直接得到两个相机之间的相对位姿，即L2R和T。
      cv::Mat intrinsic_left ;// 3x3
      cv::Mat intrinsic_right;// 3x3
      cv::Mat extrinsic_left ;// 3x3 
      cv::Mat extrinsic_right;// 3x3
      cv::Mat trans_left     ;// 3x1
      cv::Mat trans_right    ;// 3x1
      cv::Mat distortion_left  ; 
      cv::Mat distortion_right ; 
      // 计算左相机相对于右相机的旋转和平移矩阵，双目标定结果不需要此操作
      cv::Mat L2R =  extrinsic_right*extrinsic_left.inv();
      cv::Mat T = trans_right - R*trans_left;
      // 立体校正
      cv::Size img_size(ori_img_left.cols,ori_img_left.rows);
      cv::stereoRectify(intrinsic_left, distortion_left, intrinsic_right, 
          distortion_right,img_size, L2R, T, R_L, R_R, P1, 
          P2, Q, CALIB_ZERO_DISPARITY, 1,img_size, &validROIL, &validROIR);
      cv::initUndistortRectifyMap(intrinsic_left, distortion_left, R_L, P1, 
          img_size, CV_16SC2, mapLx, mapLy);
      cv::initUndistortRectifyMap(intrinsic_right, distortion_right, R_R, P2, 
          img_sizee, CV_16SC2, mapRx, mapRy);
      cv::remap(ori_img_left, rectifyImageL, mapLx, mapLy, INTER_LINEAR);
      cv::remap(ori_img_right, rectifyImageR, mapRx, mapRy, INTER_LINEAR);
      // 显示立体校正结果
      cv::Mat show;
      cv::Mat image_l,image_r;
      //cv::resize(rectifyImageL,image_l,rectifyImageL.size()/2);
      //cv::resize(rectifyImageR,image_r,rectifyImageR.size()/2);
      cv::hconcat(image_l,image_r,show);
      for (int i = 0; i < show[0].rows; i += 20)
            line(show[0], cv::Point(0, i), cv::Point(show[0].cols, i), 
                 cv::Scalar((i/2*5)%255, 125, (i/2*2)%255), 1, 8);
      cv::imshow("极线校正图",show); 
      cv::waitKey(1);
      // 立体匹配
      cv::Mat disp16S,disp8,disp32F,xyz,disp_color;
      Ptr<StereoBM> bm = StereoBM::create();
      bm->compute(grayImageL, grayImageR, disp16S);//输入图像必须为灰度图
      disp16S.convertTo(disp32F, CV_32F, 1.0 / 16);
      disp16S.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16)*16.));
      cv::reprojectImageTo3D(disp32F, xyz,Q, true); 
      // 显示视差图
      cv::Mat draw;
      cv::applyColorMap(disp8, disp_color, cv::COLORMAP_JET);
      cv::resize(disp_color,draw,disp_color.size()/2);
      cv::imshow("disparity", draw);
      cv::waitKey(1);


}

#include <pcl/point_types.h>
#include <pcl/surface/gp3.h>
#include <pcl/visualization/point_cloud_geometry_handlers.h>
#include <pcl/visualization/impl/point_cloud_geometry_handlers.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
using namespace pcl;
// PCL 显示点云
    PointCloud<PointXYZRGB>::Ptr cloud(new PointCloud<PointXYZRGB>);
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "3D Viewer");
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();
    viewer->setCameraPosition(-4.5,0,2,0.1,0.035,0.99);
        cloud->points.clear();
        for (int row = 0; row < xyz.rows; row++)
        {
            for (int col = 0; col < xyz.cols; col++)
            {
                float z = xyz.ptr<float>(row)[col * 3 + 2];
                if(z < 20 && z > 0)
                {
                    PointXYZRGB p;
                    // depth
                    p.z = z ; // Zc = baseline * f / d 垂直像面向前
                    p.x = xyz.ptr<float>(row)[col * 3] ; // Xc向右
                    p.y = xyz.ptr<float>(row)[col * 3 + 1] ;// Yc向下
                    cv::Mat cam = (cv::Mat_<float>(3,1) << p.x,p.y,p.z);
                    cv::Mat wrd = cam_adapter[Front120].getExtrinsic().inv()*(cam - cam_adapter[Front120].getTranslation());

                    // 车辆坐标系 unit:m
                    p.x = wrd.at<float>(0,0);
                    p.y = wrd.at<float>(1,0);
                    p.z = wrd.at<float>(2,0);

                    // RGB
                    p.b = rectifyImageL.ptr<uchar>(row)[col * 3];
                    p.g = rectifyImageL.ptr<uchar>(row)[col * 3 + 1];
                    p.r = rectifyImageL.ptr<uchar>(row)[col * 3 + 2];

                    cloud->points.push_back(p);
                }
            }
        }
        cloud->height = xyz.rows;
        cloud->width = xyz.cols;
        cloud->points.resize(cloud->height * cloud->width);
        viewer->removeAllPointClouds();
        viewer->addPointCloud<pcl::PointXYZRGB>(cloud, "3D Viewer");
        viewer->updatePointCloud(cloud,"3D Viewer");             
        while (!viewer->wasStopped())
       {
        viewer->spinOnce(0.001,false);
       }

效果展示：