【自动驾驶】《无人驾驶原理与实践》源码教程（二）——NDT算法实例

教程基于《无人驾驶原理与实践》3.3.4，请结合此书学习。

教程源码下载：https://download.csdn.net/download/xiangxiang613/13192600

解压后，此教程文件结构如下：
在这里插入图片描述
build文件夹：

说明：
1.cloud1.pcd和cloud2.pcd为用于配准的点云文件，cloud3.pcd为保存配准后的点云图文件，最终将用于可视化。

2.修改程序：
原配置文件中的路径为：/home/adam/book_code/chapter_3/NDT_PCL_demo
需要修改为自己的项目文件路径后才能cmake通过，eg：/home/tzx/book_code/chapter_3/NDT_PCL_demo

涉及的文件有：build/CMakeCache.txt, build/cmake_install.cmake, 以及build/Makefile，
用查找替换功能很方便替换！

3.需要编写的核心文件为nomal_distributions_transform.cpp（文件名在书中没有提及名称）

nomal_distributions_transform.cpp代码：

#include <iostream>
#include <pcl/io/pcd_io.h>

#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>

#include <pcl/visualization/pcl_visualizer.h>

// 代码清单3-5 读取点云信息
pcl::PointCloud<pcl::PointXYZ>::Ptr read_cloud_point(std::string const &file_path){
    // Loading first scan.
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ> (file_path, *cloud) == -1)
    {
        PCL_ERROR ("Couldn't read the pcd file\n");
        return nullptr;
    }
    return cloud;
}

// 代码清单3-13 将配准以后的点云图可视化
void visualizer(pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud){
    // Initializing point cloud visualizer
    boost::shared_ptr<pcl::visualization::PCLVisualizer>
            viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer_final->setBackgroundColor (0, 0, 0);

    // Coloring and visualizing target cloud (red).
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
            target_color (target_cloud, 255, 0, 0);
    viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
    viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                    1, "target cloud");

    // Coloring and visualizing transformed input cloud (green).
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
            output_color (output_cloud, 0, 255, 0);
    viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
    viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                    1, "output cloud");

    // Starting visualizer
    viewer_final->addCoordinateSystem (1.0, "global");
    viewer_final->initCameraParameters ();

    // Wait until visualizer window is closed.
    while (!viewer_final->wasStopped ())
    {
        viewer_final->spinOnce (100);
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }
}

int main(int argc, char **argv) {
	// 代码清单3-6 读取点云 
    auto target_cloud = read_cloud_point(argv[1]);   // arg[1]就是cloud1.pcd
    std::cout << "Loaded " << target_cloud->size () << " data points from cloud1.pcd" << std::endl;

    auto input_cloud = read_cloud_point(argv[2]);	 // arg[2]就是cloud2.pcd
    std::cout << "Loaded " << input_cloud->size () << " data points from cloud2.pcd" << std::endl;

	// 代码清单3-8 过滤输入点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
    approximate_voxel_filter.setLeafSize(0.2, 0.2, 0.2);

    approximate_voxel_filter.setInputCloud(input_cloud);
    approximate_voxel_filter.filter(*filtered_cloud);

    std::cout<<"Filtered cloud contains "<< filtered_cloud->size() << "data points from cloud2.pcd" << std::endl;

	// 代码清单3-10 初始化NDT并且设置NDT参数
    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
    ndt.setTransformationEpsilon(0.01);
    ndt.setStepSize(0.1);
    ndt.setResolution(1.0);

    ndt.setMaximumIterations(35);
    ndt.setInputSource(filtered_cloud);
    ndt.setInputTarget(target_cloud);

	// 代码清单3-11 初始化变换参数并开始优化
    Eigen::AngleAxisf init_rotation(0.6931, Eigen::Vector3f::UnitZ());
    Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
    Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();

    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);

    ndt.align(*output_cloud, init_guess);
    std::cout << "Normal Distribution Transform has converged:" << ndt.hasConverged()
              << "score: " << ndt.getFitnessScore() << std::endl;

	// 代码清单3-12 保存配准以后的点云图，输出到文件cloud3.pcd
    pcl::transformPointCloud(*input_cloud, *output_cloud, ndt.getFinalTransformation());
    pcl::io::savePCDFileASCII("../cloud3.pcd", *output_cloud);

	// 代码清单3-13 将配准以后的点云图可视化
    visualizer(target_cloud, output_cloud);

    return 0;
}

怎么使用？（书中没有提及，但项目内的README.md有说明）
——————————————————————————————————————————————————
这里演示一下！

cd到项目的build路径：
eg：cd ~/book_code/chapter_3/NDT_PCL_demo/build/
在build路径下输入以下命令：
cmake …

编译信息很多，最后会显示如下，表示编译成功：
在build路径下，接着输入命令：
make

4.在build路径下，执行编译结果，可视化点云图：
./normal_distributions_transform …/cloud1.pcd …/cloud2.pcd

此时会弹出一个新的窗口，显示如下：

这个看不出什么，把它缩小一点就有感觉了。
在此窗口内，滚动鼠标滑轮即可实现缩放查看全局，按住滑动可实现平移拖动，如下所示：

OK，演示结束！其它请结合书中内容学习。

【自动驾驶】《无人驾驶原理与实践》源码教程（二）——NDT算法实例

悦读