开源包链接：
git https://github.com/ccny-ros-pkg/scan_tools.git
１．简介
所述laser_scan_matcher包是增量激光扫描配准工具。该软件包允许扫描连续的sensor_msgs / LaserScan消息之间的匹配，并将估计的激光位置发布为geometry_msgs / Pose2D或tf变换。
该包装可以在没有其他传感器提供的任何测距估计的情况下使用。因此，它可以作为独立的里程计算器估算器。或者，您可以提供多种类型的测距输入，以提高注册速度和准确性。
２．输入
支持的消息类型
所述laser_scan_matcher可以操作使用sensor_msgs /激光扫描消息或sensor_msgs / PointCloud2消息。使用sensor_msgs / PointCloud2时，请确保它们没有nan值。
扫描预测
虽然laser_scan_matcher可以通过仅使用扫描数据来操作，但我们可以通过在每次新的扫描消息到达时提供对传感器的当前位置的猜测来加速扫描注册过程。当没有猜测时，合理的（并且广泛使用的）假设是传感器不移动（零速度模型）。以下是laser_scan_matcher接受的输入列表：
IMU：以sensor_msgs / IMU消息的形式估计机器人的方位角（Δ-θ）的变化。我们假设IMU消息的偏航分量对应于机器人的方向。因此，我们并不需要一个完整的6DoF IMU传感器 - 只要其输出被打包为IMU消息，便宜的单轴陀螺仪也可以工作。所需主题是imu / data。
车轮测距法：从车轮编码器等测距传感器估算机器人的x，y和方向角。所需的主题是odom。
恒速模型：假设机器人根据机器人速度的估计值移动。可以从外部传感器获得速度估计，或者通过导出和过滤扫描匹配器本身的输出来获得速度估计。所需主题是vel。有关如何使用简单过滤器实现此目的的示例，请查看Alpha-beta跟踪以了解扫描匹配预测。
零速度模型：不要使用任何预测，即假设机器人停留在同一个地方。
我们可以使用上述的组合，例如IMU以及车轮里程计或IMU以及alpha beta跟踪。当启用多个预测模式时，优先级为IMU> Odometry> Constant Velocity> Zero Velocity。
我应该在实践中使用什么？
IMU和（在某种程度上）车轮里程计输入显着提高了旋转运动的收敛速度。因此强烈建议添加IMU输入。
当扫描匹配器的性能稳定时，Alpha-beta跟踪可以显着提高速度，但可能会导致高动态环境或功能较差的环境出现奇怪的行为。我们建议启用它并根据经验确定它是否对您的环境有用。
关键帧与帧到帧扫描匹配
在经典的帧到帧激光测距仪中，每个激光扫描与先前的扫描进行比较。随着时间的推移聚合两者之间的变换以计算机器人在固定框架中的位置。
扫描中的一些噪音是不可避免的。因此，即使对于静止不动的机器人，增量变换也可能是非零的。这可能导致机器人姿势的缓慢漂移。
为了缓解这种情况，我们实现了基于关键帧的匹配。在当前激光扫描和“关键帧”扫描之间计算姿势的变化。在机器人移动一定距离后更新关键帧扫描。因此，如果机器人静止不动，关键帧扫描将不会改变，并且姿势将保持更加漂移。
设置更新关键帧的容差可以通过kf_dist_linear和kf_dist_angular参数来实现。它们的默认值在静止和移动时提供更强大的性能。
为了将scan_matching模式改回到经典的帧到帧，用户可以简单地将两个阈值中的任何一个设置为零。
３．参数
3.1 坐标系
~fixed_frame (string, default: "world") 世界坐标系
~base_frame (string, default: "base_link") 机器人本体坐标系
3.2运动预测
~use_imu (bool, default: true) 是否使用imu进行扫描注册的θ预测。需要输入/ imu / data话题。
~use_odom (bool, default: true) 是否使用车轮测距法进行扫描配准的x，y和θ预测。需要输入odom话题。
~use_vel (bool, default: false) 是否使用恒定速度模型进行扫描配准的x，y和θ预测。需要输入vel话题。
3.3 点云输入
使用sensor_msgs / PointCloud2而不是sensor_msgs / LaserScan消息时的参数。
~use_cloud_input (bool, default: false)
是否订阅/云话题sensor_msgs / PointCloud2的消息，而不是/扫描与话题sensor_msgs /激光扫描消息。
~cloud_range_min (double, default: 0.1)
传感器的最小范围，如果使用sensor_msgs / PointCloud2消息。如果使用sensor_msgs / LaserScan消息则不需要。
~cloud_range_max (double, default: 50.0)
传感器的最大范围，如果使用sensor_msgs / PointCloud2消息。如果使用sensor_msgs / LaserScan消息则不需要。
3.4关键帧
用于设置基于关键帧扫描的注册的参数。使用默认值，当传感器移动10厘米或10度时，关键帧会更新。将这些中的任何一个设置为零将减少到帧到帧扫描匹配。将它们保持在默认水平应该可以减少机器人静止时的漂移。
~kf_dist_linear (double, default: 0.10) 在更新关键帧扫描之前固定帧需要移动的距离（以米为单位）
~kf_dist_angular (double, default: 0.175) 在更新关键帧扫描之前固定帧需要移动的角度（以弧度表示）
3.5输出
~publish_tf (bool, default: true) 是否发布扫描匹配器对世界帧中基帧位置的估计作为变换。
~publish_pose (bool, default: true) 是否发布扫描匹配器对世界框架中基础框架位置的估计作为geometry_msgs / Pose2D
~publish_pose_stamped (bool, default: false) 是否发布扫描匹配器对世界框架中基础框架位置的估计作为geometry_msgs / PoseStamped
3.6扫描匹配
~max_iterations (int, default: 10) 最大ICP周期迭代
~max_correspondence_dist (double, default: 0.3) 通信有效的最大距离
~max_angular_correction_deg (double, default: 45.0) 扫描之间的最大角位移，以度为单位
~max_linear_correction (double, default: 0.50) 扫描之间的最大位移（m）
~epsilon_xy (double, default: 0.000001) 停止阈值（m）
~epsilon_theta (double, default: 0.000001) 停止阈值（rad）
~outliers_maxPerc (double, default: 0.90) 要考虑的对应关系的百分比：如果为0.90，则总是丢弃前10％的对应关系，错误更多
3.7扫描匹配（高级）
~sigma (double, default: 0.010) 扫描中的噪声（m）（不确定在当前实现中是否有任何影响）
~use_corr_tricks (int, default: 1) 如果为1，请使用智能技巧查找对应关系（参见论文）。
~restart (int, default: 0) 重新启动：如果为1，则在错误超过阈值时重新启动
~restart_threshold_mean_error (double, default: 0.01) 重新启动：重新启动的阈值
~restart_dt (double, default: 1.0) 重新启动：置换以重新启动。（M）
~restart_dtheta (double, default: 0.1) 重新启动：置换以重新启动。（弧度）
~clustering_threshold (double, default: 0.25) 保持相同群集的最大距离
~orientation_neighbourhood (int, default: 10) 用于估计方向的相邻光线的数量
~use_point_to_line_distance (int, default: 1) 如果为0，则为普通ICP
~do_alpha_test (int, default: 0) 如果为1，则根据角度丢弃对应关系
~do_alpha_test_thresholdDeg (double, default: 20.0) 丢弃基于角度的对应关系 - 阈值角度，以度为单位
~outliers_adaptive_order (double, default: 0.7)
描述用于丢弃的简单自适应算法的参数。1）订购错误。2）根据outliers_adaptive_order选择百分位数（如果为0.7，则获得70％的百分位数）。3）定义自适应阈值乘以outliers_adaptive_mult与所选百分位数的误差值。4）放弃超过阈值的对应关系。这对于保守是有用的; 但删除最大的错误。
~outliers_adaptive_mul (double, default: 2.0)
描述用于丢弃的简单自适应算法的参数。1）订购错误。2）根据outliers_adaptive_order选择百分位数（如果为0.7，则获得70％的百分位数）。3）定义自适应阈值乘以outliers_adaptive_mult与所选百分位数的误差值。4）放弃超过阈值的对应关系。这对于保守是有用的; 但删除最大的错误。
~do_visibility_test (int, default: 0)
如果您已经猜到了解决方案，则可以计算新位置中一次扫描点的极角。如果极角不是读数指数的单调函数，则意味着表面在下一个位置不可见。如果它不可见，那么我们不会将它用于匹配。
~outliers_remove_doubles (int, default: 1)
如果为1，则laser_sens中没有两个点可以具有相同的对应关系。
~do_compute_covariance (int, default: 0)
如果为1，则使用方法http://purl.org/censi/2006/icpcov计算ICP的协方差（更改此操作对当前实现没有影响）
~debug_verify_trick (int, default: 0)
如果为1，检查find_correspondences_tricks给出正确的答案
~use_ml_weights (int, default: 0)
如果为1，则使用第一次扫描中的字段“true_alpha”（或“alpha”）来计算发生率β，并使用因子（1 / cos ^ 2（beta））来加权对应关系。（更改此操作对当前实现没有影响）
~use_sigma_weights (int, default: 0)
如果为1，则第二次扫描中的字段'readings_sigma'用于将对应关系加权1 / sigma ^ 2（不确定是否在当前实现中对此进行更改有效）
参考文献
[1] ROS laser_scan_matcher http://wiki.ros.org/laser_scan_matcher