深度图转点云的原理

深度图转点云的计算过程很简洁,而里面的原理实际是从内外参矩阵变换得到.下面来介绍其推导的过程.

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

深度图转点云的原理的相关文章

ICP点云配准原理及优化

ICP算法简介 根据点云数据所包含的空间信息,可以直接利用点云数据进行配准.主流算法为最近迭代算法(ICP,Iterative Closest Point),该算法是根据点云数据首先构造局部几何特征,然后再根据局部几何特征进行点云数据重定位. 一. ICP原理 假设两个点云数据集合P和G,要通过P转换到G(假设两组点云存在局部几何特征相似的部分),可以通过P叉乘四元矩阵进行旋转平移变换到G,或者SVD法将P转换到G位置,总体思想都是需要一个4x4的旋转平移矩阵.对于每次旋转平移变换后计算P的所有

iOS融云使用原理篇

App Key / Secret App Key / Secret 相当于您的 App 在融云的账号和密码.是融云 SDK 连接服务器所必需的标识,每一个 App 对应一套 App Key / Secret. 融云提供了两套环境,开发环境和生产环境,前者是方便您集成开发和测试的,后者是 App 上线之后真正运营的商业环境.两者间数据隔离,避免开发环境数据和线上生产环境数据互相冲突.针对开发者的生产环境和开发环境,我们提供两套 App Key / Secret ,在正式上线前,请务必切换到生产环境

深度图向点云图的转换

深度图转点云的计算方法不复杂,是根据内外参矩阵变换公式得到的. 一个相机的三维点[x,y,z]和它在图像中的像素坐标[u,v,d],存在着一个固定的对应关系(对于小孔成像的相机原理的相机) 上述的公式是从世界坐标得到图像中像素坐标,同理也可从从图像的像素坐标转换到世界坐标下. 根据这个公式可以对点云进行构建. 如果相机发生了位移和旋转,那么只需要对这些点进行相应的处理即可.(一般相机在0时刻是与世界坐标重合的) 对于深度图中的点,转换成三维坐标在程序中一般是如下这样的: for (int m =

使用Python定制词云

一.实验介绍 1.1 实验内容 在互联网时代,人们获取信息的途径多种多样,大量的信息涌入到人们的视线中.如何从浩如烟海的信息中提炼出关键信息,滤除垃圾信息,一直是现代人关注的问题.在这个信息爆炸的时代,我们每时每刻都要更新自己的知识储备,而网络是最好的学习平台.对信息过滤和处理能力强,学习效率就会得到提高."词云"就是为此而诞生的."词云"是对网络文本中出现频率较高的"关键词"予以视觉上的突出,形成"关键词云层"或"

Android 即时音视频解决方案2——腾讯云

上一篇文章介绍了环信的解决方案,见Android 即时音视频解决方案1--环信,这篇文章,介绍一下更加靠谱,也就是腾讯云的解决方案,毕竟腾讯是是这方面的头头,比较靠谱.当然,集成腾讯云比集成环信稍微复杂那么一点,需要有一点点的耐心. 官方地址音视频云通信 AVC SDK下载AV Andriod1.3 文档地址音视频云通讯 先讲讲腾讯云的原理,使用腾讯云的时候,要有一个账号体系,这个账号体系比较灵活,可以使用独立模式也可以只用第三方账号体系,这里使用独立模式. 使用独立模式,要使用腾讯云的服务的时

文字云

Python +wordcloud 生成词云 什么是词云 词云又叫文字云,是对文本数据中出现频率较高的"关键词"在视觉上的突出呈现,形成关键词的渲染形成类似云一样的彩色图片,从而一眼就可以领略文本数据的主要表达意思. 准备工作: python开发环境.wordcloud.jieba.matplotlib.numpy .PIL 等库文件安装好. wordcloud生成词云的原理简介 wordcloud生成词云的原理其实并不复杂,大体分成5步(具体可自行查看源码): wordcloud制作

词云制作没那么难,Python 10 行代码就实现了!

写在前面 想必大家有一个问题.什么是词云呢? 词云又叫名字云,是对文本数据中出现频率较高的"关键词"在视觉上的突出呈现,形成关键词的渲染形成类似云一样的彩色图片,从而一眼就可以领略文本数据的主要表达意思.. 网页上有许多词云的效果图: 而且,目前有许多制作词云的工具: 但是作为一个学习Python的程序员来说,我更喜欢通过自己的编程去解决问题. 而且用Python制作词云只需十行代码就行了哦~ 一起来看看吧! 代码块 import matplotlib.pyplot as plt fr

关于slam

什么是SLAM?机器人在未知环境中,要实现智能化需要完成三个任务,第一个是定位(Localization),第二个是建图(Mapping),第三个则是随后的路径规划(Navigation). 之前地平线的高翔博士用这样一句话概括SLAM的释义.不过实际生活中的SLAM都是和激光雷达或者单目/双目摄像头结合的形式出现在我们面前的,有时甚至跟更多的传感器融合,这让很多人觉得这个技术不是一两句话能概括清楚的,但当我们熟悉完它在每一个领域的应用啊,就会明白,它,SLAM,原来是这样一个东西,而且会比我们

3D建模的初步了解

相关学习资料如下: cousera课程: https://www.coursera.org/learn/robotics-perception youtube课程: https://www.youtube.com/watch?v=RDkwklFGMfo Tutorial:  https://www.cse.wustl.edu/~furukawa/papers/fnt_mvs.pdf @知乎问题:三维重建 3D reconstruction有哪些使用算法 以下为知乎中的相关回答 用一组图片来做3D