视觉SLAM ch12 建图（RGB-D）
一、RGB-D稠密建图
RGB-D相机通结构光和飞行时间获取深度。
稠密重建方法：根据估计的相机位姿，将RGB-D数据转化为点云，然后进行拼接，最终得到由离散的点组成的点云地图。
在此基础上，如果希望估计物体的表面，可以用三角网格（Mesh）和面片（Surfel）进行建图；如果希望知道地图的障碍物信息，可以通过体素（Voxel）建立占据网格地图（Occupancy Map）。
二、点云地图
对点云地图还需要进行滤波处理：外点去除滤波器和体素网格的降采样滤波器（Voxel grid filter）
1.PCL点云库头文件，参考【1】
#include <boost/make_shared.hpp> //boost指针相关头文件
#include <pcl/point_types.h> //点类型定义头文件
#include <pcl/point_cloud.h> //点云类定义头文件
#include <pcl/features/normal_3d.h> //法线特征头文件
#include <pcl/features/normal_3d_omp.h> //法线特征加速头文件
#include <pcl/features/fpfh.h> //fpfh类特征头文件
#include <pcl/features/fpfh_omp.h> //fpfh加速类头文件
#include <pcl/features/boundary.h> //边界提取头文件
#include <pcl/search/kdtree.h> //kdtree搜索对象类头文件
#include <pcl/point_representation.h> //点表示相关的头文件
#include <pcl/io/pcd_io.h> //PCD文件打开存储类头文件
#include <pcl/filters/passthrough.h> //直通滤波器头文件
#include <pcl/filters/voxel_grid.h> //用于体素网格化的滤波类头文件
#include <pcl/filters/filter.h> //滤波相关头文件
#include <pcl/filters/project_inliers.h> //点云投影头文件
#include <pcl/filters/extract_indices.h> //索引提取头文件
#include <pcl/filters/statistical_outlier_removal.h> //统计离群点类头文件
#include <pcl/segmentation/sac_segmentation.h> //RanSaC分割
#include <pcl/segmentation/region_growing.h> //区域生长
#include <pcl/segmentation/region_growing_rgb.h> //基于颜色的区域生长
#include <pcl/segmentation/supervoxel_clustering.h> //超体聚类
#include <pcl/segmentation/lccp_segmentation.h> //基于凹凸性分割
#include <pcl/surface/mls.h> //点云平滑类头文件
#include <pcl/registration/icp.h> //ICP类相关头文件
#include <pcl/registration/icp_nl.h> //非线性ICP 相关头文件
#include <pcl/registration/transforms.h> //变换矩阵类头文件
#include <pcl/registration/ia_ransac.h> //sac-ia类头文件
#include <pcl/registration/correspondence_estimation.h> //直方图配准
#include <pcl/registration/correspondence_rejection_features.h>//特征的错误对应关系去除
#include <pcl/registration/correspondence_rejection_sample_consensus.h>//随机采样一致性去除
#include <pcl/visualization/pcl_visualizer.h> //可视化类头文件typedef pcl::PointXYZ Pxyz;
typedef pcl::PointXYZRGB Prgb;
typedef pcl::PointCloud<Pxyz> PXYZ;
typedef pcl::PointCloud<Prgb> PRGB;typedef pcl::Normal Pnl;
typedef pcl::PointNormal Pnt;
typedef pcl::PointCloud<Pnt> PNT;2.点云的创建、访问与转换以及读写
//创建点云指针对象
//PointXYZRGB是单个点包含xyz以及rgb信息
pcl::PointCloud< pcl::PointXYZRGB>::Ptr pointCloud (new pcl::PointCloud< pcl::PointXYZRGB>);//访问点云
pcl::PointXYZRGB p;
p.x = xx;
p.y = xx;
p.z = xx;
p.b = xx;
p.g = xx;
p.r = xx;pointCloud->points.push_back(p);
//points是个vector变量
//转换
pcl::PointCloud<pcl::PointXYZRGB> Cloud //点云对象pcl::PointCloud< pcl::PointXYZRGB>::Ptr pointCloud (new pcl::PointCloud< pcl::PointXYZRGB>); //点云指针对象cloud = * pointCloud ;
pointCloud = cloud.makeShared();//读
pcl::PointCloud< pcl::PointXYZRGB>::Ptr pointCloud(new pcl::PointCloud< pcl::PointXYZRGB>);
if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("map.pcd", *pointCloud) == -1){cout<<("读取pcd失败");return 1;}cout << "读取到 "<< pointCloud_read->width * pointCloud_read->height << " 个点"<<endl;//写
pcl::io::savePCDFileBinary("map.pcd", *pointCloud); //二进制形式写入文件
pcl::io::savePCDFileASCII("map.pcd", *pointCloud); //以ascii格式写入文件3.统计滤波器
StatisticalOutlierRemoval 滤波器主要用于剔除点云中的离群点。滤波思想为:对每一个点的利用 KD-tree 进行一个统计分析，利用KD-tree得到的结果，计算该点到所有临近点的平均距离。假设每个点与临近点之间的距离是一个高斯分布，那么平均距离就是均值，并且可以根据均值和每一个距离求得标准差，那么平均距离在标准范围之外的点，可以被定义为离群点并从数据中去除。【2】
// 新建临时点云
pcl::PointCloud< pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud< pcl::PointXYZRGB>); //点云指针对象
// 创建滤波器对象
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> statistical_filter;statistical_filter.setMeanK(50);//设置在进行统计时考虑查询点邻居点数
statistical_filter.setStddevMulThresh(1.0);//设置判断是否为离群点的阈值
statistical_filter.setInputCloud(current);//设置待滤波的点云(导入当前点云)//将滤波结果保存在临时点云中，并将临时点云存入总点云
statistical_filter.filter(*tmp);
(*pointCloud) += *tmp;4.体素滤波器
利用体素网格滤波器进行降采样。由于多个视角存在视野重叠，在重叠区域会存在大量的位置十分相近的点。会占据内存空间。体素滤波器保证了在某个一定大小的立方体(体素)内仅有一个点，相当于对三维空间进行了降采样，从而节省了存储空间。
在体素滤波器中，把分辨率设成0.03，表示每个0.03x0.03x0.03的立方体中只有一个点，这个分辨率较高，所以实际中感觉不到地图的差异，单程序输出中点数明显减少。
// 体素滤波器
pcl::VoxelGrid<PointT> voxel_filter;//体素网格分辨率为0.03 * 0.03 * 0.03，一个立方体晶格存放一个点
double resolution = 0.03;
voxel_filter.setLeafSize(resolution, resolution, resolution); // resolution//临时点云
pcl::PointCloud< pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud< pcl::PointXYZRGB>);// 输入总点云，降采样结果保存到tmp
voxel_filter.setInputCloud(pointCloud);
voxel_filter.filter(*tmp);
// 交换总点云与临时点云
tmp->swap(*pointCloud);
5.点云可视化
#include <pcl/visualization/cloud_viewer.h>pcl::visualization::CloudViewer viewer ("点云");
viewer.showCloud (pointCloud);
while (!viewer.wasStopped ())
{}Cloudviewer只是一个简单的可视化工具，其功能只有有限的showCloud,以及注册键盘、鼠标的回调函数。更加复杂的工具是PCLVisualizer，参考【3】
#include <pcl/visualization/cloud_viewer.h>
#include <thread>
#include <chrono>//初始化
pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));//设置背景颜色
viewer->setBackgroundColor (0, 0, 0);
//添加坐标系（即红绿蓝三色轴，放置在原点）
viewer->addCoordinateSystem (3.0);//3.0指轴的长度
//viewer->addCoordinateSystem (3.0,1,2,3);一个重载函数，3.0指轴的长度，放置在（1，2，3）位置
//初始化默认相机参数
viewer->initCameraParameters ();//创建一个点云的句柄
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointCloud);
//将点云加入到viewer，并定义一个唯一的字符串作为ID号，利用此字符串在其他成员方法中也能表示引用该点云
viewer->addPointCloud<pcl::PointXYZRGB> (pointCloud, rgb, "label_pc");//用于改变显示点云的尺寸，可以利用该方法控制点云在视窗中的显示方式
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "label_pc");
while (!viewer->wasStopped ())
{viewer->spinOnce (100); //更新显示boost::this_thread::sleep (boost::posix_time::microseconds (100));
}点云地图提高了基本的可视化地图，但不能满足对于稠密地图的需求。
1）定位：取决与前端视觉里程计的处理方式。如果是基于特征点的视觉里程计，由于点云中没有存储特征点的信息，则无法用于基于特征点的定位方式。如果前端是点云的ICP，那么可以将局部点云对全局点云进行ICP以估计位姿。
2）导航与避障:点云无法直接用于避障，纯粹的点云无法表示“是否有障碍物",不过，可以在点云的基础上加工，得到更适合导航避障的地图形式。
3）可视化和交互:点云只有离散的点，没有物体表面的信息，所以不符合可视化要求。
点云是基础的，初级的，接近传感器读取的原始数据。更高级的功能，如导航避障，可以从点云出发，构建占据网格地图,以供导航算法查询某点是否能通过。SfM（三维重建）中常使用的柏松重建，就能通过基本的点云重建物体网格地图，得到物体的表面信息。还有Surfel也是一种表达物体表面的方式，以面元作为地图的基本单位，能建立可视化地图。
三、点云重建网格
思路：先计算点云的法线，在从法线计算网格。
//
// Created by gaoxiang on 19-4-25.
//#include <pcl/point_cloud.h> //点云类定义头文件
#include <pcl/point_types.h> //点类型定义头文件
#include <pcl/io/pcd_io.h> //PCD文件打开存储类头文件
#include <pcl/visualization/pcl_visualizer.h> //可视化类头文件
#include <pcl/kdtree/kdtree_flann.h> //用于对点云进行Kd树搜索
#include <pcl/surface/surfel_smoothing.h> //对点云进行平滑处理
#include <pcl/surface/mls.h> //点云平滑类头文件
#include <pcl/surface/gp3.h> //对点云进行三角化处理
#include <pcl/surface/impl/mls.hpp>// typedefs
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointCloud<PointT>::Ptr PointCloudPtr;typedef pcl::PointXYZRGBNormal SurfelT;
typedef pcl::PointCloud<SurfelT> SurfelCloud;
typedef pcl::PointCloud<SurfelT>::Ptr SurfelCloudPtr;//把基本点云转化成光滑的带有法线信息的点云
SurfelCloudPtr reconstructSurface(const PointCloudPtr &input, float radius, int polynomial_order) {// 定义对象 (第二种定义类型是为了存储法线 pcl::MovingLeastSquares<PointT, SurfelT> mls;//由于MLS算法是基于KDTree实现的，所以需要创建KDTree对象pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);mls.setSearchMethod(tree);//搜索半径mls.setSearchRadius(radius);//计算法线mls.setComputeNormals(true);// 设置近邻高斯权重系数，一般为搜索半径平方mls.setSqrGaussParam(radius * radius);//设置是否使用多项式拟合提高精度mls.setPolynomialFit(polynomial_order > 1);// 设置阶数mls.setPolynomialOrder(polynomial_order);// 输入点云mls.setInputCloud(input);SurfelCloudPtr output(new SurfelCloud);// 处理结果保存到输出点云并放返回mls.process(*output);return (output);
}// 为点云表面添加三角网格面元
pcl::PolygonMeshPtr triangulateMesh(const SurfelCloudPtr &surfels) {// 创建搜索树pcl::search::KdTree<SurfelT>::Ptr tree(new pcl::search::KdTree<SurfelT>);tree->setInputCloud(surfels);// Initialize objectspcl::GreedyProjectionTriangulation<SurfelT> gp3;pcl::PolygonMeshPtr triangles(new pcl::PolygonMesh);// Set the maximum distance between connected points (maximum edge length)gp3.setSearchRadius(0.05);// Set typical values for the parametersgp3.setMu(2.5);gp3.setMaximumNearestNeighbors(100);gp3.setMaximumSurfaceAngle(M_PI / 4); // 45 degreesgp3.setMinimumAngle(M_PI / 18); // 10 degreesgp3.setMaximumAngle(2 * M_PI / 3); // 120 degreesgp3.setNormalConsistency(true);// Get resultgp3.setInputCloud(surfels);gp3.setSearchMethod(tree);gp3.reconstruct(*triangles);return triangles;
}int main(int argc, char argv) {// Load the pointsPointCloudPtr cloud(new PointCloud);if (argc == 0 || pcl::io::loadPCDFile(argv[1], *cloud)) {cout << "failed to load point cloud!";return 1;}cout << "point cloud loaded, points: " << cloud->points.size() << endl;// Compute surface elementscout << "computing normals ... " << endl;double mls_radius = 0.05, polynomial_order = 2;auto surfels = reconstructSurface(cloud, mls_radius, polynomial_order);// Compute a greedy surface triangulationcout << "computing mesh ... " << endl;pcl::PolygonMeshPtr mesh = triangulateMesh(surfels);cout << "display mesh ... " << endl;pcl::visualization::PCLVisualizer vis;vis.addPolylineFromPolygonMesh(*mesh, "mesh frame");vis.addPolygonMesh(*mesh, "mesh");vis.resetCamera();vis.spin();
}四、八叉树地图
点云地图与八叉树地图对比：点云地图规模大，而且无法处理运动物体；八叉树地图占用空间很小，而且可以用于导航。
八叉树octomap参考【4】
五、如何在八叉树地图中进行导航或路径规划吗？
在起点与终点之间，选择概率值最小的路径。若概率值最小的路径仍超过某一阈值，则说明无法到达终点。若有多条概率值小的路径，可以根据高斯分布选择路最“宽”的路径。
