已知条件:
左右长短焦相机的内参、外参、畸变系数。
#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);
}
效果展示: