基于PCL，分别实现了point2point，point2plane，plane2plane三种形式的icp算法，输入原始轨迹，并完成原始轨迹到真实轨迹(groudTruth)的对齐。

mkdir build cd build cmake .. make ./[target algorithm]

CMakeLists.txt 目录CMake配置文件 icp_pp.cpp 主文件

在icp_pp.cpp文件中分别有四个函数：

PointToPlane 对应点到面形式ICP实现 PointToPoint 对应点到点形式ICP实现 icpPlaneToPlane 对应面到面形式ICP实现 icpPointToPlane 对应另一种点到面ICP实现。

1. 在yaml文件中导入tum格式的slam输出的轨迹:slamTrajectoryFile， 真值轨迹gtTrajectoryFile和输出轨迹outputTrajectoryFile
2. 设置icp方法icp_pattern：icpPlaneToPlane icpPointToPlane PointToPlane PointToPoint NDT
3. 设置初始参数init_params：初值越准，匹配的效果越好

note:

• 一次成功不了，多试几种icp方法
• 初值要给定尽量和gt近似，不能相差太远，否则也会无法对齐轨迹

note: 白色是原轨迹，蓝色是真值轨迹，绿色是ICP对齐后的轨迹

对齐前：

icp对齐后：