沉默杀手HomeArchivesTagsCategories
PCL中的NDT
|PCL点云|
Word count: 1.4k|Reading time: 6 min
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/common/transforms.h>
#include <pcl/console/time.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>      //NDT(正态分布)配准类头文件
#include <pcl/filters/approximate_voxel_grid.h>   //滤波类头文件  （使用体素网格过滤器处理的效果比较好）
#include <pcl/visualization/pcl_visualizer.h>
#include <iostream>
#include <boost/thread/thread.hpp>

typedef pcl::PointXYZ PointType;
typedef pcl::PointCloud<PointType> PointCloud;
typedef pcl::PointCloud<PointType>::Ptr PointCloudPtr;
typedef pcl::PointCloud<PointType>::ConstPtr PointCloudConstPtr;

int main (int argc, char** argv)
{
    ros::init(argc, argv, "test_ndt");
    ros::NodeHandle nh;

    PointCloudPtr target_cloud(new PointCloud);
    if(pcl::io::loadPCDFile<PointType>("/home/zzp/test_nodes/src/test_pcl/room_scan1.pcd", *target_cloud) == -1)
    {
        ROS_ERROR("load pcd file error !");
        return -1;
    }
    cout << "load target pcl " << target_cloud->size() << " points" << endl; 

    PointCloudPtr input_cloud(new PointCloud);
    if(pcl::io::loadPCDFile<PointType>("/home/zzp/test_nodes/src/test_pcl/room_scan2.pcd", *input_cloud) == -1)
    {
        ROS_ERROR("load pcd file error !");
        return -1;
    }
    cout << "load target pcl " << input_cloud->size() << " points" << endl;

    PointCloudPtr filtered_cloud(new PointCloud);
    pcl::VoxelGrid<PointType> filter;
    filter.setLeafSize(0.1, 0.1, 0.1);
    filter.setInputCloud(input_cloud);
    filter.filter(*filtered_cloud);
    cout << "after filtered, input cloud has "<< filtered_cloud->size() << " points" << endl;
    
    pcl::NormalDistributionsTransform<PointType, PointType>::Ptr ndt_ptr(new pcl::NormalDistributionsTransform<PointType, PointType>() );
    ndt_ptr->setMaximumIterations(35);
    ndt_ptr->setTransformationEpsilon(0.01);
    ndt_ptr->setStepSize(0.1);
    ndt_ptr->setResolution(0.1);
    ndt_ptr->setInputSource(filtered_cloud);
    ndt_ptr->setInputTarget(target_cloud);

    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 ();

    PointCloudPtr output_cloud(new PointCloud);
    ndt_ptr->align(*output_cloud, init_guess);
    cout << "output cloud points size: "<< output_cloud->size() << endl;
    cout << "ndt transform: "<< endl << ndt_ptr->getFinalTransformation() << endl;
    cout << "score: "<< ndt_ptr->getFitnessScore() << endl;
    
    pcl::transformPointCloud(*input_cloud, *output_cloud, ndt_ptr->getFinalTransformation() );
    pcl::io::savePCDFileASCII("/home/zzp/test_nodes/src/test_pcl/final.pcd", *output_cloud );

    // 初始化点云可视化对象
    boost::shared_ptr<pcl::visualization::PCLVisualizer>
    viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer_final->setBackgroundColor (0, 0, 0);  //设置背景颜色为黑色

    // 对目标点云着色可视化 (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");

    // 对转换后的源点云着色 (green)可视化.
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
    output_color(output_cloud, 0, 0, 255);
    viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
    viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                    1, "output cloud");
    // 启动可视化
    viewer_final->addCoordinateSystem (1.0);  //显示XYZ指示轴
    viewer_final->initCameraParameters ();   //初始化摄像头参数

    // 等待直到可视化窗口关闭
    while (!viewer_final->wasStopped ())
    {
      viewer_final->spinOnce (101);
      boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }
}

三个迭代终止条件设置：

  1. setMaximumIterations(30): 设置最大的迭代次数。 一般来说在这个限制值之前，优化程序会在epsilon（最大容差）变换阀值下终止，添加最大迭代次数限制能够增加程序的鲁棒性阻止了它在错误的方向上运行时间过长
  2. setTransformationEpsilon: 终止条件设置最小转换差异，设置前后两次迭代的转换矩阵的最大容差epsilion，在变换中Epsilon参数分别从长度和弧度，定义了变换矢量[x, y, z, roll, pitch, yaw]的最小许可的递增量，一旦两次迭代小于这个最大容差，则认为已经收敛到最优解，默认值为：0
  3. setEuclideanFitnessEpsilon: 设置前后两次迭代的点对的欧式距离均值的最大容差，迭代终止条件之三，默认值为：-std::numeric_limits::max

把setTransformationEpsilon的参数从0.01改为0.001，FitnessScore 只有细微变化，而且相对之前有时变大有时变小。但是改为0.1，FitnessScore与0.01时相比，也是有大有小。迭代次数不管怎么调整，FitnessScore毫无变化，可见确实是以epsilon为优先终止条件。

  • setStepSize(0.1): More-Thuente线搜索设置最大步长，即设置牛顿法优化的最大步长。当你靠近最优解时该算法会缩短迭代步长，在最大步长将会在较少的迭代次数下遍历较大的距离，但是却有过度迭代和在不符合要求的局部最小值处结束的风险。 调试的经验 ：step的话可以先放大后变小。

  • setResolution(1.0);: 设置网格化时立方体的边长，网格大小设置在NDT中非常重要，太大会导致精度不高，太小导致内存过高，并且只有两幅点云相差不大的情况才能匹配。它需要足够大，每个体素包含与点有关的统计数据，平均值，协方差等。每个体素至少包含6个点，但小到足以唯一地描述环境。 调试的经验 ：resolution在追求精度的情况下最好与点密度相等或者低一个数量级,这里是激光点云，所以设了1米

  • getFitnessScore()：欧几里得适合度得分 FitnessScore ，该分数计算为从输出云到目标云中最近点的距离的平方。分数越大，准确率越低，配准效果也就越差。

align函数中，output_cloud保存变换后的input_cloud，它和target_cloud其实很相近了。但是它不能作为最终的源点云变换，因为上面对input_cloud进行了滤波处理，所以又经过pcl::transformPointCloud(*input_cloud, *output_cloud, ndt_ptr->getFinalTransformation() );获得未经滤波的output_cloud

注意的地方：

  • 开始做体素滤波，栅格太大，过滤太多，最后的output_cloud和target_cloud看着不太契合
  • 只对源点云进行滤波，减少其数据量，而目标点云不需要滤波处理


参考: 参考程序