第 7 章 PCL 库 ｜ 实现点云拼接

7.1 PCL 库

官网：http://pointclouds.org/。
安装：（Ubuntu16.04）

sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl
sudo apt-get update
sudo apt-get install libpcl-dev

7.2 拼接点云

完整代码：
  https://github.com/deng-ke/slam-base/tree/master/joinMap。

输入：

• color/：RGB 图像
• depth/：深度图像
• pose.txt：每幅图对应的相机位姿（T_wc 形式，与公式相反），格式为平移向量加四元数。
  $$

[x,y,z,q_x,q_y,q_z,q_w]

$$
过程：

7.2.1 读入数据

  读入 RGB、深度图像，直接给定相机内参，读入外参。

/*读入RGB、深度图像，生成外参*/
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;

7.2.2 转换到相机坐标

  根据内参方程（逆向），将像素坐标转换为相机坐标。

point[2] = double(d)/depthScale; 
point[0] = (u-cx)*point[2]/fx;
point[1] = (v-cy)*point[2]/fy;

7.2.3 转换到世界坐标

  把相机坐标转换为世界坐标。

Eigen::Vector3d pointWorld = T*point;

7.2.4 拼接点云（地图）

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 );

7.2.5 结果

$$