參考

• Cartographer 官方文檔
• Cartographer 從入門到精通

1. Cartographer 安裝

1.1 前置條件

• 推薦在剛裝好的 Ubuntu 16.04 或 Ubuntu 18.04 上進(jìn)行編譯
• ROS 安裝：ROS學(xué)習(xí)1：ROS概述與環(huán)境搭建

1.2 依賴庫(kù)安裝

• 資源下載完解壓并執(zhí)行以下指令
  • https://pan.baidu.com/s/1LWqZ4SOKn2sZecQUDDXXEw?pwd=j6cf

  $ sudo chmod 777 auto-carto-build.sh
  $ ./auto-carto-build.sh
  

1.3 編譯

本文只編譯 cartographer_ros，以下為同時(shí)開(kāi)三個(gè)終端操作

$ mkdir -p cartographer_ws/src

$ cd ~
$ git clone https://github.com/xiangli0608/cartographer_detailed_comments_ws
$ mv ~/cartographer_detailed_comments_ws/src/cartographer_ros ~/cartographer_ws/src

$ cd ~/cartographer_ws
$ catkin_make

2. Cartographer 運(yùn)行

2.1 數(shù)據(jù)集下載

• 百度網(wǎng)盤鏈接（rslidar-outdoor-gps.bag、landmarks_demo_uncalibrated.bag）
  • https://pan.baidu.com/s/1leRr4JDGg61jBNCwNlSCJw?pwd=5nkq

2.2 配置文件

• lx_rs16_2d_outdoor.launch

  <launch><!-- bag 的地址與名稱（根據(jù)自己情況修改，建議使用絕對(duì)路徑） --><arg name="bag_filename" default="/home/yue/bag/rslidar-outdoor-gps.bag"/><!-- 使用 bag 的時(shí)間戳 --><param name="/use_sim_time" value="true" /><!-- 啟動(dòng) cartographer --><node name="cartographer_node" pkg="cartographer_ros"type="cartographer_node" args="-configuration_directory $(find cartographer_ros)/configuration_files-configuration_basename lx_rs16_2d_outdoor.lua"output="screen"><remap from="points2" to="rslidar_points" /><remap from="scan" to="front_scan" /><remap from="odom" to="odom_scout" /><remap from="imu" to="imu" /></node><!-- 生成 ros 格式的地圖 --><node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"type="cartographer_occupancy_grid_node" args="-resolution 0.05" /><!-- 啟動(dòng) rviz --><node name="rviz" pkg="rviz" type="rviz" required="true"args="-d $(find cartographer_ros)/configuration_files/lx_2d.rviz" /><!-- 啟動(dòng) rosbag --><node name="playbag" pkg="rosbag" type="play"args="--clock $(arg bag_filename)" /></launch>
  

