概述
从零开始搭二维激光SLAM --- 基于PL-ICP的帧间匹配
什么叫PL-ICP呢
PL-ICP 实际就是point-to-line ICP,是点到线的ICP方法.相比于传统的ICP方法,即点到点的匹配方法,PL-ICP在算法方面实际上是对误差形式进行了改进,其他方面基本相同,但迭代速度以及匹配精度
得到了很大的提升室内环境通常是结构化环境,即譬如墙壁等有众多规则的曲面,而激光数据实际是对实际环境中曲面的离散采样,因此,最好的误差尺度就是点到实际曲面的距离。我们目前通过传感器已经知晓激光点云数据,实际的重点就是对环境中曲面的恢复。PL-ICP采用的是用分段线性的方式近似替代曲面,从而获得激光点到实际曲面的距离。
pl-icp的代码命名为csm( Canonical Scan Matcher).
第一步安装CSM
sudo apt-get install ros-kinetic-csm
一般这个地方会出问题,解决办法有两个
deb http://packages.ros.org/ros/ubuntu xenial main
deb http://packages.ros.org/ros-shadow-fixed/ubuntu xenial main
如果不成功就换热点
使用csm时需要在CMake.list里添加
# Find csm project
find_package(PkgConfig)
pkg_check_modules(csm REQUIRED csm)
include_directories(
include
${catkin_INCLUDE_DIRS}
${csm_INCLUDE_DIRS}
)
link_directories(${csm_LIBRARY_DIRS})
#Create library
add_executable(${PROJECT_NAME}_scan_match_plicp_node src/scan_match_plicp.cc)
#Note we don't link against pcl as we're using header-only parts of the library
target_link_libraries( ${PROJECT_NAME}_scan_match_plicp_node
${catkin_LIBRARIES}
${csm_LIBRARIES}
)
add_dependencies(${PROJECT_NAME}_scan_match_plicp_node
${csm_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
launch文件的配置
<launch>
<!-- bag的地址与名称 -->
<arg name="bag_filename" default="/home/lx/bagfiles/lesson3.bag"/>
<!-- 使用bag的时间戳 -->
<param name="use_sim_time" value="true" />
<!-- base_link to front_laser_link -->
<node pkg="tf2_ros" type="static_transform_publisher" name="link_broadcaster"
args="0 0 0.254 0 0 3.1415926 base_link front_laser_link" />
<!-- 启动 plicp_odometry 节点 -->
<node name="lesson3_plicp_odometry_node"
pkg="lesson3" type="lesson3_plicp_odometry_node" output="screen" >
<rosparam file="$(find lesson3)/config/plicp_odometry.yaml" command="load"/>
</node>
<!-- launch rviz -->
<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find lesson3)/config/plicp_odometry.rviz" />
<!-- play bagfile -->
<node name="playbag" pkg="rosbag" type="play"
args="--clock $(arg bag_filename)" />
</launch>
源程序
#include "lesson3/plicp_odometry.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
ScanMatchPLICP::ScanMatchPLICP() : private_node_("~"), tf_listener_(tfBuffer_)
{
// 33[1;32m, 33[0m 终端显示成绿色
ROS_INFO_STREAM("