解决 autoware 里的 ndt-mapping 算法不能建太大地图的问题

把 1 个大的点云bag包分成 n 个连续的小点云bag包（分包的时候注意！后一个包的第一帧必须是前一个包的最后一帧，除了第一个包外，每一个包都必须是这样），再对这 n 个小bag包分别用 autoware 里的 ndt-mapping 算法建成 n 个连续的点云地图，最后用下面的代码把这 n 个地图拼接在一起：

文件命名为：main.cpp ，全部代码如下（C++）：

// 把第 n 个点云地图和第 n-1 个点云地图进行拼接

#include <iostream>
#include <Eigen/Dense>
#include "Eigen/Core"
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include <vector>

//根据欧拉角计算旋转矩阵
Eigen::Matrix3d rotation(std::vector<double> pose){
    Eigen::Matrix3d R;
    R = Eigen::AngleAxisd(pose[5], Eigen::Vector3d::UnitZ()) *
           Eigen::AngleAxisd(pose[4], Eigen::Vector3d::UnitY()) *
           Eigen::AngleAxisd(pose[3], Eigen::Vector3d::UnitX());
    return R;
}

int main() {
    //第 n-1 个bag包建图时输出的最后一帧点云的位姿（通过 ndt-mapping 在终端的输出得到）
    //pose: x,y,z,roll,pitch,yaw，其中 x、y、z 以米为单位，roll、pitch、yaw 以弧度为单位
    std::vector<double> pose = {-0.895841, 114.148, -0.741639, -0.0389029, 0.00106248, -0.00255419};

    //根据欧拉角（roll,pitch,yaw）计算旋转矩阵
    Eigen::Matrix3d R;
    R = rotation(pose);
    //计算平移向量
    Eigen::Vector3d T(pose[0], pose[1], pose[2]);

    //把旋转矩阵 R 和平移向量 T合并到一个 4×4 转换矩阵里
    Eigen::Matrix4d transform = Eigen::Matrix4d::Identity();
    transform.block<3,3>(0,0) = R; // first 3x3 block set to rotation matrix
    transform.block<3,1>(0,3) = T; // fourth column set to translation vector

    //第 n 个点云地图
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_1(new pcl::PointCloud<pcl::PointXYZI>);
    //转换后的第 n 个点云地图
    pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_cloud_1(new pcl::PointCloud<pcl::PointXYZI>);
     //第 n-1 个点云地图
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_2(new pcl::PointCloud<pcl::PointXYZI>);
    //第 n 个和第 n-1 个点云地图合并后的新的点云地图
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_all(new pcl::PointCloud<pcl::PointXYZI>);
    //第 n 个点云地图
    pcl::io::loadPCDFile("/media/zy/UBUNTU 18_0/test_8.pcd", *cloud_1);
    //第 n-1 个点云地图
    pcl::io::loadPCDFile("/media/zy/UBUNTU 18_0/test_7.pcd", *cloud_2);

    //转换后的第 n 个点云地图
    pcl::transformPointCloud(*cloud_1, *transformed_cloud_1, transform);

    //第 n 个和第 n-1 个点云地图合并后的新的点云地图
    *cloud_all = *cloud_2 + *transformed_cloud_1;
    //保存为ASCII格式的pcd点云文件
    //pcl::io::savePCDFile("/media/zy/UBUNTU 18_0/test_all_2.pcd", *cloud_all);
    //保存为二进制格式的pcd点云文件
    pcl::io::savePCDFileBinary("/media/zy/UBUNTU 18_0/test_all.pcd", *cloud_all);

    return 0;
}

全部 CMakeLists.txt 如下，CMakeLists.txt 要和上面的 main.cpp 代码放在同级目录下：

cmake_minimum_required(VERSION 3.10)
project(untitled3)

set(CMAKE_CXX_STANDARD 14)

find_package( PCL REQUIRED COMPONENT common io )
include_directories( ${PCL_INCLUDE_DIRS} )
add_definitions( ${PCL_DEFINITIONS} )

add_executable(untitled3 main.cpp)
target_link_libraries(untitled3 ${PCL_LIBRARIES})

注意！！

1、先把第 n 个点云地图和第 n-1 个点云地图进行合并，再把合并后的地图和第 n-2 个点云地图进行合并，然后再把合并后的地图和第 n-3 个点云地图进行合并…，这样一直合并到第 1 个点云地图，最终得到一个完整的点云地图。

如果运行时报错：Process finished with exit code 139 (interrupted by signal 11: SIGSEGV)，说明可能是由于要合并的点云地图过大，这时可以先把点云进行滤波（下采样），再进行合并。

步骤如下：

1、在头文件中加入：

#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>

2、在倒数第二行代码 *cloud_all = *cloud_2 + *transformed_cloud_1; 和倒数第一行代码 pcl::io::savePCDFileBinary("/media/zy/UBUNTU 18_0/test_all.pcd", *cloud_all); 之间插入：

pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>);

double filter_res= 0.2;  //体素的大小，值越小保存下来的点云越多，地图体积相应的也会越大。
pcl::VoxelGrid<pcl::PointXYZI> filter;
filter.setLeafSize(filter_res, filter_res, filter_res);
filter.setInputCloud(cloud_all);
filter.filter(*cloud_filtered);

并把最后一行的 pcl::io::savePCDFileBinary("/media/zy/UBUNTU 18_0/test_all.pcd", *cloud_all); 相应的改成：pcl::io::savePCDFileBinary("/media/zy/UBUNTU 18_0/test_all.pcd", *cloud_filtered); 。

3、在 CMakeList.txt 的 find_package( PCL REQUIRED COMPONENT common io ) 后面加入：find_package(PCL REQUIRED QUIET)。

4、这样保存下来的就是滤波后的点云地图了，体积会大大减小，再和后面的点云地图进行合并就不会出现上面的问题了。

如果代码在下采样时报错：[pcl::VoxelGrid::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.

说明体素的体积设的太小了，而点云地图又太大，导致体素的总数目超过了限制，这时只需要把体素大小 filter_res 改大就可以了。

参考文献：

1、彻底搞懂“旋转矩阵/欧拉角/四元数”，让你体会三维旋转之美
2、欧拉角和旋转矩阵之间的转换
3、Eigen中旋转矩阵和欧拉角的转换
4、Eigen中四元数、欧拉角、旋转矩阵、旋转向量之间的转换
5、PCL：点云平移、旋转
6、PCL：合并两个点云
7、[pcl::VoxelGrid::applyFilter] Leaf size is too small for the input dataset. Integer indices would ov
8、Leaf size is too small for the input dataset. PCL Voxel Grid 问题
9、Leaf size is too small for the input dataset 解决办法