彩色图像和深度图像合成点云

绝地灬酷狼 2021-12-21 08:55 590阅读 0赞

彩色图像

![更改浏览器试一下][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70]

深度图像

![更改浏览器试一下][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70 1]

点云图

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70 2][]

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

*  上面两个图像给出了机器人外部世界的一个局部的信息.
 *  假设这个世界由一个点云来描述：X=\{x1,…,xn\}.其中每一个点,有r,g,b,x,y,z一共6个分量，表示它们颜色与空间位置。 颜色方面，主要由彩色图像记录； 而空间位置，可以由图像和相机模型、姿态一起计算出来。
 *  对于常规相机，SLAM里使用针孔相机模型:
 *  ![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70 3][]

简而言之，一个空间点\[x,y,z\]和它在图像中的像素坐标\[u,v,d\] (d指深度数据) 的对应关系是这样的：

![20190627150357251.png][]

这个公式是从(x,y,z)推到(u,v,d)的。反之，我们也可以把它写成已知(u,v,d)，推导(x,y,z)的方式.公式如下：

![20190627150451149.png][]

根据这个公式就可以构建点云

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70 4][]

如果相机发生了位移和旋转，那么只要对这些点进行位移和旋转操作即可

**从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( "../data/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后，这个指针指向的数据就是我们需要读取的数据啦
 *  **编译并运行**
 *  最后，我们在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 src/generatePointCloud.cpp )
        TARGET_LINK_LIBRARIES( generate_pointcloud ${OpenCV_LIBS} 
            ${PCL_LIBRARIES} )
 *  计算三维点坐标的公式我们已经给出过了，代码里原封不动地实现了一遍。我们根据这个公式，新增了一个空间点，并放入了点云中。最后，把整个点云存储为 ./data/pointcloud.pcd 文件
 *      cd build
        cmake ..
        make
        cd ..
    
    如果编译通过，就可在build目录下找到新写的二进制：generate\_pointcloud 运行它：
 *      ./generate_pointcloud

*  即可在data目录下生成点云文件。
 *  现在，你肯定希望查看一下新生成的点云了。如果已经安装了pcl，就可以通过：

pcl_viewer pointcloud.pcd

*  提示：

如果你打开点云，只看到红绿蓝三个方块，请按R重置视角。刚才你是站在原点盯着坐标轴看呢。
    如果点云没有颜色，请按5显示颜色。
    cmake过程可能有PCL的警告，如果你编译成功了，无视它即可。这是程序员的本能。

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70]: /images/20211221/99733cbf0a8545d78c557d2241b7ec64.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70 1]: /images/20211221/0574ed32dc264bb5bdc7b4e3fe97fbeb.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70 2]: /images/20211221/f70f9dd260ce409d821f82cbf82f840c.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70 3]: /images/20211221/769375169ae543c29b1de2ee63f22641.png
[20190627150357251.png]: /images/20211221/f356406c89414e17a0aaf7073b1cecfa.png
[20190627150451149.png]: /images/20211221/82dac0ee4af94eecbd3175bc013f3c16.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70 4]: /images/20211221/0d4d6e32ef77416a903ddf043396965f.png