< >
Home » 一起做RGB-D SLAM » 一起做RGB-D SLAM (2)-从图像到点云

一起做RGB-D SLAM (2)-从图像到点云

一起做RGB-D SLAM (2)-从图像到点云

说明：

  • 介绍编写一个将图像转换为点云的程序

从图像到点云

  • 最简单的点云地图即是把不同位置的点云进行拼接得到
  • 使用RGB-D相机时，会从相机里读到两种数据：彩色图像和深度图像。
  • 如果你有Kinect和ros，可以运行：
roslaunch openni_launch openni.launch

  • 彩色图像与深度图像就会发布在 /camera/rgb/image_color 和 /camera/depth_registered/image_raw 中。

  • 你可以通过如下命令显示彩色图像：

    rosrun image_view image_view image:=/camera/rgb/image_color

  • 也可以在Rviz里看到图像与点云的可视化数据

rosrun rviz rviz
  • 效果图：
    请输入图片描述

请输入图片描述

  • 这两张图来自于nyuv2数据集：http://cs.nyu.edu/~silberman/datasets/ 原图格式是ppm和pgm的，被我转成了png格式
  • 你可以直接另存为这两个图，也可以到我的git里面获取这两个图
  • 实际Kinect里（或其他rgb-d相机里）直接采到的RGB图和深度图可能会有些小问题
    • 有一些时差（约几到十几个毫秒）。这个时差的存在，会产生“RGB图已经向右转了，怎么深度图还没转”的感觉
    • 光圈中心未对齐。因为深度毕竟是靠另一个相机获取的，所以深度传感器和彩色传感器参数可能不一致
    • 深度图里有很多“洞”。因为RGB-D相机不是万能的，它有一个探测距离的限制啦！太远或太近的东西都是看不见的呢。
    • 关于这些“洞”，我们暂时睁一只眼闭一只眼，不去理它。以后我们也可以靠双边bayes滤波器去填这些洞。
    • 但是！这是RGB-D相机本身的局限性。软件算法顶多给它修修补补，并不能完全弥补它的缺陷
  • 在我们给出的这两个图中，都进行了预处理。你可以认为“深度图就是彩色图里每个像素距传感器的距离”
  • 把这两个图转成点云，因为计算每个像素的空间点位置，可是后面配准、拼图等一系列事情的基础.

