はじめに
Cartographer は、PC等端末 + 2D Lidar さえあれば SLAM （地図生成＆自己位置推定）できてしまう、便利なソフトです。
使い始める時にパラメータの設定がちょっとわかりづらかったので、設定をまとめておきます。
作った地図
図：キッチン～リビング～自室 の作った地図
（畳む前の洗濯物の山、ダイニングテーブルの脚、ピアノのイスの脚、ソファがわかりますでしょうか？）
前回の記事
なお、ページ下部の「公式パラメータガイド」「参考になるリンク」はとても参考になりましたので、合わせてご覧下さい。
Cartographer インストール後の 1st step
Cartographer を起動するのは、
・ lua ファイル にパラメータを書き、
・ URDF ファイル に想定するロボットのリンクを一度書いてしまえば、
・ launch ファイル を実行するだけ
です。
初期設定ファイル（*.lua / *.urdf / *.launch） をコピーして利用するようにしましょう。
ファイルへのリンクは後程にも置いてますが、 ここ に置いてあります。
cd catkin_ws
#1. 前記事のように、catkin build でビルドした場合
source devel/setup.bash
cd src/cartographer_ros/cartographer_ros/configuration_files
#2. Google 公式チュートリアルのように、catkin_make_isolated でビルドした場合
#source devel_isolated/setup.bash
#cd install_isolated/share/cartographer_ros/configuration_files
#以下、共通
cp ./backpack_2d.lua ./myrobot2d.lua
cd ../launch
cp ./backpack_2d.launch ./myrobot2d.launch
cd ../urdf
cp ./backpack_2d.urdf ./myrobot.urdf
myrobot2d.launch
以下を修正します。
<launch>
<param name="robot_description"
- textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" />
+ textfile="$(find cartographer_ros)/urdf/myrobot.urdf" />
<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" />
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
- -configuration_basename backpack_2d.lua"
+ -configuration_basename myrobot2d.lua"
output="screen">
<remap from="echoes" to="horizontal_laser_2d" />
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
+ <node name="rviz" pkg="rviz" type="rviz" required="true"
+ args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
</launch>
myrobot.urdf
ひとまず動くためのリンクやジョイント構成にしています。
今回は、map -> base_link -> imu_link, laser というようになりました。
これらリンクをジョイントで接続しています。
<robot name="cartographer_backpack_2d">
<material name="orange">
<color rgba="1.0 0.5 0.2 1" />
</material>
<material name="gray">
<color rgba="0.2 0.2 0.2 1" />
</material>
<link name="imu_link">
<visual>
<origin xyz="0 0 0" />
<geometry>
<box size="0.06 0.04 0.02" />
</geometry>
<material name="orange" />
</visual>
</link>
- <link name="horizontal_laser_link">
+ <link name="laser">
<visual>
<origin xyz="0 0 0" />
<geometry>
<cylinder length="0.05" radius="0.03" />
</geometry>
<material name="gray" />
</visual>
</link>
- <link name="vertical_laser_link">
- <visual>
- <origin xyz="0 0 0" />
- <geometry>
- <cylinder length="0.05" radius="0.03" />
- </geometry>
- <material name="gray" />
- </visual>
- </link>
<link name="base_link" />
+ <joint name="laser_joint" type="fixed">
+ <parent link="base_link" />
+ <child link="laser" />
+ <origin rpy="0 0 0" xyz="0.1007 0 0.0558" />
+ </joint>
<joint name="imu_link_joint" type="fixed">
<parent link="base_link" />
<child link="imu_link" />
<origin xyz="0 0 0" />
</joint>
- <joint name="horizontal_laser_link_joint" type="fixed">
- <parent link="base_link" />
- <child link="horizontal_laser_link" />
- <origin xyz="0.1007 0 0.0558" />
- </joint>
- <joint name="vertical_laser_link_joint" type="fixed">
- <parent link="base_link" />
- <child link="vertical_laser_link" />
- <origin rpy="0 -1.570796 3.141593" xyz="0.1007 0 0.1814" />
- </joint>
</robot>
myrobot2d.lua
以下が初期設定状態から、2D Lidar １台のみを利用する設定への一部修正です。
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
- tracking_frame = "base_link",
+ tracking_frame = "imu_link", -- URDF の設定に合わせる
published_frame = "base_link",
odom_frame = "odom",
- provide_odom_frame = true,
+ provide_odom_frame = false, -- オドメトリフレームは発行しない
publish_frame_projected_to_2d = false,
use_pose_extrapolator = true,
use_odometry = false,
use_nav_sat = false,
use_landmarks = false,
- num_laser_scans = 0,
+ num_laser_scans = 1, --2D Lidar ひとつ。
- num_multi_echo_laser_scans = 1,
+ num_multi_echo_laser_scans = 0, --マルチエコーのレーザースキャナーは無し。
- num_subdivisions_per_laser_scan = 10,
+ num_subdivisions_per_laser_scan = 1, -- レーザースキャン分割は１。
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 10
+TRAJECTORY_BUILDER_2D.use_imu_data = false -- IMU 使用しない
return options
これらの設定変更で、2D Lidar のみ利用での SLAM が実行できました！ 便利！
ですが、動作が重いので、少し軽くしたいです。また、地図がうっすらと出来ていきますが、出来る速度ももう少し速くしたい。
ここまでのファイルと、最終的な編集ファイルは、こちらに置いておきます。
設定するにあたっての方針
もう少しサクサク地図生成したい
私は2D での利用をしていますが、今回の設定の流れは以下のようになります。作ったファイルはダウンロード先にあります。
（時短の方は、ダウンロード先 myrobot2d_edit.lua の中身を myrobot2d.luaにコピペして実行して下さい。）
①Cartographer で使う機能の ON / OFF（Lidar 2D/3D, IMU, オドメトリ, GPS 等）
②地図生成/SLAM： 挙動の設定（解像度や距離など）
③追加設定
④自己位置推定： localization モードでの設定
これらを、cartographer_ros/configuration_files の自前 myrobot2d.lua ファイルに設定します。
なお、設定が動作しているかどうかは、Cartographer 実行時のログとRviz画面の挙動をよく見るしかありませんｗ
（マッチング点群が表示/非表示や、地図生成されているかどうかなど）
また、１つ１つのパラメータの効果を見るには、Google の公式ガイドによれば
「It is recommended to only explore one option at a time, starting with the first. 」１つずつ確認してくれだそうです。
（Cartographer のバージョン更新によって、パラメータ名が変わる場合もあります。）
参考：デフォルトの *.lua ファイル（パラメータ初期値が書いてある）
（折りたたんであるので、必要時にクリックして表示してください。）
map builder 基本的な設定
cartographer/configuration_files/map_builder.lua
include "pose_graph.lua"
MAP_BUILDER = {
use_trajectory_builder_2d = false,
use_trajectory_builder_3d = false,
num_background_threads = 4,
pose_graph = POSE_GRAPH,
collate_by_trajectory = false,
}
Global （大域的） SLAM 設定
（POSE_GRAPH. という名前）
cartographer/configuration_files/pose_graph.lua
POSE_GRAPH = {
optimize_every_n_nodes = 90,
constraint_builder = {
sampling_ratio = 0.3,
max_constraint_distance = 15.,
min_score = 0.55,
global_localization_min_score = 0.6,
loop_closure_translation_weight = 1.1e4,
loop_closure_rotation_weight = 1e5,
log_matches = true,
fast_correlative_scan_matcher = {
linear_search_window = 7.,
angular_search_window = math.rad(30.),
branch_and_bound_depth = 7,
},
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,
},
},
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_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,
optimization_problem = {
huber_scale = 1e1,
acceleration_weight = 1.1e2,
rotation_weight = 1.6e4,
local_slam_pose_translation_weight = 1e5,
local_slam_pose_rotation_weight = 1e5,
odometry_translation_weight = 1e5,
odometry_rotation_weight = 1e5,
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,
global_sampling_ratio = 0.003,
log_residual_histograms = true,
global_constraint_search_after_n_seconds = 10.,
-- overlapping_submaps_trimmer_2d = {
-- fresh_submaps_count = 1,
-- min_covered_area = 2,
-- min_added_submaps_count = 5,
-- },
}
Local （局所的） (2D) SLAM 設定
（TRAJECTORY_BUILDER_2D. という名前）
cartographer/configuration_files/trajectory_builder_2d.lua
TRAJECTORY_BUILDER_2D = {
use_imu_data = true,
min_range = 0.,
max_range = 30.,
min_z = -0.8,
max_z = 2.,
missing_data_ray_length = 5.,
num_accumulated_range_data = 1,
voxel_filter_size = 0.025,
adaptive_voxel_filter = {
max_length = 0.5,
min_num_points = 200,
max_range = 50.,
},
loop_closure_adaptive_voxel_filter = {
max_length = 0.9,
min_num_points = 100,
max_range = 50.,
},
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,
rotation_delta_cost_weight = 1e-1,
},
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,
},
},
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.,
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;
},
},
},
submaps = {
num_range_data = 90,
grid_options_2d = {
grid_type = "PROBABILITY_GRID",
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_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,
},
},
},
}
Local （局所的） (3D) SLAM 設定
（TRAJECTORY_BUILDER_3D. という名前）
cartographer/configuration_files/trajectory_builder_3d.lua
MAX_3D_RANGE = 60.
INTENSITY_THRESHOLD = 40
TRAJECTORY_BUILDER_3D = {
min_range = 1.,
max_range = MAX_3D_RANGE,
num_accumulated_range_data = 1,
voxel_filter_size = 0.15,
high_resolution_adaptive_voxel_filter = {
max_length = 2.,
min_num_points = 150,
max_range = 15.,
},
low_resolution_adaptive_voxel_filter = {
max_length = 4.,
min_num_points = 200,
max_range = MAX_3D_RANGE,
},
use_online_correlative_scan_matching = false,
real_time_correlative_scan_matcher = {
linear_search_window = 0.15,
angular_search_window = math.rad(1.),
translation_delta_cost_weight = 1e-1,
rotation_delta_cost_weight = 1e-1,
},
ceres_scan_matcher = {
occupied_space_weight_0 = 1.,
occupied_space_weight_1 = 6.,
intensity_cost_function_options_0 = {
weight = 0.5,
huber_scale = 0.3,
intensity_threshold = INTENSITY_THRESHOLD,
},
translation_weight = 5.,
rotation_weight = 4e2,
only_optimize_yaw = false,
ceres_solver_options = {
use_nonmonotonic_steps = false,
max_num_iterations = 12,
num_threads = 1,
},
},
motion_filter = {
max_time_seconds = 0.5,
max_distance_meters = 0.1,
max_angle_radians = 0.004,
},
rotational_histogram_size = 120,
-- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS.
imu_gravity_time_constant = 10.,
pose_extrapolator = {
use_imu_based = false,
constant_velocity = {
imu_gravity_time_constant = 10.,
pose_queue_duration = 0.001,
},
-- TODO(wohe): Tune these parameters on the example datasets.
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;
},
},
},
submaps = {
high_resolution = 0.10,
high_resolution_max_range = 20.,
low_resolution = 0.45,
num_range_data = 160,
range_data_inserter = {
hit_probability = 0.55,
miss_probability = 0.49,
num_free_space_voxels = 2,
intensity_threshold = INTENSITY_THRESHOLD,
},
},
-- When setting use_intensites to true, the intensity_cost_function_options_0
-- parameter in ceres_scan_matcher has to be set up as well or otherwise
-- CeresScanMatcher will CHECK-fail.
use_intensities = false,
}
パラメータひとつずつの説明は、前記事のこちらをご覧下さい。
設定をはじめる
それでは見ていきましょう。
オプションは myrobo2d.lua ファイル の上部 optins 内 に、
MAP_BUILDER、POSE_GRAPH、TRAJECTORY_BUILDER_2D は下部（ return options の前まで）に書きます。
距離 (distance) は全てメートル表記です。時間は全て秒単位です。
"-- " 以降はコメントアウトになります。
"5." などは 5.0 の意味です。
(例： 3e3 = 3 * (10 ^ 3) = 3000 )
(例： 4e-2 = 4 * (0.1 ^ 2) = 0.04 )
{} で囲む中の行には、行末に "," が必要で、１行ごとのパラメータには "," は不要。
同じパラメータを複数書くと、後ろに書いた方が優先されるようです。
①Cartographer で使う機能の ON / OFF（Lidar 2D/3D, IMU, オドメトリ, GPS 等）
まずは基本の Options の設定です。
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
-- ●この下は、URDF のロボット構成に合わせて記述する感じです。
map_frame = "map", -- Rviz の Global Options の Fixed Frame に入るフレーム名です。
tracking_frame = "imu_link", -- IMU tracking 用のフレーム名です。
published_frame = "base_link",
odom_frame = "odom", -- オドメトリのフレーム名です。
-- ●この下は、機能を ON/OFF します。
provide_odom_frame = false, -- オドメトリを発行しない
use_pose_extrapolator = true, -- ???
use_odometry = false, -- オドメトリデータをSLAM に利用する（true だと必要となる）
use_nav_sat = false, -- NavSat (GPS等) をSLAM に利用する（true だと必要となる）
use_landmarks = false, -- ランドマークをSLAM に利用する（true だと必要となる）
-- ●この下は、センサの数を指定します。
num_laser_scans = 1, -- 2D Lidar の数
num_multi_echo_laser_scans = 0, -- マルチエコーのレーザースキャナ―の数
num_subdivisions_per_laser_scan = 1, -- スキャンを分割する数
num_point_clouds = 0, -- sensor_msgs/PointCloud2 を受け取る数
-- ●この下は、各種の時間やサンプリング比率を設定します。
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
MAP_BUILDER.use_trajectory_builder_2d = true -- 2D モードで利用する
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 10
TRAJECTORY_BUILDER_2D.use_imu_data = false -- IMU は利用しない
②地図生成/SLAM： 挙動の設定 （解像度や距離など）
では次に、公式チューニングガイドを参考に動作設定パラメータについて見ていきましょう。
図： Cartographer 公式ガイドより引用
ここは、基本的に上記リンク先の公式ガイド "Algorithm walkthrough for tuning" "Tuning methodology"に沿った内容になります。
--- "Algorithm walkthrough for tuning" ---
"Algorithm walkthrough for tuning" のページを読んでみると、
Local SLAM が局所的なマップを生成するもので、submapというものをたくさん作っていきます。
Global SLAM が大きなマップを生成するもので、Pose グラフというものを利用したものだそうです。各々のPose （姿勢）をロープでつなげるように、Trajectory（軌跡）をConstraint（制約）して組み合わせていく感じでしょうか。さらにループクローズという機能で、その繋がりが補正され地図の精度が向上します。
図 紫の線： Trajectory (軌跡)
図 カラフルな線： Constraint (制約)
これらの大まかな概念を踏まえた上で、Lidar を基本に用いつつ、必要に応じて IMU、オドメトリ、GPSを利用してさらに地図生成に役立てるようです。（なお 3D 版は IMU 必須とのこと）
今回は 2D Lidar のみでの SLAM を想定しています。
基本設定
Lidar の距離設定です。この範囲外はトリミングされます。
TRAJECTORY_BUILDER_2D.min_range = 0.
TRAJECTORY_BUILDER_2D.max_range = 20. -- 自分の Lidar の性能に合わせる。 LD06 Lidar なら 12m + α
-- TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5 --'max_range' を超える点は、この長さで空白部として挿入されます。
TRAJECTORY_BUILDER_2D.max_range = 30.（例： 30. -> 3. に変更）
設定した距離(3m)以上の点群が消え、フリーな空間のみ白くなる
また、以下の2個の設定を行うことで、レスポンスが良くなりました。（Local SLAM のところで後述）
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true -- 相関スキャンマッチャーというのを有効にします。
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1 -- 1 (歪み対策。増やすとレスポンス低下したので 1 or 2?)
次は、フィルタのサイズの調整です。点群に対するいくつかのフィルタ処理により、処理が重くなったり軽くなったりします。
voxel_filter_size は、
小さくすれば処理は重くなり、解像度・品質は上がります。
大きくすれば処理は軽くなり、解像度・品質は下がります。
（この先、デフォルト値のままにするものはコメントアウトしています。）
-- TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.025
-- > 切り出し直後の範囲データに適用されるボクセルフィルタ。
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 0.5
-- > ボクセルフィルタの 'max_length' 。
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 200
-- > 点群数が多くありつつも、 'min_num_points' より少ない場合、この最小点数を得るために
-- > ボクセルフィルタ長が減少される。
-- TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.max_length = 50.
-- > ボクセルフィルタの 'max_length' 。
-- TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.min_num_points = 100
-- > 点群数が多くありつつも、 'min_num_points' より少ない場合、この最小点数を得るために
-- > ボクセルフィルタ長が減少される。
TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.25 （例： 0.025 -> 0.25 に変更）
スキャン点群がフィルタされ少なくなっている
次は、IMU を用いる場合の設定です。
今回はIMU は利用しない想定です。
-- TRAJECTORY_BUILDER_2D.use_imu_data = true （既に前で false に設定済。ここではコメントアウト）
-- TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 10.
Local SLAM の設定
次に、Local SLAM の設定です。
どうやらスキャンマッチを行うものは２種類あるようです。
(CeresScanMatcher / RealTimeCorrelativeScanMatcher)
1. CeresScanMatcher （Google のCeres ライブラリによるスキャンマッチャー）
基本、高速、大きなエラーは直せない とのこと。
センサがよくセットアップされていれば、これだけで良いとのこと。
（原文）
The CeresScanMatcher takes the initial guess as prior and finds the best spot where the scan match fits the submap. It does this by interpolating the submap and sub-pixel aligning the scan. This is fast, but cannot fix errors that are significantly larger than the resolution of the submaps. If your sensor setup and timing is reasonable, using only the CeresScanMatcher is usually the best choice to make.
2. RealTimeCorrelativeScanMatcher （相関スキャンマッチャー）
ロバスト、Lidar 以外のセンサが無い場合は有効にすると良い、とのこと。
（原文）
The RealTimeCorrelativeScanMatcher can be enabled if you do not have other sensors or you do not trust them. It uses an approach similar to how scans are matched against submaps in loop closure (described later), but instead it matches against the current submap. The best match is then used as prior for the CeresScanMatcher.
This scan matcher is very expensive and will essentially override any signal from other sensors but the range finder, but it is robust in feature rich environments.
今回は Lidar １つの想定で、先程この相関スキャンマッチャーというものを有効にすることでレスポンスが良くなりました。
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
Ceres スキャンマッチャーにおける weight (重み) ですが、移動や回転、空間に対する信頼する重みや挙動を変えられるようです。
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 1.
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 10.
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 40.
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.use_nonmonotonic_steps = false
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 20
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.num_threads = 1
相関スキャンマッチャーは、こちらは window というパラメータを修正することで、センサの移動や回転に対するマッチングにおける信頼する重みを変えられるようです。
-- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
-- > 最適なスキャンアライメントを見つけるための最小の線形探索 Window。
-- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(20.)
-- > 最適なスキャンアライメントが得られる最小の角度探索 Window。
-- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 1e-1
-- > スコアの各パートに適用される重み。
-- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1
-- > Not yet documented.
個々のレーザースキャンデータ間の無駄なものを省くモーションフィルタというものがあるようです。
距離・角度・時間の閾値を超える場合はドロップしたり挿入されたりするようです。
-- TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 5.
-- > これ以上の時間の時に、範囲データが挿入される閾値（秒）。
-- TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.2
-- > これ以上の距離の時に、線形移動に基づき範囲データが挿入される閾値。
-- TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(1.)
-- > これ以上の角度の時に、回転に基づいて範囲データが挿入される閾値（ラジアン）。
次のパラメータは、１つのサブマップを作るにあたりどれだけの数の範囲データを受信するの設定値です。
ドリフトが影響しない程度に小さく、ループクロージャが適切に機能する程度には十分大きくするとのことです。
-- TRAJECTORY_BUILDER_2D.submaps.num_range_data = 90 -- 後で120 にする
サブマップは、範囲データを格納するデータ構造の形式を選べるようです。
PROBABILITY_GRID (確率グリッド)、
または 2D であれば Truncated Signed Distance Fields (TSDF)
も選択できるようです。
-- TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.grid_type = "PROBABILITY_GRID" -- or "TSDF"
参考 issue ：
https://github.com/cartographer-project/cartographer/issues/1533
PROBABILITY_GRID (確率グリッド) は、オッズを利用して、Hit(距離データがある場所)/Miss(空白) に従って各セルが更新されるようです。
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D"
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.insert_free_space = true
-- > 'false' の時、フリーな空間は占有グリッドの確率を変更しない。
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability = 0.55
-- > hit した場合の確率の変化（オッズに変換されるため、0.5以上でなければならない）。
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.miss_probability = 0.49
-- > miss した場合の確率の変化（オッズに変換されるため、0.5未満でなければならない）。
2D では、１つのグリッドが１つのサブマップに格納されます。
3D では、遠距離測定用の低解像度ハイブリッドグリッド、近接測定用の高解像度ハイブリッドグリッド の２つがあるようです。
-- TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.05
-- > 地図の解像度（メートル）。
-- TRAJECTORY_BUILDER_3D.submaps.high_resolution = 0.10
-- > Local SLAM とループクロージャーに使用する 'high_resolution'
-- > マップの解像度 （メートル）。
-- TRAJECTORY_BUILDER_3D.submaps.low_resolution = 0.45
-- > Local SLAM のみに使用される 'low_resolution' 版のマップの解像度（メートル）。
-- TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter.max_range = 15.
-- > 原点から遠く削除される点の距離。
-- TRAJECTORY_BUILDER_3D.low_resolution_adaptive_voxel_filter.max_range = 60.
-- > 原点から遠く削除される点の距離。
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.25（例： 0.05 -> 0.25 に変更）
ボクセル（ピクセル）が荒く低い解像度になる
以下は TSDF の設定項目です。
（あまり説明がない？）
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.truncation_distance = 0.3
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.maximum_weight = 10.
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.update_free_space = false
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.normal_estimation_options.num_normal_samples = 4
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.normal_estimation_options.sample_radius = 0.5
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.project_sdf_distance_to_scan_normal = true
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.update_weight_range_exponent = 0
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0.5
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.update_weight_distance_cell_to_hit_kernel_bandwidth = 0.5
Global SLAM の設定
さて次は Global SLAM についてです。
optimize_every_n_nodes は、この数の軌道ノードが挿入されるごとに Global SLAM が実行されます。その頻度です。
0 にすれば、Global SLAM をオフにすることができます。（Local SLAM だけにして、パラメータ調整をしやすくする事ができる）
-- POSE_GRAPH.optimize_every_n_nodes = 90
Global SLAM には２つの Constraint (制約) があるようです。
1. 非グローバル制約 (サブマップ内制約)
軌跡上で互いに密接に追従しているノード間に自動的に構築されます。
直観的には、これらの「非グローバルなロープ」は軌道のローカル構造を首尾一貫した状態に保ちます。
2. グローバル制約 (ループ クロージャ制約、サブマップ間制約)
空間 (特定の検索ウィンドウの一部) で「十分に近い」と見なされ、強い適合 (良好な一致) と見なされる新しいサブマップと以前のノードの間で定期的に検索されます。(スキャンマッチングの実行時) 。
直観的には、これらの「グローバルなロープ」は構造に結び目を導入し、2つのストランドをしっかりと近づけます。
関連するパラメータです。
-- POSE_GRAPH.constraint_builder.max_constraint_distance = 15.
-- > サブマップと近い poses について、考慮する距離の閾値。
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.linear_search_window = 7.
-- > 最適なスキャンアライメントを見つけるための最小の線形探索 Window。
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.linear_xy_search_window = 5.
-- > 重力に直交する平面において、最適なスキャンアライメントが得られるための線形探索 window の数。
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.linear_z_search_window = 1.
-- > 重力方向において、最適なスキャンアライメントが得られるための線形探索 window の数。
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.angular_search_window = math.rad(30.)
-- > 最適なスキャンアライメントが得られる最小の角度探索 Window。
Constraint (制約) をどれだけ行うかのレートです。
制約を行う回数が、多ければ重くなり、少なければマップ構築がうまくいきません。
-- POSE_GRAPH.constraint_builder.sampling_ratio = 0.3
-- > 潜在的な制約に対する、追加された制約の割合が、この設定値より低くなると、
-- > 制約が追加されることになります。
Cartographer の Global SLAM では、特別に設計された FastCorrelativeScanMatcher というスキャンマッチャーが使われるそうです。
以下の depth というもので挙動変更できるようです。
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.branch_and_bound_depth = 7
-- > 事前計算に使用するグリッドの数。
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.branch_and_bound_depth = 8
-- > 事前計算に使用するグリッドの数。
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.full_resolution_depth = 3
-- > フル解像度グリッドに使う設定値。グリッドを追加すると、それぞれ解像度が半分になります。
FastCorrelativeScanMatcher がマッチングの min_score を超え十分であるとなると、Ceres スキャン マッチャーにフィードされ pose が調整されます。
その際の Ceres の挙動は、Local SLAM 時のものと同様にパラメータを調整します。
-- POSE_GRAPH.constraint_builder.min_score = 0.55
-- POSE_GRAPH.constraint_builder.loop_closure_translation_weight = 1.1e4
-- POSE_GRAPH.constraint_builder.loop_closure_rotation_weight = 1e5
-- POSE_GRAPH.matcher_translation_weight = 5e2
-- POSE_GRAPH.matcher_rotation_weight = 1.6e3
-- POSE_GRAPH.optimization_problem.*_weight
-- POSE_GRAPH.optimization_problem.ceres_solver_options
IMU と Ceres に関わる挙動パラメータです。
デフォルトで、Ceres は IMU ポーズを自由に最適化するようです。
IMU が信頼できない場合は、Ceres のグローバル最適化の結果をログに記録し、外部キャリブレーションを改善するために使用できます。
IMU が信頼でき、キャリブレーションが十分信頼できる場合は、このポーズを一定にすることができます。
（原文）
As part of its IMU residual, the optimization problem gives some flexibility to the IMU pose and, by default, Ceres is free to optimize the extrinsic calibration between your IMU and tracking frame. If you don’t trust your IMUpose, the results of Ceres’ global optimization can be logged and used to improve your extrinsic calibration. If Ceres doesn’t optimize your IMU pose correctly and you trust your extrinsic calibration enough, you can make this pose constant.
-- POSE_GRAPH.optimization_problem.log_solver_summary = false
-- > 有効なら、Ceres ソルバーのサマリーはすべての最適化についてログに記録します。
-- POSE_GRAPH.optimization_problem.use_online_imu_extrinsics_in_3d = true
詳しくわかりませんが、以下の Huber スケールが大きいほど、(潜在的な) 外れ値の影響が大きくなります。
-- POSE_GRAPH.optimization_problem.huber_scale = 1e1
-- > Huber loss function のスケーリング値
軌跡が完成すると、Cartographer は以下の反復回数に沿って最終的な最適化を実行します。
-- POSE_GRAPH.max_num_final_iterations = 200
-- > 'optimization_problem options' で最終的な最適化に使用する反復回数
--- "Tuning methodology" ---
次に、"Tuning methodology" のページでの内容です。
先程までの説明とかぶるパラメータの説明も出てきます。
「Local SLAM」を調整してから、「Global SLAM」を調整しています。
まず「Global SLAM」をオフにし、「Local SLAM」だけの状態で設定して、その後「Global SLAM」をオンにしています。
POSE_GRAPH.optimize_every_n_nodes = 0 -- 90 -> 0 Global SLAM を OFF にする
そのあと、下記のような設定を変えているのですが、正直リビングだけでの SLAM だと違いがわからず。元に戻しました。
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 2e2 -- 10 -> 200
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 4e2 -- 40 -> 400
このページが参考になりました。
低レイテンシ（高速化）設定
次に、低レイテンシつまり高速化の設定についても公式チューニングガイドに記載がありました。
（高速化は、つまり品質は低下すると思います。逆にすれば高品質化ということでしょうか。）
いくつかの変更例の画像は、ここより前に載せました。
この中の上２個（voxel_filter_size / resolution）を変えると、一気に処理の重さが変わります。
単位はメートルのはずです。論文では Cartographer を 2.5cm 単位の～と書いてますが、
2D Lidar (LD06, 10m性能) で自室リビングをSLAMするのはラズパイでもできましたが、
2D Lidar (RPLIDAR S2, 30m性能)ではつくばチャレンジのような広い屋外をSLAMするのは、デフォルト値では
解像度が高すぎることになるのか、処理が重すぎて無理でした。（Core i5 4世代と古いPCでの感覚）
0.25 m (25 cm)くらいにしてほどよくなりました。
Local SLAM 高速化について
TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.025 -- 増やす デフォルト： 0.025
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.05 -- 増やす デフォルト： 0.05
TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 200 -- 小さく デフォルト： 200
TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_range = 50. -- 小さく デフォルト： 50.
TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 0.5 -- 増やす デフォルト： 0.5
TRAJECTORY_BUILDER_2D.max_range = 30. -- 小さく デフォルト： 30.
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 20 -- 小さく デフォルト： 20
Global SLAM 高速化について
ここはまだ自分は検証できていません。大きな地図生成には、これらが大きく影響してくるかと思っています。
調整は Rviz 上でTrajectory / Constraint を ON/OFF し視覚的に行えるとのことです。
POSE_GRAPH.optimize_every_n_nodes = 90 -- 小さく デフォルト： 90
MAP_BUILDER.num_background_threads = 4 -- 増やす(CPUコア数)デフォルト: 4
POSE_GRAPH.global_sampling_ratio = 0.003 -- 小さく デフォルト： 0.003
POSE_GRAPH.constraint_builder.sampling_ratio = 0.3 -- 小さく デフォルト： 0.3
POSE_GRAPH.constraint_builder.min_score = 0.55 -- 増やす デフォルト: 0.55
TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 200 -- 小さく デフォルト： 200 (Local SLAM 設定と同じ)
TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_range = 50. -- 小さく デフォルト： 50. (Local SLAM 設定と同じ)
TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 0.5 -- 増やす デフォルト： 0.5 (Local SLAM 設定と同じ)
TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.025 -- 増やす デフォルト： 0.025 (Local SLAM 設定と同じ)
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.05 -- 増やす デフォルト： 0.05 (Local SLAM 設定と同じ)
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 20 -- 小さく デフォルト： 20 (Local SLAM 設定と同じ)
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.linear_xy_search_window = 5.
-- 小さく デフォルト： 5.
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.linear_z_search_window = 1.
-- 小さく デフォルト： 1.
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.angular_search_window = math.rad(30.)
-- 小さく デフォルト： math.rad(30.)
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(20.)
-- 小さく デフォルト： math.rad(20.)
POSE_GRAPH.global_constraint_search_after_n_seconds = 10. -- 増やす デフォルト: 10.
POSE_GRAPH.constraint_builder.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 10
-- 小さく デフォルト： 10
POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 50
-- 小さく デフォルト： 50
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 20
-- 小さく デフォルト： 20
③追加設定
こちらの PINTO03091 さんの素晴らしい記事（2018年、先駆者！）を参考に、パラメータ追加する事で地図生成のレスポンスと品質が良くなったので、追加します。
-- from Mr.PINTO03091 's settings. Thanks!
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 120
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.15
-- デフォルト: 0.1 -> 0.15
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(35.)
-- デフォルト: math.rad(20.) -> math.rad(35.)
POSE_GRAPH.constraint_builder.min_score = 0.65 -- デフォルト: 0.55 -> 0.65
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7 -- デフォルト: 0.6 -> 0.7
POSE_GRAPH.optimization_problem.huber_scale = 1e3 -- デフォルト: 1e1 -> 1e3
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 10
-- デフォルト: 1. -> 10
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 120 -- デフォルト: 90 -> 120
TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.1 -- デフォルト: 0.2 -> 0.1
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.2)
-- デフォルト: math.rad(1.) -> math.rad(0.2)
④自己位置推定： localization モードでの設定
これはさほど設定項目はありません。
Pure Localization モードは、前記事のように launch/myrobot2d_localization.launch を実行しますが、
下記のようにTRAJECTORY_BUILDER.pure_localization_trimmer の max_submaps_to_keep を設定すると有効になるようです。
POSE_GRAPH.optimize_every_n_nodes はすごく小さい値にしているのですが、この場合でも Local SLAM はなされている？ようでマップの更新が続きます。
include "myrobot2d.lua"
TRAJECTORY_BUILDER.pure_localization_trimmer = {
max_submaps_to_keep = 3,
}
POSE_GRAPH.optimize_every_n_nodes = 3
return options
まとめ（と集約した lua ファイル）
今回のパラメータ説明は以上になりますが、下記に myrobot2d_edit.lua としてダウンロードできるようにしておきます。
myrobot2d_edit.lua の内容を myrobot2d.lua に書き込んで使用してください。
（内容は以下に折りたたんでありますので、下記クリックで表示）
myrobot2d_edit.lua
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "imu_link",
published_frame = "base_link",
odom_frame = "odom",
provide_odom_frame = false,
publish_frame_projected_to_2d = false,
use_pose_extrapolator = true,
use_odometry = false,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 1,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 10
TRAJECTORY_BUILDER_2D.use_imu_data = false
--added
TRAJECTORY_BUILDER_2D.min_range = 0.
TRAJECTORY_BUILDER_2D.max_range = 20. -- 自分の Lidar の性能に合わせる。 LD06 Lidar なら 12m + α
-- TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5 --'max_range' を超える点は、この長さで空白部として挿入されます。
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true -- 相関スキャンマッチャーというのを有効にします。
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1 -- 1 (歪み対策。増やすとレスポンス低下したので 1 or 2?)
-- TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.025
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 0.5
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 200
-- TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.max_length = 50.
-- TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.min_num_points = 100
-- TRAJECTORY_BUILDER_2D.use_imu_data = true （既に前で false に設定済）
-- TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 10.
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 1.
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 10.
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 40.
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.use_nonmonotonic_steps = false
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 20
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.num_threads = 1
-- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
-- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(20.)
-- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 1e-1
-- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1
-- TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 5.
-- TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.2
-- TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(1.)
-- TRAJECTORY_BUILDER_2D.submaps.num_range_data = 90
-- TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.grid_type = "PROBABILITY_GRID" -- or "TSDF"
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D"
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.insert_free_space = true
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability = 0.55
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.miss_probability = 0.49
-- TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.05
-- TRAJECTORY_BUILDER_3D.submaps.high_resolution = 0.10
-- TRAJECTORY_BUILDER_3D.submaps.low_resolution = 0.45
-- TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter.max_range = 15.
-- TRAJECTORY_BUILDER_3D.low_resolution_adaptive_voxel_filter.max_range = 60.
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.truncation_distance = 0.3
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.maximum_weight = 10.
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.update_free_space = false
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.normal_estimation_options.num_normal_samples = 4
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.normal_estimation_options.sample_radius = 0.5
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.project_sdf_distance_to_scan_normal = true
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.update_weight_range_exponent = 0
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0.5
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.update_weight_distance_cell_to_hit_kernel_bandwidth = 0.5
-- POSE_GRAPH.optimize_every_n_nodes = 90
-- POSE_GRAPH.constraint_builder.max_constraint_distance = 15.
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.linear_search_window = 7.
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.linear_xy_search_window = 5.
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.linear_z_search_window = 1.
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.angular_search_window = math.rad(30.)
-- POSE_GRAPH.constraint_builder.sampling_ratio = 0.3
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.branch_and_bound_depth = 7
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.branch_and_bound_depth = 8
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.full_resolution_depth = 3
-- POSE_GRAPH.constraint_builder.min_score = 0.55
-- POSE_GRAPH.constraint_builder.loop_closure_translation_weight = 1.1e4
-- POSE_GRAPH.constraint_builder.loop_closure_rotation_weight = 1e5
-- POSE_GRAPH.matcher_translation_weight = 5e2
-- POSE_GRAPH.matcher_rotation_weight = 1.6e3
-- POSE_GRAPH.optimization_problem.*_weight
-- POSE_GRAPH.optimization_problem.ceres_solver_options
-- POSE_GRAPH.optimization_problem.log_solver_summary = false
-- POSE_GRAPH.optimization_problem.use_online_imu_extrinsics_in_3d = true
-- POSE_GRAPH.optimization_problem.huber_scale = 1e1
-- POSE_GRAPH.max_num_final_iterations = 200
-- POSE_GRAPH.optimize_every_n_nodes = 0 -- 90 -> 0 Global SLAM を OFF にする
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 2e2 -- 10 -> 200
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 4e2 -- 40 -> 400
-- TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.025 -- 増やす デフォルト： 0.025
-- TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.05 -- 増やす デフォルト： 0.05
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 200 -- 小さく デフォルト： 200
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_range = 50. -- 小さく デフォルト： 50.
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 0.5 -- 増やす デフォルト： 0.5
-- TRAJECTORY_BUILDER_2D.max_range = 30. -- 小さく デフォルト： 30.
-- TRAJECTORY_BUILDER_2D.submaps.num_range_data = 20 -- 小さく デフォルト： 20
-- POSE_GRAPH.optimize_every_n_nodes = 90 -- 小さく デフォルト： 90
-- MAP_BUILDER.num_background_threads = 4 -- 増やす(CPUコア数)デフォルト: 4
-- POSE_GRAPH.global_sampling_ratio = 0.003 -- 小さく デフォルト： 0.003
-- POSE_GRAPH.constraint_builder.sampling_ratio = 0.3 -- 小さく デフォルト： 0.3
-- POSE_GRAPH.constraint_builder.min_score = 0.55 -- 増やす デフォルト: 0.55
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 200 -- 小さく デフォルト： 200
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_range = 50. -- 小さく デフォルト： 50.
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 0.5 -- 増やす デフォルト： 0.5
-- TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.025 -- 増やす デフォルト： 0.025
-- TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.05 -- 増やす デフォルト： 0.05
-- TRAJECTORY_BUILDER_2D.submaps.num_range_data = 20 -- 小さく デフォルト： 20
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.linear_xy_search_window = 5.
-- 小さく デフォルト： 5.
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.linear_z_search_window = 1.
-- 小さく デフォルト： 1.
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.angular_search_window = math.rad(30.)
-- 小さく デフォルト： math.rad(30.)
-- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(20.)
-- 小さく デフォルト： math.rad(20.)
-- POSE_GRAPH.global_constraint_search_after_n_seconds = 10. -- 増やす デフォルト: 10.
-- POSE_GRAPH.constraint_builder.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 10
-- 小さく デフォルト： 10
-- POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 50
-- 小さく デフォルト： 50
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 20
-- 小さく デフォルト： 20
-- from Mr.PINTO03091 's settings. Thanks!
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 120
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.15
-- デフォルト: 0.1 -> 0.15
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(35.)
-- デフォルト: math.rad(20.) -> math.rad(35.)
POSE_GRAPH.constraint_builder.min_score = 0.65 -- デフォルト: 0.55 -> 0.65
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7 -- デフォルト: 0.6 -> 0.7
POSE_GRAPH.optimization_problem.huber_scale = 1e3 -- デフォルト: 1e1 -> 1e3
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 10
-- デフォルト: 1. -> 10
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 120 -- デフォルト: 90 -> 120
TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.1 -- デフォルト: 0.2 -> 0.1
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.2)
-- デフォルト: math.rad(1.) -> math.rad(0.2)
return options
この先ですが、Cartographer には多大なパラメータがあります。
さらなる精度向上には内部の動作の流れを理解した上で重みや Window 、閾値の各設定を実際のデータとともに調整していくことになるかと思います。
(rosbag にセンサデータを記録・再生してパラメータ調整するやり方が捗りそうです)
今回はまずその前の、パラメータのいじり方を知る 1st step としての記事を書きました。参考になれば幸いです。開発者の方々、そして Web 記事を残されている先駆者の皆様にも感謝です。
それでは、素敵な SLAM ライフを！
関連記事
公式パラメータガイド
（↑ 下の方に ceres solver のオプションの説明があります）
参考になるリンク
Cartographer ROS Documentation
(2D)
(2D / 3D)