深度图转点云原理
深度图转点云的计算过程很简洁,而里面的原理是根据内外参矩阵变换公式得到,下面来介绍其推导的过程。
1. 原理
首先,要了解下世界坐标到图像的映射过程,考虑世界坐标点M(Xw,Yw,Zw)映射到图像点m(u,v)的过程,如下图所示:
详细原理请参考教程"相机标定(2)---摄像机标定原理",这里不做赘述。形式化表示如下:
其中为图像坐标系下的任意坐标点。分别为图像的中心坐标。表示世界坐标系下的三维坐标点。表示相机坐标的z轴值,即目标到相机的距离。分别为外参矩阵的3x3旋转矩阵和3x1平移矩阵。
对外参矩阵的设置:由于世界坐标原点和相机原点是重合的,即没有旋转和平移,所以:
,
注意到,相机坐标系和世界坐标系的坐标原点重合,因此相机坐标和世界坐标下的同一个物体具有相同的深度,即.于是公式可进一步简化为:
从以上的变换矩阵公式,可以计算得到图像点 到世界坐标点的变换公式:
2. 代码
根据上述公式,再结合以下ROS给出的代码,就能理解其原理了。代码如下:
#ifndef DEPTH_IMAGE_PROC_DEPTH_CONVERSIONS #define DEPTH_IMAGE_PROC_DEPTH_CONVERSIONS #include <sensor_msgs/Image.h> #include <sensor_msgs/point_cloud2_iterator.h> #include <image_geometry/pinhole_camera_model.h> #include "depth_traits.h" #include <limits> namespace depth_proc { typedef sensor_msgs::PointCloud2 PointCloud; // Handles float or uint16 depths template<typename T> void convert( const sensor_msgs::ImageConstPtr& depth_msg, PointCloud::Ptr& cloud_msg, const image_geometry::PinholeCameraModel& model, double range_max = 0.0) { // Use correct principal point from calibration float center_x = model.cx();//内参矩阵中的图像中心的横坐标u0 float center_y = model.cy();//内参矩阵中的图像中心的纵坐标v0 // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y) double unit_scaling = DepthTraits<T>::toMeters( T(1) );//如果深度数据是毫米单位的,结果将会为0.001;如果深度数据是米单位的,结果将会为1; float constant_x = unit_scaling / model.fx();//内参矩阵中的 f/dx float constant_y = unit_scaling / model.fy();//内参矩阵中的 f/dy float bad_point = std::numeric_limits<float>::quiet_NaN(); sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x"); sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y"); sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z"); const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]); int row_step = depth_msg->step / sizeof(T); for (int v = 0; v < (int)cloud_msg->height; ++v, depth_row += row_step) { for (int u = 0; u < (int)cloud_msg->width; ++u, ++iter_x, ++iter_y, ++iter_z) { T depth = depth_row[u]; // Missing points denoted by NaNs if (!DepthTraits<T>::valid(depth)) { if (range_max != 0.0) { depth = DepthTraits<T>::fromMeters(range_max); } else { *iter_x = *iter_y = *iter_z = bad_point; continue; } } // Fill in XYZ *iter_x = (u - center_x) * depth * constant_x;//这句话计算的原理是什么,通过内外参数矩阵可以计算 *iter_y = (v - center_y) * depth * constant_y;//这句话计算的原理是什么,通过内外参数矩阵可以计算 *iter_z = DepthTraits<T>::toMeters(depth); } } } } // namespace depth_image_proc #endif
Make Change - Focus on Computer Vision and Pattern Recognition
版权声明:本文为博主原创文章,未经博主允许不得转载
版权声明:本文为博主原创文章,未经博主允许不得转载
【推荐】国内首个AI IDE,深度理解中文开发场景,立即下载体验Trae
【推荐】编程新体验,更懂你的AI,立即体验豆包MarsCode编程助手
【推荐】抖音旗下AI助手豆包,你的智能百科全书,全免费不限次数
【推荐】轻量又高性能的 SSH 工具 IShell:AI 加持,快人一步
· 10年+ .NET Coder 心语,封装的思维:从隐藏、稳定开始理解其本质意义
· .NET Core 中如何实现缓存的预热?
· 从 HTTP 原因短语缺失研究 HTTP/2 和 HTTP/3 的设计差异
· AI与.NET技术实操系列:向量存储与相似性搜索在 .NET 中的实现
· 基于Microsoft.Extensions.AI核心库实现RAG应用
· 10年+ .NET Coder 心语 ── 封装的思维:从隐藏、稳定开始理解其本质意义
· 地球OL攻略 —— 某应届生求职总结
· 提示词工程——AI应用必不可少的技术
· Open-Sora 2.0 重磅开源!
· 周边上新:园子的第一款马克杯温暖上架