从2D到3D（数学部分）

  • 上面两个图像给出了机器人外部世界的一个局部的信息.
  • 假设这个世界由一个点云来描述：X={x1,…,xn}.其中每一个点,有r,g,b,x,y,z一共6个分量，表示它们颜色与空间位置。 颜色方面，主要由彩色图像记录； 而空间位置，可以由图像和相机模型、姿态一起计算出来。
  • 对于常规相机，SLAM里使用针孔相机模型:
  • (图来自http://www.comp.nus.edu.sg/~cs4243/lecture/camera.pdf)
    请输入图片描述
  • 简而言之，一个空间点[x,y,z]和它在图像中的像素坐标[u,v,d] (d指深度数据) 的对应关系是这样的：
    请输入图片描述
  • 这个公式是从(x,y,z)推到(u,v,d)的。反之，我们也可以把它写成已知(u,v,d)，推导(x,y,z)的方式.公式如下：
    请输入图片描述
  • 根据这个公式就可以构建点云
  • 请输入图片描述
  • 如果相机发生了位移和旋转，那么只要对这些点进行位移和旋转操作即可

从2D到3D （编程部分）

  • 代码根目录/src/ 文件夹中新建一个generatePointCloud.cpp文件
touch src/generatePointCloud.cpp
  • 内容如下：
// C++ 标准库
#include <iostream>
#include <string>
using namespace std;

// OpenCV 库
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

// PCL 库
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

// 定义点云类型
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud; 

// 相机内参
const double camera_factor = 1000;
const double camera_cx = 325.5;
const double camera_cy = 253.5;
const double camera_fx = 518.0;
const double camera_fy = 519.0;

// 主函数 
int main( int argc, char** argv )
{
    // 读取./data/rgb.png和./data/depth.png，并转化为点云

    // 图像矩阵
    cv::Mat rgb, depth;
    // 使用cv::imread()来读取图像
    // API: http://docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html?highlight=imread#cv2.imread
    rgb = cv::imread( "./data/rgb.png" );
    // rgb 图像是8UC3的彩色图像
    // depth 是16UC1的单通道图像，注意flags设置-1,表示读取原始数据不做任何修改
    depth = cv::imread( "./data/depth.png", -1 );

    // 点云变量
    // 使用智能指针，创建一个空点云。这种指针用完会自动释放。
    PointCloud::Ptr cloud ( new PointCloud );
    // 遍历深度图
    for (int m = 0; m < depth.rows; m++)
        for (int n=0; n < depth.cols; n++)
        {
            // 获取深度图中(m,n)处的值
            ushort d = depth.ptr<ushort>(m)[n];
            // d 可能没有值，若如此，跳过此点
            if (d == 0)
                continue;
            // d 存在值，则向点云增加一个点
            PointT p;

            // 计算这个点的空间坐标
            p.z = double(d) / camera_factor;
            p.x = (n - camera_cx) * p.z / camera_fx;
            p.y = (m - camera_cy) * p.z / camera_fy;
            
            // 从rgb图像中获取它的颜色
            // rgb是三通道的BGR格式图，所以按下面的顺序获取颜色
            p.b = rgb.ptr<uchar>(m)[n*3];
            p.g = rgb.ptr<uchar>(m)[n*3+1];
            p.r = rgb.ptr<uchar>(m)[n*3+2];

            // 把p加入到点云中
            cloud->points.push_back( p );
        }
    // 设置并保存点云
    cloud->height = 1;
    cloud->width = cloud->points.size();
    cout<<"point cloud size = "<<cloud->points.size()<<endl;
    cloud->is_dense = false;
    pcl::io::savePCDFile( "./pointcloud.pcd", *cloud );
    // 清除数据并退出
    cloud->points.clear();
    cout<<"Point cloud saved."<<endl;
    return 0;
}
  • 程序运行需要数据。请把上面的那两个图存放在代码根目录/data下。
  • OpenCV的imread函数读取图片。在OpenCV2里，图像是以矩阵(cv::MAt)作为基本的数据结构。
  • Mat结构既可以帮你管理内存、像素信息，还支持一些常见的矩阵运算，是非常方便的结构。
  • 彩色图像含有R,G,B三个通道，每个通道占8个bit（也就是unsigned char），故称为8UC3（8位unsigend char, 3通道）结构。
  • 而深度图则是单通道的图像，每个像素由16个bit组成（也就是C++里的unsigned short），像素的值代表该点离传感器的距离。通常1000的值代表1米，所以我们把camera_factor设置成1000. 这样，深度图里每个像素点的读数除以1000，就是它离你的真实距离了
  • 我们按照“先列后行”的顺序，遍历了整张深度图。在这个双重循环中：
for (int m = 0; m < depth.rows; m++)
      for (int n=0; n < depth.cols; n++)

  • m指图像的行，n是图像的列。它和空间点的坐标系关系是这样的：
    请输入图片描述

  • 深度图第m行，第n行的数据可以使用depth.ptr(m) [n]来获取。其中，cv::Mat的ptr函数会返回指向该图像第m行数据的头指针。然后加上位移n后，这个指针指向的数据就是我们需要读取的数据啦

  • 计算三维点坐标的公式我们已经给出过了，代码里原封不动地实现了一遍。我们根据这个公式，新增了一个空间点，并放入了点云中。最后，把整个点云存储为 ./data/pointcloud.pcd 文件

编译并运行

  • 最后，我们在src/CMakeLists.txt里加入几行代码，告诉编译器我们希望编译这个程序。请在此文件中加入以下几行：
# 增加PCL库的依赖
FIND_PACKAGE( PCL REQUIRED COMPONENTS common io )

# 增加opencv的依赖
FIND_PACKAGE( OpenCV REQUIRED )

# 添加头文件和库文件
ADD_DEFINITIONS( ${PCL_DEFINITIONS} )
INCLUDE_DIRECTORIES( ${PCL_INCLUDE_DIRS}  )
LINK_LIBRARIES( ${PCL_LIBRARY_DIRS} )

ADD_EXECUTABLE( generate_pointcloud generatePointCloud.cpp )
TARGET_LINK_LIBRARIES( generate_pointcloud ${OpenCV_LIBS} 
    ${PCL_LIBRARIES} )
  • 编译新的工程：
cd build
cmake ..
make
cd ..
  • 如果编译通过，就可在bin目录下找到新写的二进制：generate_pointcloud 运行它：
bin/generate_pointcloud
  • 即可在data目录下生成点云文件。
  • 现在，你肯定希望查看一下新生成的点云了。如果已经安装了pcl，就可以通过：
pcl_viewer pointcloud.pcd
  • 提示：
如果你打开点云，只看到红绿蓝三个方块，请按R重置视角。刚才你是站在原点盯着坐标轴看呢。
如果点云没有颜色，请按5显示颜色。
cmake过程可能有PCL的警告，如果你编译成功了，无视它即可。这是程序员的本能。

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: 一起做rgb-d slam