zoukankan      html  css  js  c++  java
  • 深度图转点云原理

    转载于: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
    复制代码
  • 相关阅读:
    进击的实时数仓:Flink 在 OPPO 实时计算平台的研发与应用实践
    vue中将时间戳转换为YYYY-MM-dd hh:mm格式时间的组件
    vue 将时间戳转换成日期格式 (一)
    element-ui DatePicker 日期选择器 让结束日期大于开始日期
    css 边框上如何写入文字?
    2019最新Web前端经典面试试题(含答案)
    Nginx 安装及配置
    前端基础面试题(JS部分)
    const与指针
    linux配置-------redhat虚拟机的中文输入法配置
  • 原文地址:https://www.cnblogs.com/leoking01/p/13508320.html
Copyright © 2011-2022 走看看