• lx_rs16_2d_outdoor.lua

  include "map_builder.lua"
  include "trajectory_builder.lua"options = {map_builder = MAP_BUILDER，               -- map_builder.lua的配置信息trajectory_builder = TRAJECTORY_BUILDER， -- trajectory_builder.lua的配置信息map_frame = "map"，                       -- 地圖坐標(biāo)系的名字tracking_frame = "imu_link"，             -- 將所有傳感器數(shù)據(jù)轉(zhuǎn)換到這個(gè)坐標(biāo)系下published_frame = "footprint"，           -- tf: map -> footprintodom_frame = "odom"，                     -- 里程計(jì)的坐標(biāo)系名字provide_odom_frame = false，              -- 是否提供odom的tf，如果為true,則tf樹(shù)為map->odom->footprint-- 如果為false tf樹(shù)為map->footprintpublish_frame_projected_to_2d = false，   -- 是否將坐標(biāo)系投影到平面上--use_pose_extrapolator = false，           -- 發(fā)布tf時(shí)是使用pose_extrapolator的位姿還是前端計(jì)算出來(lái)的位姿use_odometry = false，                    -- 是否使用里程計(jì),如果使用要求一定要有odom的tfuse_nav_sat = false，                     -- 是否使用gpsuse_landmarks = false，                   -- 是否使用landmarknum_laser_scans = 0，                     -- 是否使用單線激光數(shù)據(jù)num_multi_echo_laser_scans = 0，          -- 是否使用multi_echo_laser_scans數(shù)據(jù)num_subdivisions_per_laser_scan = 1，     -- 1幀數(shù)據(jù)被分成幾次處理,一般為1num_point_clouds = 1，                    -- 是否使用點(diǎn)云數(shù)據(jù)lookup_transform_timeout_sec = 0.2，      -- 查找tf時(shí)的超時(shí)時(shí)間submap_publish_period_sec = 0.3，         -- 發(fā)布數(shù)據(jù)的時(shí)間間隔pose_publish_period_sec = 5e-3,trajectory_publish_period_sec = 30e-3,rangefinder_sampling_ratio = 1.，         -- 傳感器數(shù)據(jù)的采樣頻率odometry_sampling_ratio = 1.,fixed_frame_pose_sampling_ratio = 1.,imu_sampling_ratio = 1.,landmarks_sampling_ratio = 1.,
  }MAP_BUILDER.use_trajectory_builder_2d = trueTRAJECTORY_BUILDER_2D.use_imu_data = true
  TRAJECTORY_BUILDER_2D.min_range = 0.3
  TRAJECTORY_BUILDER_2D.max_range = 100.
  TRAJECTORY_BUILDER_2D.min_z = 0.2
  --TRAJECTORY_BUILDER_2D.max_z = 1.4
  --TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.02--TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 0.5
  --TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 200.
  --TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_range = 50.--TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.max_length = 0.9
  --TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.min_num_points = 100
  --TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.max_range = 50.TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = false
  TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 1.
  TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 1.
  TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 1.
  --TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 12--TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.1
  --TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = 0.004
  --TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 1.TRAJECTORY_BUILDER_2D.submaps.num_range_data = 80.
  TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.1POSE_GRAPH.optimize_every_n_nodes = 160.
  POSE_GRAPH.constraint_builder.sampling_ratio = 0.3
  POSE_GRAPH.constraint_builder.max_constraint_distance = 15.
  POSE_GRAPH.constraint_builder.min_score = 0.48
  POSE_GRAPH.constraint_builder.global_localization_min_score = 0.60return options
  

