深度图转点云原理
转载于:https://www.cnblogs.com/cv-pr/p/5719350.html
深度图转点云的计算过程很简洁,而里面的原理是根据内外参矩阵变换公式得到,下面来介绍其推导的过程。
1. 原理
首先,要了解下世界坐标到图像的映射过程,考虑世界坐标点M(Xw,Yw,Zw)映射到图像点m(u,v)的过程,如下图所示:
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
_________________________________________________________________________________________________________________________________________________
每一个不曾起舞的日子,都是对生命的辜负。
But it is the same with man as with the tree. The more he seeks to rise into the height and light, the more vigorously do his roots struggle earthward, downward, into the dark, the deep - into evil.
其实人跟树是一样的,越是向往高处的阳光,它的根就越要伸向黑暗的地底。----尼采
每一个不曾起舞的日子,都是对生命的辜负。
But it is the same with man as with the tree. The more he seeks to rise into the height and light, the more vigorously do his roots struggle earthward, downward, into the dark, the deep - into evil.
其实人跟树是一样的,越是向往高处的阳光,它的根就越要伸向黑暗的地底。----尼采