Slam c++编程学习——拼接点云

实验名称：点云拼接

本实验基于视觉Slam十四讲第五章的实验：
使用五张RGB图像，五张深度图和五个相机的位姿，来拼接点云，具体的代码如下：

#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;         // 相机位姿
    //vecto中的第一个参数表示变化矩阵，包括旋转和平移，而第二参数为分配器。 pose用来存储相机的位姿。
    
    ifstream fin("./pose.txt");
    //std::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" ); //图像文件格式
        //boost::format 类用来创建一个格式化的字符串模板，后面只要在占位符的地方填写具体内容即可。
        colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));
        depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1读取原始图像，
        //如果不加-1会按照设置的颜色标准来读取图像，会导致错误
        
        double data[7] = {0};
        for ( auto& d:data )
        //based for 循环 其中auto为自动推断数据的类型，而&d为引用，操作data对应的内存空间。
            fin>>d;
        //将数据流中的数据依次输入到d，>>为重载输入流操作符。
        Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );
        //构建四元数q
        Eigen::Isometry3d T(q);
        //将四元数转换为变换矩阵，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 ); 
    //构建了一个pointcloud指针，pointcloud指向堆区的空间，new为在堆区动态开辟一块空间。而不是在栈上
    //开辟空间，因为可以随时调整大小，不会受到栈上空间大小的限制。
    int j=0;
    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]; // 深度值
                //opencv中获取图像指定行的方法，返回第v行的指针，数据类型为unsigned short型。
                if ( d==0 ) continue; // 为0表示没有测量到
                Eigen::Vector3d point; 
                //eigen库中对三维向量的数据类型，在这里代表点的坐标。
                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() ];
                //opencv中.data为图像的数据指针，通过后面的式子计算其一维偏移量。
                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 );
                j++;
            }
    }
    //pcl中会默认height和width的值为0，如果不设一个值，后续使用pcl_viewer会报错
    //height  given (0) but no width!
    pointCloud->height=1;
    pointCloud->width=j;


    pointCloud->is_dense = false;
    cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
    pcl::io::savePCDFileBinary("map.pcd", *pointCloud );
    return 0;
}

代码的运行结果为保存一个pcd文件，使用pcl_viewer来可视化他：

pcl_viewer map.pcd

运行结果如下：