• trajectory_builder_2d.lua

  TRAJECTORY_BUILDER_2D = {use_imu_data = true,            -- 是否使用imu數(shù)據(jù)min_range = 0.,                 -- 雷達(dá)數(shù)據(jù)的最遠(yuǎn)最近濾波, 保存中間值max_range = 30.,min_z = -0.8,                   -- 雷達(dá)數(shù)據(jù)的最高與最低的過(guò)濾, 保存中間值max_z = 2.,missing_data_ray_length = 5.,   -- 超過(guò)最大距離范圍的數(shù)據(jù)點(diǎn)用這個(gè)距離代替num_accumulated_range_data = 1, -- 幾幀有效的點(diǎn)云數(shù)據(jù)進(jìn)行一次掃描匹配voxel_filter_size = 0.025,      -- 體素濾波的立方體的邊長(zhǎng)-- 使用固定的voxel濾波之后, 再使用自適應(yīng)體素濾波器-- 體素濾波器 用于生成稀疏點(diǎn)云 以進(jìn)行 掃描匹配adaptive_voxel_filter = {max_length = 0.5,             -- 嘗試確定最佳的立方體邊長(zhǎng), 邊長(zhǎng)最大為0.5min_num_points = 200,         -- 如果存在 較多點(diǎn) 并且大于'min_num_points', 則減小體素長(zhǎng)度以嘗試獲得該最小點(diǎn)數(shù)max_range = 50.,              -- 距遠(yuǎn)離原點(diǎn)超過(guò)max_range 的點(diǎn)被移除},-- 閉環(huán)檢測(cè)的自適應(yīng)體素濾波器, 用于生成稀疏點(diǎn)云 以進(jìn)行 閉環(huán)檢測(cè)loop_closure_adaptive_voxel_filter = {max_length = 0.9,min_num_points = 100,max_range = 50.,},-- 是否使用 real_time_correlative_scan_matcher 為ceres提供先驗(yàn)信息-- 計(jì)算復(fù)雜度高 , 但是很魯棒 , 在odom或者imu不準(zhǔn)時(shí)依然能達(dá)到很好的效果use_online_correlative_scan_matching = false,real_time_correlative_scan_matcher = {linear_search_window = 0.1,             -- 線性搜索窗口的大小angular_search_window = math.rad(20.),  -- 角度搜索窗口的大小translation_delta_cost_weight = 1e-1,   -- 用于計(jì)算各部分score的權(quán)重rotation_delta_cost_weight = 1e-1,},-- ceres匹配的一些配置參數(shù)ceres_scan_matcher = {occupied_space_weight = 1.,translation_weight = 10.,rotation_weight = 40.,ceres_solver_options = {use_nonmonotonic_steps = false,max_num_iterations = 20,num_threads = 1,},},-- 為了防止子圖里插入太多數(shù)據(jù), 在插入子圖之前之前對(duì)數(shù)據(jù)進(jìn)行過(guò)濾motion_filter = {max_time_seconds = 5.,max_distance_meters = 0.2,max_angle_radians = math.rad(1.),},-- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS.imu_gravity_time_constant = 10.,-- 位姿預(yù)測(cè)器pose_extrapolator = {use_imu_based = false,constant_velocity = {imu_gravity_time_constant = 10.,pose_queue_duration = 0.001,},imu_based = {pose_queue_duration = 5.,gravity_constant = 9.806,pose_translation_weight = 1.,pose_rotation_weight = 1.,imu_acceleration_weight = 1.,imu_rotation_weight = 1.,odometry_translation_weight = 1.,odometry_rotation_weight = 1.,solver_options = {use_nonmonotonic_steps = false;max_num_iterations = 10;num_threads = 1;},},},-- 子圖相關(guān)的一些配置submaps = {num_range_data = 90,          -- 一個(gè)子圖里插入雷達(dá)數(shù)據(jù)的個(gè)數(shù)的一半grid_options_2d = {grid_type = "PROBABILITY_GRID", -- 地圖的種類, 還可以是tsdf格式resolution = 0.05,},range_data_inserter = {range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D",-- 概率占用柵格地圖的一些配置probability_grid_range_data_inserter = {insert_free_space = true,hit_probability = 0.55,miss_probability = 0.49,},-- tsdf地圖的一些配置tsdf_range_data_inserter = {truncation_distance = 0.3,maximum_weight = 10.,update_free_space = false,normal_estimation_options = {num_normal_samples = 4,sample_radius = 0.5,},project_sdf_distance_to_scan_normal = true,update_weight_range_exponent = 0,update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0.5,update_weight_distance_cell_to_hit_kernel_bandwidth = 0.5,},},},
  }
  

• pose_graph.lua

  POSE_GRAPH = {-- 每隔多少個(gè)節(jié)點(diǎn)執(zhí)行一次后端優(yōu)化optimize_every_n_nodes = 90,-- 約束構(gòu)建的相關(guān)參數(shù)constraint_builder = {sampling_ratio = 0.3,                 -- 對(duì)局部子圖進(jìn)行回環(huán)檢測(cè)時(shí)的計(jì)算頻率, 數(shù)值越大, 計(jì)算次數(shù)越多max_constraint_distance = 15.,        -- 對(duì)局部子圖進(jìn)行回環(huán)檢測(cè)時(shí)能成為約束的最大距離min_score = 0.55,                     -- 對(duì)局部子圖進(jìn)行回環(huán)檢測(cè)時(shí)的最低分?jǐn)?shù)閾值global_localization_min_score = 0.6,  -- 對(duì)整體子圖進(jìn)行回環(huán)檢測(cè)時(shí)的最低分?jǐn)?shù)閾值loop_closure_translation_weight = 1.1e4,loop_closure_rotation_weight = 1e5,log_matches = true,                   -- 打印約束計(jì)算的log-- 基于分支定界算法的2d粗匹配器fast_correlative_scan_matcher = {linear_search_window = 7.,angular_search_window = math.rad(30.),branch_and_bound_depth = 7,},-- 基于ceres的2d精匹配器ceres_scan_matcher = {occupied_space_weight = 20.,translation_weight = 10.,rotation_weight = 1.,ceres_solver_options = {use_nonmonotonic_steps = true,max_num_iterations = 10,num_threads = 1,},},-- 基于分支定界算法的3d粗匹配器fast_correlative_scan_matcher_3d = {branch_and_bound_depth = 8,full_resolution_depth = 3,min_rotational_score = 0.77,min_low_resolution_score = 0.55,linear_xy_search_window = 5.,linear_z_search_window = 1.,angular_search_window = math.rad(15.),},-- 基于ceres的3d精匹配器ceres_scan_matcher_3d = {occupied_space_weight_0 = 5.,occupied_space_weight_1 = 30.,translation_weight = 10.,rotation_weight = 1.,only_optimize_yaw = false,ceres_solver_options = {use_nonmonotonic_steps = false,max_num_iterations = 10,num_threads = 1,},},},matcher_translation_weight = 5e2,matcher_rotation_weight = 1.6e3,-- 優(yōu)化殘差方程的相關(guān)參數(shù)optimization_problem = {huber_scale = 1e1,                -- 值越大,（潛在）異常值的影響就越大acceleration_weight = 1.1e2,      -- 3d里imu的線加速度的權(quán)重rotation_weight = 1.6e4,          -- 3d里imu的旋轉(zhuǎn)的權(quán)重-- 前端結(jié)果殘差的權(quán)重local_slam_pose_translation_weight = 1e5,local_slam_pose_rotation_weight = 1e5,-- 里程計(jì)殘差的權(quán)重odometry_translation_weight = 1e5,odometry_rotation_weight = 1e5,-- gps殘差的權(quán)重fixed_frame_pose_translation_weight = 1e1,fixed_frame_pose_rotation_weight = 1e2,fixed_frame_pose_use_tolerant_loss = false,fixed_frame_pose_tolerant_loss_param_a = 1,fixed_frame_pose_tolerant_loss_param_b = 1,log_solver_summary = false,use_online_imu_extrinsics_in_3d = true,fix_z_in_3d = false,ceres_solver_options = {use_nonmonotonic_steps = false,max_num_iterations = 50,num_threads = 7,},},max_num_final_iterations = 200,   -- 在建圖結(jié)束之后執(zhí)行一次全局優(yōu)化, 不要求實(shí)時(shí)性, 迭代次數(shù)多global_sampling_ratio = 0.003,    -- 純定位時(shí)候查找回環(huán)的頻率log_residual_histograms = true,global_constraint_search_after_n_seconds = 10., -- 純定位時(shí)多少秒執(zhí)行一次全子圖的約束計(jì)算--  overlapping_submaps_trimmer_2d = {--    fresh_submaps_count = 1,--    min_covered_area = 2,--    min_added_submaps_count = 5,--  },
  }
  

