代码收藏家 Python代码转为C代码实现双目测距，生成ROS点云图并可视化-第一个项目总结
目录
1.双目成像原理
2.双目测距python代码
3.python代码转为c++代码
(1)双目相机参数
（2）立体校正
（3）立体匹配
4.opencv的点云图转为ros点云图
1.双目成像原理
摘自《视觉SLAM十四讲》
2.双目测距python代码
(46条消息) 双目测距理论及其python实现_python双目测距_javastart的博客-CSDN博客
具体过程为：
双目标定 –> 立体校正（含消除畸变） –> 立体匹配 –> 视差计算 –> 深度计算(3D坐标)计算
3.python代码转为c++代码
给到我的任务是将上面博客内的python代码转为c++代码，实现点云图输出，接下来从各个模块讲
解转换：
首先要对OpenCv有一定了解，知道OpenCv图像处理，知道OpenCv的库，大概怎么用。
(46条消息) Opencv快速入门(C++版)，新手向_opencv c++入门_骚火棍的博客-CSDN博客
OpenCV [c++]（图像处理基础示例程序汇总）_图像处理程序_NCUTer的博客-CSDN博客
可以看看这两篇博客。
双目标定这一步具体可以参考上面博客，然后获得双目相机参数：内外参和畸变系数。
(1)双目相机参数
python头文件里的存储着双目相机参数的类，我们换成c++可以直接放到主文件中也建个类，然后实例化。或者直接放入main函数中，这样后面立体校正就不用再读取参数了。这一步很简单，主要是c++的矩阵表示：
例如左参数：
Mat cam_matrix_left = (Mat_<double>(3, 3) //三行三列的矩阵
<< 532.9200/2 , 0. , 633.4000/2, 0. , 532.8150/2, 361.1735/2, 0., 0., 1.);
（2）立体校正
特别说明：Mat、remap,undistort等函数为opencv库内函数，可以直接使用，不需要cv::
因为我的参数直接放入main函数内，所以我这一步不再读取参数，整个立体校正过程我也直接放入main函数内。
//1.计算矫正变换:得到立体矫正的映射矩阵
cv::Mat maplx, maply;
cv::Mat maprx, mapry;
cv::Mat R1, R2, P1, P2,Q;//输出参数，分别为左右相机的校正旋转矩阵、投影矩阵和重投
影矩阵
cv::stereoRectify(left_K, left_distortion, right_K, right_distortion, imageSize, R, T, R1, R2, P1, P2, Q )
//stereoRectify:计算出用于立体矫正的参数
cv::initUndistortRectifyMap(left_K, left_distortion, R1, cv::Mat(), imageSize, CV_32FC1, maplx, maply);
cv::initUndistortRectifyMap(right_K, right_distortion, R2, cv::Mat(), imageSize, CV_32FC1, maprx, mapry);
//initUndistortRectifyMap函数用于计算映射矩阵
//2.用映射矩阵做remap，得到矫正后的图像
cv::Mat iml1,imr1;
cv::undistort(iml, iml1, config.cam_matrix_left, config.distortion_l );
cv::undistort(imr, imr1, config.cam_matrix_right, config.distortion_r );
//使用undistort函数后输出的图像为channel等于1的灰度图像，因为在后面的remap过程中，需要
使用的是channel为1的图像
cv::Mat rectified_iml,rectified_imr;
remap(iml1, rectified_iml, maplx, maply, INTER_AREA);
remap(imr1, rectified_imr, maprx, mapry, INTER_AREA);
（3）立体匹配
目的：为左图中的每一个像素点在右图中找到其对应点（世界中相同的物理点），这样就可以计算出视差：disparity = u_{l}-u_{r}（u_{l}和u_{r}分别是两个对应点在图像中的列坐标
1.预处理：彩色图转灰度图，调用preprocess函数，
preprocess(rectified_iml, rectified_imr);
2.SGBM匹配参数设置
//根据输入的图像判断其通道数，如果是二维的则通道数为1，否则为3
int img_channels;
if (rectified_iml.dims== 2)
//dims：矩阵维度。如 3 * 4 的矩阵为 2 维， 3 *4 * 5 的为3维
img_channels = 1;
//Mat::channels()函数，返回mat的通道数，如rgb图像为3，8位灰度图为1。但是用imread()函数读取灰度图和rgb图时，得到的mat的channels均为3。
else
img_channels = 3;
double NumDisparities = 4;
double BlockSize = 3;
double P1 = 4*img_channels*BlockSize*BlockSize;
//BlockSize表示邻域块大小,用来计算区域阈值,奇数,一般选择为3、5、7…等
double P2 = 32*img_channels*BlockSize*BlockSize;
//sgbm是StereoSGBM类的一个对象，用于实现立体匹配算法
sgbm->setMinDisparity(0); //设置最小视差为0同理可得其他参数设置。
3.计算视差图
Mat disparity;
cv :: Mat disparity_sgbm;
sgbm -> compute(rectified_iml,rectified_imr, disparity_sgbm);
//使用SGBM算法计算左右图像之间的视差图。视差图是一幅灰度图像，
//其中每个像素的值表示该像素在左图像中的位置与右图像中对应像素位置之间的水平偏移量
disparity_sgbm.convertTo(disparity, CV_8UC1, 1.0/16.0f);
//视差图转换为8位无符号整数类型，并将每个像素值除以16，以便在显示时更好地可视化
//最终输出为disparity
4.计算像素点的三维坐标
OpenCV中提供了reprojectImageTo3D()这个函数用于计算像素点的三维坐标，该函数会返回一个3
通道的矩阵，分别存储X、Y、Z坐标（左摄像机坐标系下）
Mat points3D;
cv::reprojectImageTo3D(disparity, points3D, Q);5.转换成点云图
cv::Mat colors;
cv::cvtColor(imgl, colors, cv::COLOR_GRAY2BGR);
//cv::Mat::create()函数创建一个与points3D相同大小的三通道浮点型矩阵output，
//然后将每个像素的三维坐标赋值给output矩阵中对应像素的三通道浮点型值
cv::Mat output;
output.create(points3D.size(), CV_32FC3);
for (int i = 0; i < points3D.rows; i++) {
for (int j = 0; j < points3D.cols; j++) {
cv::Vec3f point = points3D.at<cv::Vec3f>(i, j);
cv::Vec3b color = colors.at<cv::Vec3b>(i, j);
output.at<cv::Vec3f>(i, j) = point;
}
}然后我用imshow（）函数来显示点云图，但是没有结果，于是我用opencv的viz模块实现点云图输出：
(55条消息) OpenCV学习篇2：viz模块简单用法_robinhjwy的博客-CSDN博客
cv::viz::Viz3d viewer("Point Cloud"); //创建可视化窗口
viewer.showWidget("Coordinate Widget", cv::viz::WCoordinateSystem()); //构造一个坐标系，并显示到窗口中
cv::viz::WCloud cloud_widget(points3D, colors);
viewer.showWidget("Point Cloud", cloud_widget);
viewer.spin(); //开启永久循环暂留6.左图像、视差图、点云图显示
namedWindow("Left Image"); //namewindow函数调用，帮助你将分辨率过高的图片正常在窗口显示
setMouseCallback("Left Image", onMouse); //鼠标回调函数
imshow("Left Image", iml); //imshow( )函数功能就是把载入的图片显示出来
waitKey(0); //"Left Image"是显示窗口的名称
imshow("disparity image", disparity);
waitKey(0);7.编译
CMakeLists.txt中添加：按下面说明根据自己情况修改
# Define project name
#project(opencv_example_project) #设置工程名
# Find OpenCV, you may need to set OpenCV_DIR variable
# to the absolute path to the directory containing OpenCVConfig.cmake file
# via the command line or GUI
#find_package(OpenCV REQUIRED) #不用改（cmake用来查找opencv包用的）
# If the package has been found, several variables will
# be set, you can find the full list with descriptions
# in the OpenCVConfig.cmake file.
# Print some message showing some of them
#message(STATUS "OpenCV library status:")
#message(STATUS " config: ${OpenCV_DIR}")
#message(STATUS " version: ${OpenCV_VERSION}")
#message(STATUS " libraries: ${OpenCV_LIBS}")
#message(STATUS " include path: ${OpenCV_INCLUDE_DIRS}") #以上四行干啥用不知道？！
# Declare the executable target built from your sources
#add_executable(opencv_example example.cpp) #工程文件名和编译文件名
# Link your application with OpenCV libraries
#target_link_libraries(opencv_example PRIVATE ${OpenCV_LIBS}) #工程文件名cmake .
make
./工程文件名
可能遇到的问题：
1.cv::viz::Viz3d::spin()’未定义的引用
大概是因为链接器无法找到OpenCV库文件
opencv在ubuntu下是通过调用早已编译好的动态链接库.so文件来生成程序的，通过g++ 的编译控制选项也许可以制定编译器的查找路径
添加opencv模块方法皆如下：
Ubuntu安装opencv的扩展模块-viz模块_Abdullah al-Sa的博客-CSDN博客
解决：可以在自己的opencv包里面module里看是否有，如果没有就去GitHub上下载一下viz模块：mirrors / opencv / opencv_contrib
4.opencv的点云图转为ros点云图
1.使用ROS中的PCL库（Point Cloud Library）读取YAML文件并将其转换为PointCloud2消息类型
2.需要在CMakeLists.txt中添加依赖项和链接库等
cmake_minimum_required(VERSION 2.8.3)
project(your_project_name)
find_package(catkin REQUIRED COMPONENTS
roscpp
cv_bridge
image_transport
)
find_package(OpenCV REQUIRED)
catkin_package()
include_directories(
${catkin_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
)
add_executable(your_executable_name src/your_source_file.cpp)
target_link_libraries(your_executable_name
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
)可能遇到的问题：
1.在ros下使用opencv读取图像话题时，在OpenCV图像与ROS图像之间的转换中会使用到cv_bridge。由于自己安装了opencv与ros自带的opencv发生冲突而有问题。因此需要解决cv_bridge。
Ubuntu+ROS+opencv4.3.0 修改cv_bridge配置文件，解决ROS与opencv版本冲突问题_ubutnu查看cv_bridge版本_NENGSHUIYIZHOU的博客-CSDN博客
当中查看ubuntu的opencv安装路径：
sudo find / -iname "*opencv*"
