1. 建图的启动文件hdl_graph_slam_imu.launch
根据GitHub上的hdl_graph_slam/launch/hdl_graph_slam_imu.launch
文件修改而来,添加了imu消息(存疑,不知道有没有起作用,后期需要排查)。
修改后,自定义文件hdl_graph_slam_sx.launch
的路径如下:hdl_graph_slam/launch/hdl_graph_slam_sx.launch
。
具体代码如下,已经在代码中详细注释了参数内容:
<?xml version="1.0"?>
<launch>
<!--是否使用录制的数据进行仿真,通常默认为false-->
<param name="/use_sim_time" value="false" />
<!-- arguments -->
<arg name="nodelet_manager" default="velodyne_nodelet_manager" /> <!--节点管理器的参数名-->
<arg name="enable_floor_detection" default="true" /> <!--是否使用地面检测-->
<arg name="enable_gps" default="false" /> <!--是否使用GPS数据-->
<arg name="enable_imu_acc" default="true" /> <!--是否使用imu的加速度数据-->
<arg name="enable_imu_ori" default="true" /> <!--是否使用imu的角速度数据-->
<arg name="points_topic" default="/iexpress/iexpress_rs_driver2/rslidar_points" /> <!--激光雷达驱动的节点,传递点云格式的激光雷达数据-->
<arg name="imu_topic" default="/iexpress/iexpress_imu_driver/imuyesence/imu" /> <!--imu驱动的节点,传递imu数据-->
<!-- TF变换,激光雷达相对于车子中心坐标系的相对位移 -->
<!--传入参数顺序为 x y z yaw pitch roll frame_id child_frame_id period_in_ms(发布频率)-->
<node pkg="tf" type="static_transform_publisher" name="lidar2base_publisher" args="-1.925 1.1 0.0 2.25 0 0 base_link i_scan2 10" />
<!-- TF变换,imu传感器相对于车子中心坐标系的相对位移 -->
<node pkg="tf" type="static_transform_publisher" name="imu2base_publisher" args="1.86 0 -0.35 0 0 0 base_link imu 10" />
<!-- in case you use velodyne_driver, comment out the following line -->
<!-- 添加节点 velodyne_nodelet_manager -->
<node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen"/>
<!-- prefiltering_nodelet 点云过滤-->
<!-- 添加节点 prefiltering_nodelet ,加载节点管理器 velodyne_nodelet_manager的参数 -->
<node pkg="nodelet" type="nodelet" name="prefiltering_nodelet" args="load hdl_graph_slam/PrefilteringNodelet $(arg nodelet_manager)">
<!--接口名称重映射-->
<remap from="/velodyne_points" to="$(arg points_topic)" /> <!--不修改源码,将点云数据的接口名称重映射为当前雷达数据的消息名称-->
<remap from="/imu/data" to="$(arg imu_topic)" /> <!--不修改源码,将imu数据的接口名称重映射为当前imu数据的消息名称-->
<param name="deskewing" value="true" /> <!--是否进行偏移校准-->
<param name="scan_period" value="0.1" /> <!--雷达的扫描频率-->
<!-- in case base_link_frame is blank, mapping will be performed in the lidar frame -->
<!--设置base_link坐标系,以小车车体中心为圆心-->
<param name="base_link_frame" value="base_link" />
<!-- distance filter 距离滤波-->
<param name="use_distance_filter" value="true" /> <!--是否使用距离滤波-->
<param name="distance_near_thresh" value="0.1" /> <!--使用大于该距离范围的点云-->
<param name="distance_far_thresh" value="100.0" /> <!--使用小于该距离范围的点云-->
<!-- NONE, VOXELGRID, or APPROX_VOXELGRID 下采样参数 -->
<param name="downsample_method" value="VOXELGRID" /> <!--采取的下采样方法-->
<param name="downsample_resolution" value="0.25" /> <!--下采用分辨率-->
<!-- NONE, RADIUS, or STATISTICAL 外点过滤参数-->
<param name="outlier_removal_method" value="RADIUS" /> <!--使用的外点过滤方法-->
<param name="statistical_mean_k" value="30" /> <!--周围的neighbor的最小数目-->
<param name="statistical_stddev" value="1.2" /> <!--判断是否为离群点的阈值-->
<param name="radius_radius" value="0.5" /> <!--半径,以半径长度画圆的方式过滤外点-->
<param name="radius_min_neighbors" value="2" /> <!--检测到的最少neighbors的数量阈值-->
</node>
<!-- scan_matching_odometry_nodelet 点云匹配,即前端里程计参数-->
<!-- 添加节点 scan_matching_odometry_nodelet ,加载节点管理器 velodyne_nodelet_manager的参数 -->
<node pkg="nodelet" type="nodelet" name="scan_matching_odometry_nodelet" args="load hdl_graph_slam/ScanMatchingOdometryNodelet $(arg nodelet_manager)">
<param name="points_topic" value="$(arg points_topic)" /> <!--设置点云消息的来源-->
<param name="odom_frame_id" value="odom" /> <!--设置里程计坐标系名称-->
<param name="keyframe_delta_trans" value="0.5" /> <!--两个关键帧进行匹配的最小距离间隔,如果设置的太小,就变成前后帧进行匹配-->
<param name="keyframe_delta_angle" value="0.5" /> <!--两个关键帧进行匹配的最小角度间隔-->
<param name="keyframe_delta_time" value="10000.0" /> <!--两个关键帧进行匹配的最小时间间隔-->
<param name="transform_thresholding" value="false" /> <!--是否变换阈值,默认为false-->
<param name="max_acceptable_trans" value="2.0" /> <!--两个关键帧匹配后的最大相对位移。如果两个关键帧匹配后的相对位移太大,就丢弃当前帧,而采用上一帧的变换-->
<param name="max_acceptable_angle" value="2.0" /> <!--两个关键帧匹配后的最大相对角度。如果两个关键帧匹配后的相对角度太大,就丢弃当前帧,而采用上一帧的变换-->
<param name="downsample_method" value="NONE" /> <!--下采样方法:(VOXELGRID, APPROX_VOXELGRID, NONE)-->
<param name="downsample_resolution" value="0.1" /> <!--下采样率-->
<!-- ICP, GICP, NDT, GICP_OMP, or NDT_OMP(recommended) 可选的关键帧匹配方法 -->
<param name="registration_method" value="NDT_OMP" /> <!--设置关键帧匹配方法,推荐为NDT_OMP-->
<param name="transformation_epsilon" value="0.01"/> <!--即设置变换的ϵ(两个连续变换之间允许的最大差值)-->
<param name="maximum_iterations" value="64"/> <!--最大迭代次数-->
<param name="use_reciprocal_correspondences" value="false"/><!--是否使用双向搜索方法,默认为false-->
<param name="gicp_correspondence_randomness" value="20"/> <!--设置gicp方法的相对随机范围-->
<param name="gicp_max_optimizer_iterations" value="20"/> <!--设置gicp方法优化的最多迭代次数-->
<param name="ndt_resolution" value="1.0" /> <!--设置ndt方法的分辨率-->
<param name="ndt_num_threads" value="0" /> <!--设置ndt方法的线程数,可以多线程运行,提高速度-->
<param name="ndt_nn_search_method" value="DIRECT7" /> <!--设置ndt方法的查找方法,DIRECT7比kdtree快-->
</node>
<!-- floor_detection_nodelet 地面检测 -->
<!-- 如果使用地面检测, 添加节点 floor_detection_nodelet ,加载节点管理器 velodyne_nodelet_manager的参数 -->
<node pkg="nodelet" type="nodelet" name="floor_detection_nodelet" args="load hdl_graph_slam/FloorDetectionNodelet $(arg nodelet_manager)" if="$(arg enable_floor_detection)">
<param name="points_topic" value="$(arg points_topic)" /> <!--设置点云消息的来源-->
<param name="tilt_deg" value="0.0" /> <!--设置传感器,即雷达的倾角-->
<param name="sensor_height" value="0.61" /> <!--设置传感器,即雷达距离地面的高度-->
<!--设置地面检测的范围,即【sensor_height - height_clip_range,sensor_height + height_clip_range】-->
<param name="height_clip_range" value="1.0" />
<param name="floor_pts_thresh" value="256" /> <!--地面检测得到的最少点云数量的阈值-->
<param name="use_normal_filtering" value="true" /> <!--归一化过滤,默认为true,不在垂线上的点云在进行RANSAC之前,就会被过滤-->
<param name="normal_filter_thresh" value="10.0" /> <!--归一化过滤的阈值,垂线的长度-->
</node>
<!-- hdl_graph_slam_nodelet 后端概率图构建-->
<!-- 添加节点 hdl_graph_slam_nodelet ,加载节点管理器 velodyne_nodelet_manager的参数 -->
<node pkg="nodelet" type="nodelet" name="hdl_graph_slam_nodelet" args="load hdl_graph_slam/HdlGraphSlamNodelet $(arg nodelet_manager)">
<param name="points_topic" value="$(arg points_topic)" /> <!--设置点云消息的来源-->
<param name="imu_topic" value="$(arg imu_topic)" /> <!--设置imu消息的来源-->
<!--以上添加imu消息的方式存疑,也许imu并没有利用到后端建图中,之后查看源码时,需要解决-->
<!--以下注释掉的接口重映射方式应该也能使用,但建图运行中出现问题,以后查看源码排查问题来源-->
<!--
<remap from="/velodyne_points" to="$(arg points_topic)" />
<remap from="/gpsimu_driver/imu_data" to="$(arg imu_topic)" />
-->
<!-- frame settings 坐标系参数-->
<param name="map_frame_id" value="map" /> <!--设置地图坐标系名称-->
<param name="odom_frame_id" value="odom" /> <!--设置里程计坐标系名称-->
<!-- optimization params 后端优化方法参数-->
<!-- typical solvers: gn_var, gn_fix6_3, gn_var_cholmod, lm_var, lm_fix6_3, lm_var_cholmod, ... -->
<param name="g2o_solver_type" value="lm_var_cholmod" /> <!--设置g2o_solver的类型-->
<param name="g2o_solver_num_iterations" value="512" /> <!--设置g2o_solver的迭代次数-->
<!-- constraint switches 多传感器约束开关-->
<param name="enable_gps" value="$(arg enable_gps)" /> <!--是否使用GPS数据-->
<param name="enable_imu_acceleration" value="$(arg enable_imu_acc)" /> <!--是否使用imu的加速度数据-->
<param name="enable_imu_orientation" value="$(arg enable_imu_ori)" /> <!--是否使用imu的角速度数据-->
<!-- keyframe registration params 关键帧更新参数 -->
<param name="max_keyframes_per_update" value="10" /> <!--每秒最多添加的关键帧数量-->
<param name="keyframe_delta_trans" value="1.0" /> <!--两个关键帧进行匹配的最小距离间隔,如果设置的太小,就变成前后帧进行匹配-->
<param name="keyframe_delta_angle" value="1.0" /> <!--两个关键帧进行匹配的最小角度间隔-->
<!-- fix first node for optimization stability 为了优化稳定性,设置第一个点云参数 -->
<param name="fix_first_node" value="true"/> <!--是否设置第一个点云,默认为true-->
<param name="fix_first_node_stddev" value="10 10 10 1 1 1"/> <!--设置第一个点云的各项参数-->
<param name="fix_first_node_adaptive" value="true"/> <!--设置第一个点云是否是可适应的-->
<!-- loop closure params 闭环优化参数-->
<param name="distance_thresh" value="5" /> <!--小于该阈值范围内两个关键帧为潜在闭环帧,将所有满足条件的都存起来作为candidate-->
<!--以下两个参数的含义存疑,等阅读源码后再综合理解-->
<param name="accum_distance_thresh" value="10" /> <!-- 这里指的是当前帧与临近帧的累计距离阈值(最大距离阈值),假设累计距离阈值为10m, 那么当前帧距离10米范围外的关键帧不再考虑-->
<param name="min_edge_interval" value="5" /> <!-- 这里指的是当前帧与临近帧的最小边间隔(最小距离阈值),假设距离阈值为5m, 那么当前帧距离5米范围内的关键帧不再考虑-->
<param name="fitness_score_thresh" value="1.5" /> <!-- 闭环帧匹配得分-->
<!-- scan matching params 点云匹配参数-->
<param name="registration_method" value="NDT_OMP" /> <!-- 设置点云匹配的方法-->
<param name="transformation_epsilon" value="0.01"/> <!--即设置变换的ϵ(两个连续变换之间允许的最大差值)-->
<param name="maximum_iterations" value="64"/> <!--点云匹配的最大迭代次数-->
<param name="use_reciprocal_correspondences" value="false"/> <!--是否使用双向搜索方法,默认为false-->
<param name="gicp_correspondence_randomness" value="20"/> <!--设置gicp方法的相对随机范围-->
<param name="gicp_max_optimizer_iterations" value="20"/> <!--设置gicp方法优化的最多迭代次数-->
<param name="ndt_resolution" value="1.0" /> <!--设置ndt方法的分辨率-->
<param name="ndt_num_threads" value="0" /> <!--设置ndt方法的线程数,可以多线程运行,提高速度-->
<param name="ndt_nn_search_method" value="DIRECT7" /> <!--设置ndt方法的查找方法,DIRECT7比kdtree快-->
<!-- edge params 概率图边相关参数-->
<!-- GPS 参数-->
<param name="gps_edge_robust_kernel" value="NONE" /> <!-- 设置gps边的鲁棒核的值-->
<param name="gps_edge_robust_kernel_size" value="1.0" /> <!-- 设置gps边的鲁棒核的尺寸-->
<param name="gps_edge_stddev_xy" value="20.0" /> <!-- 设置gps边的鲁棒核的xy轴的值-->
<param name="gps_edge_stddev_z" value="5.0" /> <!-- 设置gps边的鲁棒核的z轴的值-->
<!-- IMU orientation imu的加速度参数-->
<param name="imu_orientation_edge_robust_kernel" value="NONE" />
<param name="imu_orientation_edge_stddev" value="1" />
<!-- IMU acceleration (gravity vector) imu的重力加速度参数 -->
<param name="imu_acceleration_edge_robust_kernel" value="NONE" />
<param name="imu_acceleration_edge_stddev" value="1" />
<!-- ground plane 地面平面参数-->
<param name="floor_edge_robust_kernel" value="NONE" />
<param name="floor_edge_stddev" value="0.1" />
<!-- scan matching 前端匹配参数-->
<!-- robust kernels: NONE, Cauchy, DCS, Fair, GemanMcClure, Huber, PseudoHuber, Saturated, Tukey, Welsch -->
<param name="odometry_edge_robust_kernel" value="NONE" /> <!-- 设置里程计边的鲁棒核类型-->
<param name="odometry_edge_robust_kernel_size" value="1.0" />
<param name="loop_closure_edge_robust_kernel" value="Huber" /> <!-- 设置闭环边的鲁棒核类型-->
<param name="loop_closure_edge_robust_kernel_size" value="1.0" />
<!-- information matrix params 信息矩阵参数-->
<param name="use_const_inf_matrix" value="false" /> <!-- 对于闭环帧匹配,是否使用常值信息矩阵-->
<param name="const_stddev_x" value="0.5" /> <!--设置XYZ常值信息矩阵-->
<param name="const_stddev_q" value="0.1" /> <!--设置角度常值信息矩阵-->
<param name="var_gain_a" value="20.0" /> <!--设置动态信息矩阵增益-->
<param name="min_stddev_x" value="0.1" /> <!--设置动态信息矩阵xyz最小值-->
<param name="max_stddev_x" value="5.0" /> <!--设置动态信息矩阵xyz最大值-->
<param name="min_stddev_q" value="0.05" /> <!--设置动态信息矩阵角度最小值-->
<param name="max_stddev_q" value="0.2" /> <!--设置动态信息矩阵角度最大值-->
<!-- update params 图优化参数-->
<param name="graph_update_interval" value="3.0" /> <!-- 局部优化的更新频率-->
<param name="map_cloud_update_interval" value="10.0" /> <!--全局地图的更新频率-->
<param name="map_cloud_resolution" value="0.05" /> <!--全局地图的分辨率-->
</node>
<!--自动打开rviz-->
<node pkg="hdl_graph_slam" type="map2odom_publisher.py" name="map2odom_publisher" />
<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find hdl_graph_slam)/rviz/hdl_graph_slam.rviz" />
</launch>
2. 定位的启动文件hdl_localization_sx.launch
自定义文件,在项目示例的启动文件hdl_localization.launch
中添加了imu的内容。
文件路径:/hdl_localization/launch/hdl_localization_sx.launch
具体代码如下,已经在代码中详细注释了参数内容:
<?xml version="1.0"?>
<launch>
<!--是否使用录制的数据进行仿真,通常默认为false-->
<param name="/use_sim_time" value="false" />
<!-- TF变换,激光雷达相对于车子中心坐标系的相对位移 -->
<!--传入参数顺序为 x y z yaw pitch roll frame_id child_frame_id period_in_ms(发布频率)-->
<node pkg="tf" type="static_transform_publisher" name="lidar2base_publisher" args="-1.925 1.1 0.0 2.25 0 0 i_base_link i_scan2 10" />
<!-- TF变换,imu传感器相对于车子中心坐标系的相对位移 -->
<node pkg="tf" type="static_transform_publisher" name="imu2base_publisher" args="1.86 0 -0.35 0 0 0 i_base_link i_imu 10" />
<!-- arguments -->
<arg name="nodelet_manager" default="velodyne_nodelet_manager" /> <!--节点管理器的参数名称-->
<arg name="points_topic" default="/iexpress/iexpress_rs_driver2/rslidar_points" /> <!--点云数据的消息名称-->
<arg name="imu_topic" default="/iexpress/iexpress_imu_driver/imuyesence/imu" /> <!--imu数据的消息名称-->
<arg name="odom_child_frame_id" default="i_base_link" /> <!--里程计坐标系的名称-->
<!-- in case you use velodyne_driver, comment out the following line -->
<!-- 添加节点 velodyne_nodelet_manager -->
<node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen"/>
<!-- globalmap_server_nodelet 地图服务:加载地图 -->
<!-- 添加节点 globalmap_server_nodelet ,加载节点管理器 velodyne_nodelet_manager的参数 -->
<node pkg="nodelet" type="nodelet" name="globalmap_server_nodelet" args="load hdl_localization/GlobalmapServerNodelet $(arg nodelet_manager)">
<param name="globalmap_pcd" value="/home/lxr/sx_ws/map/sx.pcd" /> <!--本地地图的加载路径,该路径为绝对路径-->
<param name="downsample_resolution" value="0.1" /> <!--下采样率-->
</node>
<!-- hdl_localization_nodelet 定位 -->
<!-- 添加节点 hdl_localization_nodelet ,加载节点管理器 velodyne_nodelet_manager的参数 -->
<node pkg="nodelet" type="nodelet" name="hdl_localization_nodelet" args="load hdl_localization/HdlLocalizationNodelet $(arg nodelet_manager)">
<remap from="/velodyne_points" to="$(arg points_topic)" /> <!--不修改源码,将点云数据的接口名称重映射为当前雷达数据的消息名称-->
<remap from="/gpsimu_driver/imu_data" to="$(arg imu_topic)" /> <!--不修改源码,将imu数据的接口名称重映射为当前imu数据的消息名称-->
<!-- odometry frame_id 里程计坐标系参数-->
<param name="odom_child_frame_id" value="$(arg odom_child_frame_id)" /> <!--设置里程计坐标系名称-->
<!--<remap from="/gpsimu_driver/imu_data" to="$(arg odom_child_frame_id)" />-->
<!-- imu settings -->
<!-- during "cool_time", imu inputs are ignored -->
<param name="use_imu" value="true" /> <!--设置是否使用imu数据,默认为true-->
<param name="invert_imu" value="true" /> <!--设置是否逆置imu数据,默认为true-->
<param name="cool_time_duration" value="2.0" /> <!--设置cool_time持续时间-->
<!-- ndt settings -->
<!-- if NDT is slow for your PC, try DIRECT1 serach method, which is a bit unstable but extremely fast -->
<param name="ndt_neighbor_search_method" value="DIRECT7" /> <!--设置ndt的最近邻查找方法-->
<param name="ndt_resolution" value="1.0" /> <!--设置ndt方法的分辨率-->
<param name="downsample_resolution" value="0.1" /> <!--下采样率-->
<!-- if "specify_init_pose" is true, pose estimator will be initialized with the following params -->
<!-- otherwise, you need to input an initial pose with "2D Pose Estimate" on rviz" -->
<param name="specify_init_pose" value="true" /> <!--是否设定初始位姿,如果为false,需要在rviz上利用"2D Pose Estimate"输入初始位姿-->
<param name="init_pos_x" value="0.0" /> <!--初始位姿的x,y,z,w的四元数值-->
<param name="init_pos_y" value="0.0" />
<param name="init_pos_z" value="0.0" />
<param name="init_ori_w" value="1.0" /> <!--初始位姿的x,y,z轴的方向-->
<param name="init_ori_x" value="0.0" />
<param name="init_ori_y" value="0.0" />
<param name="init_ori_z" value="0.0" />
</node>
<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find hdl_localization)/rviz/hdl_localization.rviz" /> <!--全部参数加载完成后,打开rviz-->
</launch>