RBG-D点云拼接

灰太狼 2022-11-25 04:28 159阅读 0赞

**目录**

1 说明

2 代码演示

CMakeLists.txt

joinmap.cpp

3 运行结果

--------------------

**目标：****输入 RGB图像 + Depth深度图 + 对应的每幅图像的pose位姿， 输出一个三维点云模型(地图) map.pcd**

# 1 说明 #

*  **RBG图像（color）**：提供像素坐标(u,v)，使用以下公式（及相机内参），转换到相机坐标系下 (x,y)。**(u,v) --> (x,y)**
    
     *  ![20200807142925551.png][] =》 ![2020080714293754.png][]
     *  d: 相机坐标系下每个像素点对应的深度； K:相机内参（fx, fy, Cx, Cy）； Pc: 相机坐标系下的空间点坐标
 *  **深度图像（depth）**：提供深度值z，构成相机坐标系下的三维坐标 (x,y,z)
 *  **pose.txt文件**：提供每幅图像对应的相机位姿（旋转矩阵T：R,t），将相机坐标系下的三维坐标，变换到世界坐标系下
    
     *  pose文件的一行7个值：![20200807143726633.png][]，前三项是相机的位置(t)，后四项是相机的旋转R(四元数表示形式)
 *  文件列表：
    
    ├── ./CMakeLists.txt  
    ├── ./color  
    │ ├── ./color/1.png  
    │ ├── ./color/2.png  
    │ ├── ./color/3.png  
    │ ├── ./color/4.png  
    │ └── ./color/5.png  
    ├── ./depth  
    │ ├── ./depth/1.pgm  
    │ ├── ./depth/2.pgm  
    │ ├── ./depth/3.pgm  
    │ ├── ./depth/4.pgm  
    │ └── ./depth/5.pgm  
    ├── ./joinMap.cpp  
    └── ./pose.txt
 *  1.png
    
     *  ![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70][]

# 2 代码演示 #

*  用到的库：eigen、opencv 和 pcl

## CMakeLists.txt ##

cmake_minimum_required( VERSION 2.8 )
    project( joinMap )
    
    set( CMAKE_BUILD_TYPE Release )
    set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )
    
    # opencv 
    find_package( OpenCV REQUIRED )
    include_directories( ${OpenCV_INCLUDE_DIRS} )
    
    # eigen 
    include_directories( "/usr/include/eigen3/" )
    
    # pcl 
    find_package( PCL REQUIRED COMPONENT common io )
    include_directories( ${PCL_INCLUDE_DIRS} )
    add_definitions( ${PCL_DEFINITIONS} )
    
    #报错添加 https://blog.csdn.net/danmeng8068/article/details/77341532
    list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4") 
    
    add_executable( joinMap joinMap.cpp )
    target_link_libraries( joinMap ${OpenCV_LIBS} ${PCL_LIBRARIES} )

## joinmap.cpp ##

#include <iostream>
    #include <fstream>
    using namespace std;
    #include <opencv2/core/core.hpp>
    #include <opencv2/highgui/highgui.hpp>
    #include <Eigen/Geometry> 
    #include <boost/format.hpp>  // for formating strings
    #include <pcl/point_types.h> 
    #include <pcl/io/pcd_io.h> 
    #include <pcl/visualization/pcl_visualizer.h>
    
    int main( int argc, char** argv )
    {
        vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
        vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;         // 相机位姿
        
        ifstream fin("./pose.txt");
        if (!fin)
        {
            cerr<<"请在有pose.txt的目录下运行此程序"<<endl;
            return 1;
        }
        
        for ( int i=0; i<5; i++ )
        {
            boost::format fmt( "./%s/%d.%s" ); //图像文件格式
            colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));
            depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1读取原始图像
            
            double data[7] = {0};
            for ( auto& d:data )
                fin>>d;
            Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );
            Eigen::Isometry3d T(q);
            T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));
            poses.push_back( T );
        }
        
        // 计算点云并拼接
        // 相机内参 
        double cx = 325.5;
        double cy = 253.5;
        double fx = 518.0;
        double fy = 519.0;
        double depthScale = 1000.0;
        
        cout<<"正在将图像转换为点云..."<<endl;
        
        // 定义点云使用的格式：这里用的是XYZRGB
        typedef pcl::PointXYZRGB PointT; 
        typedef pcl::PointCloud<PointT> PointCloud;
        
        // 新建一个点云
        PointCloud::Ptr pointCloud( new PointCloud ); 
        for ( int i=0; i<5; i++ )
        {
            cout<<"转换图像中: "<<i+1<<endl; 
            cv::Mat color = colorImgs[i]; 
            cv::Mat depth = depthImgs[i];
            Eigen::Isometry3d T = poses[i];
            for ( int v=0; v<color.rows; v++ )
                for ( int u=0; u<color.cols; u++ )
                {
                    unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值
                    if ( d==0 ) continue; // 为0表示没有测量到
                    Eigen::Vector3d point; 
                    point[2] = double(d)/depthScale; 
                    point[0] = (u-cx)*point[2]/fx;  //像素坐标系-》相机坐标系
                    point[1] = (v-cy)*point[2]/fy; 
                    Eigen::Vector3d pointWorld = T*point; //相机坐标系-》世界坐标系
                    
                    PointT p ;
                    p.x = pointWorld[0];
                    p.y = pointWorld[1];
                    p.z = pointWorld[2];
                    p.b = color.data[ v*color.step+u*color.channels() ];
                    p.g = color.data[ v*color.step+u*color.channels()+1 ];
                    p.r = color.data[ v*color.step+u*color.channels()+2 ];
                    pointCloud->points.push_back( p ); //点云合成
                }
        }
        
        pointCloud->is_dense = false;
        cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
        pcl::io::savePCDFileBinary("map.pcd", *pointCloud );
        return 0;
    }

# 3 运行结果 #

![20200807161821739.png][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 1][]

报错解决方法：

*  [Ubuntu16.04安装pcl出现cannot find -lvtkproj4解决办法][Ubuntu16.04_pcl_cannot find -lvtkproj4]
 *  [No rule to make target '/usr/lib/x86\_64-linux-gnu/libproj.so 解决方法 ][No rule to make target _usr_lib_x86_64-linux-gnu_libproj.so _]

参考：

*  高翔视觉SLAM十四讲 chapter 5：[https://github.com/gaoxiang12/slambook/tree/master/ch5/joinMap][https_github.com_gaoxiang12_slambook_tree_master_ch5_joinMap]

[20200807142925551.png]: https://img-blog.csdnimg.cn/20200807142925551.png
[2020080714293754.png]: /images/20221124/1084501af10645299146d5652f349411.png
[20200807143726633.png]: /images/20221124/eaaaa4a24b18428a8215a0513afe0bc3.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70]: /images/20221124/78352c6133de46e3adbf5d571c95f440.png
[20200807161821739.png]: /images/20221124/b62e212815c7417c95930a2ca8e61469.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 1]: /images/20221124/a3bd43832b844d20a235e7268c6ab22d.png
[Ubuntu16.04_pcl_cannot find -lvtkproj4]: https://blog.csdn.net/danmeng8068/article/details/77341532
[No rule to make target _usr_lib_x86_64-linux-gnu_libproj.so _]: https://blog.csdn.net/sss_369/article/details/94857989
[https_github.com_gaoxiang12_slambook_tree_master_ch5_joinMap]: https://github.com/gaoxiang12/slambook/tree/master/ch5/joinMap