2.3 運(yùn)行演示

$ source devel/setup.bash
$ roslaunch cartographer_ros lx_rs16_2d_outdoor.launch

3. Cartographer 調(diào)參總結(jié)

3.1 降低延遲與減小計(jì)算量

• 前端

  • 減小 max_range 即減小了需要處理的點(diǎn)數(shù)，在激光雷達(dá)遠(yuǎn)距離的數(shù)據(jù)點(diǎn)不準(zhǔn)時(shí)一定要減小這個(gè)值
  • 增大 voxel_filter_size，相當(dāng)于減小了需要處理的點(diǎn)數(shù)
  • 增大 submaps.resolution，相當(dāng)于減小了匹配時(shí)的搜索量
  • 對(duì)于自適應(yīng)體素濾波 減小 min_num_points 與 max_range，增大 max_length，相當(dāng)于減小了需要處理的點(diǎn)數(shù)

• 后端

  • 減小 optimize_every_n_nodes, 降低優(yōu)化頻率, 減小了計(jì)算量
  • 增大 MAP_BUILDER.num_background_threads, 增加計(jì)算速度
  • 減小 global_sampling_ratio, 減小計(jì)算全局約束的頻率
  • 減小 constraint_builder.sampling_ratio, 減少了約束的數(shù)量
  • 增大 constraint_builder.min_score, 減少了約束的數(shù)量
  • 減小分枝定界搜索窗的大小, 包括linear_xy_search_window,inear_z_search_window, angular_search_window
  • 增大 global_constraint_search_after_n_seconds, 減小計(jì)算全局約束的頻率
  • 減小 max_num_iterations, 減小迭代次數(shù)

