代码编织梦想

目录

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.转换成点云图

6.左图像、视差图、点云图显示

    namedWindow("Left Image");     //namewindow函数调用,帮助你将分辨率过高的图片正常在窗口显示

    setMouseCallback("Left Image", onMouse);    //鼠标回调函数 

    imshow("Left Image", iml);         //imshow( )函数功能就是把载入的图片显示出来

    waitKey(0);                               //"Left Image"是显示窗口的名称

    imshow("disparity image", disparity);

    waitKey(0);

4.opencv的点云图转为ros点云图

版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
本文链接:https://blog.csdn.net/m0_72791092/article/details/131025513

ros开发增加clion常用模板及初始化配置(二)-爱代码爱编程

               ros开发增加clion常用模板及初始化配置(二) 在需要共享的文件夹内用控制台运行这个命令开启服务器,共享自己的文件,局域网电脑游览器登入这台电脑的ip加8000端口号即可访问 python -m http.server 使用gpu跑数据  下载英伟达的cuda支持以及cdnn库 开启443端口号 开启443端口

开发环境初始化配置-爱代码爱编程

代码中启动ros节点roslaunch和rosrun import subprocess import rospy import rosnode class launch_demo: def __init__(self, cmd=None): self.cmd = cmd def launch(self):

项目经历-爱代码爱编程

属性说明: 项目规模 ·大型项目 ·中型项目 ·小型项目 ·算法集合 团队性 ·独立 ·小团体合作(2-3人) ·大规模合作 代码语言 ·C ·C++ ·m语言 ·java ·python ·其他语言 熟悉度 ·熟悉(系统性学习相关原理,实现功能后可以改进算法) ·初学(简单了解原理,针对性浏览教程后可以自己独立实现功能) ·陌生(复制粘贴修改后可

激光雷达目标检测 (上)-爱代码爱编程

激光雷达目标检测 (上) **---- 转载自美团无人专送团队** 1简介 安全性是自动驾驶中人们最关注的问题之一。 在算法层面,无人车对周围环境的准确感知是保证安全的基础,因此感知算法的精度十分重要。现有感知算法的思路一般通过某种数学模型对现实世界的某个子集进行拟合。 当情况足够简单的时

一文详解点云库PCL-爱代码爱编程

3D is here: Point Cloud Library (PCL) 摘要: 随着新型,低成本的3D传感器硬件的出现(例如Kinect),以及科研人员在高级点云处理研究上的不断努力,3D感知在机器人技术以及其他领域显得愈发重要。 本文,我们将介绍在点云感知领域的一项最新举措:PCL(点云库– http://pointclouds.org)

面向自动驾驶的高精度地图框架解析和实战-爱代码爱编程

来源:每日自动驾驶(hdmap) 目前行业并没有通用的标准(OpenDrive并不是为自动驾驶设计的),Autoware自动驾驶系统的矢量地图和地图引擎计划使用Lanelet2,本文我们就来了解并且上手一下Lanelet2高精度地图框架。 前面聊了一些如何制作较大规模的点云地图以及如何使用点云地图进行定位的方法,点云图是高精度地图的一部分

点云 3D 可视化 - Open3D 库-爱代码爱编程

点云 3D 可视化 - Open3D 库 1. 文章信息2. Open3D 库简介2. 3D 可视化使用2.1 单帧点云2.2 多帧点云 1. 文章信息 (1)标题:Open3D: A Modern Library for 3D Data Processing (2018) (2)文章链接:https://arxiv.org/pdf/180

「需求广场」需求词更新明细(三)_csdn文库小助手的博客-爱代码爱编程

2022.5.7上线需求词: No.需求词No.需求词No.需求词1机器人聊天系统文献综述231爬虫如何获取页面所有url461ip-guard权限设置23d max 插件 对接渲染任务232基于情感 微博谣言的识别 python462python编程题库和答案3基于嵌入式qt界面gps定位系统233b站榜单视频 视频封面 python463spark+

apollo星火计划学习笔记第一讲——学习自动驾驶_我宿孤栈的博客-爱代码爱编程

Apollo学习笔记 零、目录一、 Apollo开源平台介绍二、如何使用Apollo学习2.1 上机2.1.1 Cyber RT(通信)2.1.2 感知2.1.3 决策规划 2.2 上车2.2.1 **车辆适配

ros深度图转化为点云_订阅深度话题转为点云,并发布-爱代码爱编程

自己编写C++的ROS代码,订阅D435i深度图像,转化为点云数据,并发布出去。 说明:D435i本身就可以输出点云,不需要自己编写代码。本博客的目的是通过自己编写深度图转点云代码来熟悉ROS工程的创建。 Step1:创建ROS工程 mkdir -p ~/catkin_ws/src cd ~/catkin_ws/ catkin_make cd ./s

【lvi-爱代码爱编程

文章目录 目录 文章目录 前言 一、思路 二、LIO部分代码阅读 1.imagaProjection() 2.featureTracker() 3、imuPreintegration() 4、mapOptmization() 总结 前言 最近打算在LVI-SAM的框架上