深度图转点云的计算过程很简洁,而里面的原理实际是从内外参矩阵变换得到.下面来介绍其推导的过程.
1. 原理
首先,要了解下世界坐标到图像的映射过程,可以参考下教程"相机标定(2)---摄像机标定原理",这里不做赘述.用公式具体的表达如下:
\(z_{c}\begin{bmatrix}u\\ v\\ 1\end{bmatrix}=\begin{bmatrix}f/dx & 0 & u_{0}\\ 0 & f/dy & v_{0}\\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix}R & T\\ 0 & 1\\ \end{bmatrix}\begin{bmatrix}x_{w}\\ y_{w}\\ z_{w}\\ 1\end{bmatrix} \)
2. 代码
#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
时间: 2024-10-06 08:29:40