3.2 降低內(nèi)存

• 增大子圖的分辨率 submaps.resolution

3.3 常調(diào)的參數(shù)

TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 100.
TRAJECTORY_BUILDER_2D.min_z = 0.2 -- / -0.8
TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.02
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 10.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 1.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 1.
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 80.
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.1 -- / 0.02
POSE_GRAPH.optimize_every_n_nodes = 160. -- 2倍的num_range_data以上
POSE_GRAPH.constraint_builder.sampling_ratio = 0.3
POSE_GRAPH.constraint_builder.max_constraint_distance = 15.
POSE_GRAPH.constraint_builder.min_score = 0.48
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.60

4. Cartographer 工程化建議

4.1 如何提升建圖質(zhì)量

• 選擇頻率高（25 Hz 以上）、精度高的激光雷達(dá)
• 如果只能用頻率低的激光雷達(dá)
  • 使用高頻、高精度 IMU，且讓機(jī)器人運(yùn)動(dòng)慢一點(diǎn)
  • 調(diào) ceres 的匹配權(quán)重，將地圖權(quán)重調(diào)大，平移旋轉(zhuǎn)權(quán)重調(diào)小
  • 將代碼中平移和旋轉(zhuǎn)的殘差注釋掉
• 里程計(jì)
  • Cartographer 中對(duì)里程計(jì)的使用不太好
  • 可以將 karto 與 GMapping 中使用里程計(jì)進(jìn)行預(yù)測(cè)的部分拿過(guò)來(lái)進(jìn)行使用，改完后就可達(dá)到比較好的位姿預(yù)測(cè)效果
• 粗匹配
  • 將 karto 的掃描匹配的粗匹配放過(guò)來(lái)，karto 的掃描匹配的計(jì)算量很小，當(dāng)做粗匹配很不錯(cuò)
• 地圖
  • 在最終生成地圖的時(shí)候使用后端優(yōu)化后的節(jié)點(diǎn)重新生成一次地圖，這樣生成的地圖效果會(huì)比前端地圖的疊加要好很多

4.2 降低計(jì)算量與內(nèi)存

• 體素濾波與自適應(yīng)體素濾波的計(jì)算量（不是很大）
• 后端進(jìn)行子圖間約束時(shí)的計(jì)算量很大
• 分支定界算法的計(jì)算量很大
• 降低內(nèi)存，內(nèi)存的占用基本就是多分辨率地圖，每個(gè)子圖的多分辨率地圖都進(jìn)行保存是否有必要

4.3 純定位的改進(jìn)建議

• 將純定位模式與建圖拆分開(kāi)，改成讀取之前軌跡的地圖進(jìn)行匹配.
• 新的軌跡只進(jìn)行位姿預(yù)測(cè)，拿到預(yù)測(cè)后的位姿與之前軌跡的地圖進(jìn)行匹配，新的軌跡不再進(jìn)行地圖的生成與保存，同時(shí)將整個(gè)后端的功能去掉.
• 去掉后端優(yōu)化會(huì)導(dǎo)致沒(méi)有重定位功能，這時(shí)可將 cartographer 的回環(huán)檢測(cè)（子圖間約束的計(jì)算）部分單獨(dú)拿出來(lái)，做成一個(gè)重定位功能，通過(guò)服務(wù)來(lái)調(diào)用這個(gè)重定位功能，根據(jù)當(dāng)前點(diǎn)云確定機(jī)器人在之前地圖的位姿

4.4 去 ros 的參考思路

• 仿照 cartographer_ros 里的操作：獲取到傳感器數(shù)據(jù)，將數(shù)據(jù)轉(zhuǎn)到 tracking_frame 坐標(biāo)系下并進(jìn)行格式轉(zhuǎn)換，再傳入到 cartographer 里就行

5. 源碼注釋

• cartographer_detailed_comments_ws