1. 建立点云地图
所谓点云，就是由一组离散的点表示的地图
最基本的点包含 x, y, z 三维坐标，也可以带有 r, g, b 的彩色信息
由于 RGB-D 相机提供了彩色图和深度图，很容易根据相机内参来计算 RGB-D 点云
如果通过某种手段，得到了相机的位姿
那么只要直接把点云进行加和，就可以获得全局的点云
在 视觉SLAM笔记（25） 拼接点云 给出了一个通过相机内外参拼接点云的例子
不过，在实际建图当中，还会对点云加一些滤波处理，获得更好的视觉效果
在本程序中，主要使用两种滤波器：外点去除滤波器以及降采样滤波器
由于部分代码与之前的相同，主要看改变的部分：
int main(int argc, char** argv)
{
vector<cv::Mat> colorImgs, depthImgs; // 彩色图和深度图
vector<Eigen::Isometry3d> poses; // 相机位姿
ifstream fin("../data/pose.txt");
if (!fin)
{
cerr << "cannot find pose file" << endl;
return 1;
}
for (int i = 0; i < 5; i++)
{
boost::format fmt("../data/%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 (int i = 0; i < 7; i++)
{
fin >> data[i];
}
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++)
{
PointCloud::Ptr current(new PointCloud);
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表示没有测量到
if (d >= 7000) continue; // 深度太大时不稳定，去掉
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];
current->points.push_back(p);
}
// 利用统计滤波器方法去除孤立点
PointCloud::Ptr tmp(new PointCloud);
pcl::StatisticalOutlierRemoval<PointT> statistical_filter;
statistical_filter.setMeanK(50);
statistical_filter.setStddevMulThresh(1.0);
statistical_filter.setInputCloud(current);
statistical_filter.filter(*tmp);
(*pointCloud) += *tmp;
}
pointCloud->is_dense = false;
cout << "点云共有" << pointCloud->size() << "个点." << endl;
// 体素滤波器
pcl::VoxelGrid<PointT> voxel_filter;
voxel_filter.setLeafSize(0.01, 0.01, 0.01); // resolution
PointCloud::Ptr tmp(new PointCloud);
voxel_filter.setInputCloud(pointCloud);
voxel_filter.filter(*tmp);
tmp->swap(*pointCloud);
cout << "滤波之后，点云共有" << pointCloud->size() << "个点." << endl;
pcl::io::savePCDFileBinary("map.pcd", *pointCloud);
return 0;
}
思路没有太大变化，主要不同之处在于：
- 在生成每帧点云时，去掉深度值太大或无效的点
这主要是考虑到 Kinect 的有效量程，超过量程之后的深度值会有较大误差 - 利用统计滤波器方法去除孤立点
该滤波器统计每个点与它最近 N 个点的距离值的分布，去除距离均值过大的点
这样，保留了那些“粘在一起”的点，去掉了孤立的噪声点 - 最后，利用体素滤波器（Voxel Filter）进行降采样
由于多个视角存在视野重叠，在重叠区域会存在大量的位置十分相近的点，这会无益地占用许多内存空间
体素滤波保证在某个一定大小的立方体（或称体素）内仅有一个点
相当于对三维空间进行了降采样，从而节省了很多存储空间
运行程序
$ ./pointcloud_mapping
最后，把生成的点云以 pcd 格式存储在 map.pcd 中
用 PCL 提供的可视化程序打开这个文件：
$ pcl_viewer map.pcd
可能需要放大才能看清，或者自己尝试一下
左侧是 视觉SLAM笔记（25） 拼接点云 生成的点云地图
而右侧是经过滤波后的点云地图
观察白色框中部分，可以看到在滤波前存在着由噪声产生的许多孤立的点
经过统计外点去除之后，消去了这些噪声，使得整个地图变得更干净
另一方面，在体素滤波器中，把分辨率调至 0.01，表示每立方厘米有一个点
这是一个比较高的分辨率，所以在截图中我们感觉不出地图的差异
然而程序输出中可以看到点数明显减少了许多（从 90 万个点减到了 44 万个点，去除了一半左右）
点云地图为提供了比较基本的可视化地图，能够大致了解环境的样子
它以三维方式存储，使得能够快速地浏览场景的各个角落，乃至在场景中进行漫游
点云的一大优势是可以直接由 RGB-D 图像高效地生成，不需要额外的处理
它的滤波操作也非常直观，且处理效率尚能接受
2. 点云地图
不过，使用点云表达地图仍然是十分初级的
不妨按照之前提的对地图的需求，看看点云地图是否能满足：
- 定位需求：取决于前端视觉里程计的处理方式
如果是基于特征点的视觉里程计，由于点云中没有存储特征点信息，所以无法用于基于特征点的定位方法
如果前端是点云的 ICP，那么可以考虑将局部点云对全局点云进行 ICP 以估计位姿
然而，这要求全局点云具有较好的精度
在这种处理点云的方式中，并没有对点云本身进行优化，所以是不够的 - 导航与避障的需求：无法直接用于导航和避障
纯粹的点云无法表示“是否有障碍物”的信息，也无法在点云中做“任意空间点是否被占据”这样的查询
而这是导航和避障的基本需要
不过，可以在点云基础上进行加工，得到更适合导航与避障的地图形式 - 可视化和交互：具有基本的可视化与交互能力
能够看到场景的外观，也能在场景里漫游
从可视化角度来说，由于点云只含有离散的点，而没有物体表面信息（例如法线）
所以不太符合人们对可视化习惯
例如，点云地图的物体从正面看和背面看是一样的，而且还能透过物体看到它背后的东西
这些都不符合日常的经验，因为没有物体表面的信息
综上所述，点云地图是“基础”的或“初级的”，是指它更接近于传感器读取的原始数据
它具有一些基本的功能，但通常用于调试和基本的显示，不便直接用于应用程序
3. 其他重建方法
如果希望地图有更高级的功能，点云地图是一个不错的出发点
例如，针对导航功能，可以从点云出发，构建占据网格地图（Occupancy Grid），以供导航算法查询某点是否可以通过
再如， SfM 中常用的泊松重建方法，就能通过基本的点云重建物体网格地图，得到物体的表面信息
除泊松重建之外， Surfel 亦是一种表达物体表面的方式，以面元作为地图的基本单位，能够建立漂亮的可视化地图
显示了泊松重建和 surfel 的一个样例
可以看成它们的视觉效果明显优于纯粹点云建图，而它们都可以通过点云进行构建
大部分由点云转换得到的地图形式都在 PCL库中提供
参考：
相关推荐：
视觉SLAM笔记（62） 单目稠密重建
视觉SLAM笔记（61） 单目稠密建图
视觉SLAM笔记（60） 建图
视觉SLAM笔记（59） 相似度计算
视觉SLAM笔记（58） 字典
谢谢！
