一、前言
本文内容源自高翔博士新书《视觉SLAM十四讲》第5章相机与图像的实践环节。如果对SLAM感兴趣，强烈建议入手这本书。读这本书要比看网上这些胡乱拼凑的博客（包括本文）好得多。
二、什么是点云？
顾名思义，点云就是一大堆点放在一起，就像一朵云彩一样。所以说点云拼接是个挺有艺术感的事儿。
譬如说，我拿着RGB-D相机拍了一组照片。与普通相机不同的是，这些照片中的每个像素既有颜色又有深度。这深度值的作用可是非同小可，我们可以根据每个照片的拍摄角度和拍摄位置把这些照片组合起来，形成三维的立体图案。以下图所示的五张图片为例，它们是分别从不同角度拍摄的同一个房间。
接下来，通过点云拼接，我们就可以还原这个房间的三维场景。
三、预备工具
- Linux操作系统
- Eigen3.2.1，boost1.59.0，pcl1.7.2等开源库
- 从这里下载代码：https://github.com/jingedawang/PointCloudsJoint
四、开始写代码
Step1：读取RGB图片和深度图片，以及相机位姿数据。
在本例程中，相机位姿数据是已知的，保存在“pose.txt”文件中。不过对于实际的SLAM，相机位姿数据是通过前端视觉里程计，并通过后端优化才得出的。由于本文只关注点云拼接，因此直接使用相机位姿就可以了，而不必关心它是从何而来的。
int main( int argc, char** argv )
{
//Step 1
vector<cv::Mat> colorImgs, depthImgs; // 彩色图和深度图
vector<Eigen::Isometry3d> poses; // 相机位姿
ifstream fin("./pose.txt");
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 );
}
//Step 2...
}
代码中值得关注的有两个地方：
使用了
boost::format的格式化字符串功能拼接出图片文件名。取出相机位姿，包括用四元数表示的旋转和xyz轴平移，保存到变换矩阵T中。
Step 2：设定相机内参
相机内参用来将图片中的像素点转换到相机坐标系，进而再使用变换矩阵T变换到世界坐标系。
int main( int argc, char** argv )
{
//Step 1...
//Step 2
// 相机内参
double cx = 325.5;
double cy = 253.5;
double fx = 518.0;
double fy = 519.0;
double depthScale = 1000.0;
//Step 3...
}
Step 3：拼接点云
pcl点云库提供了非常方便的调用接口，只需要传入每个点的三维坐标和颜色，就可以把多张图片自动拼接到一起。
int main( int argc, char** argv )
{
//Step 1...
//Step 2...
//Step 3
// 定义点云使用的格式：这里用的是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用来保存该点在相机坐标系下的坐标
point[2] = double(d)/depthScale; //深度值根据深度相机规定的尺度因子确定
point[0] = (u-cx)*point[2]/fx; //计算x坐标，该等式来源于针孔相机模型
point[1] = (v-cy)*point[2]/fy; //计算y坐标，该等式也来源于针孔相机模型
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 );
}
这段代码的关键在于最内层的循环体，每次循环处理一个像素点。关键行的代码已经加了详细的注释。需要强调的有两点：
- 从像素坐标到相机坐标的变换来自于针孔相机模型，这部分理论知识本文不做过多解释。建议阅读《视觉SLAM十四讲》的第五章“相机与图像”以加深理解。
- 世界坐标系下的点是用
Eigen::Vector3d保存的，而点云中的点是用PointT保存的，它们并不兼容。
Step 4：编译运行
到了这步，按理说应该是一马平川了。谁知在测试的过程中反而遇到了许多麻烦，我把它们列出来供大家参考。
安装pcl库
需要注意的是不同系统的安装命令不一样，书上以Ubuntu14.04为例，而我的电脑是Debian 8，应当以官网的安装教程为准。此外，pcl库需要用到boost1.62.0，缺少的话运行时会报错。链接出错
使用find_package命令查找pcl库的时候，会把Qt5Widgets也加入链接列表。这是合理的，但是它却把Qt5Widgets库文件名当成了libQt5::Widgets.so，于是找不到链接文件。其实正确的名字是libQt5Widgets.so，没有中间的冒号。这类问题出现的原因现在还不太清楚，解决办法是，创建一个指向libQt5Widgets.so的软链接，软链接名字设为libQt5::Widgets.so，程序就能链接成功了。
五、运行效果
运行程序，会生成一个map.pcd文件，这是一个点云文件。输入命令
pcl_viewer map.pcd
即可看到如下所示的三维场景。而且可以鼠标控制旋转、缩放哦，是不是很炫酷呢！
