search

cartographer-project / cartographer

Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor configurations.
Apache License 2.0
7.16k stars 2.25k forks source link

Submaps seem to have no constraints between them. #1393

Closed thomasyoung closed 6 years ago

thomasyoung commented 6 years ago

System

Ubuntu 16.04, Bazel 0.16.1,

Commit

5261c90c3421fd6e7eab570e13f99a46dd6d8c07

Phenomena

I am using cartographer without ROS. I have simulated Lidar giving scan data and simulated odometry data. From LocalSlamResultCallback in MapBuilder::AddTrajectoryBuilder() I am seeing reasonable trajectory. From MapBuilder::pose_graph()->GetAllSubmapData() I got MapById<SubmapId, SubmapData> and I am seeing reasonable pieces in SubmapData::submap->grid() which is const ::cartographer::mapping::ProbabilityGrid*. I try to use Submap::pose to stitch these pieces together. And I see these Submaps seem to have correct orientation but in-consistent translation. I also read something like I0816 18:05:41.186154 27835 constraint_builder_2d.cc:282] 1 computations resulted in 0 additional constraints. from log.

Visualization

I use pixel size of 0.05 and apply `Submap::pose to pixels before putting them together.

Feeding Data

Initialization:

  cartographer::mapping::proto::MapBuilderOptions map_builder_options;
  auto file_resolver = absl::make_unique<cartographer::common::ConfigurationFileResolver>(
      absl::StrSplit(get_lua_configuration_directory(), ','));
  map_builder_options.set_collate_by_trajectory(true);
  const std::string code = file_resolver->GetFileContentOrDie(get_lua_configuration_basename());
  cartographer::common::LuaParameterDictionary lua_parameter_dictionary(code,
                                                                        std::move(file_resolver));
  map_builder_options = ::cartographer::mapping::CreateMapBuilderOptions(
      lua_parameter_dictionary.GetDictionary("map_builder").get());
  map_builder_ = absl::make_unique<cartographer::mapping::MapBuilder>(map_builder_options);
  ::cartographer::mapping::proto::TrajectoryBuilderOptions trajectory_builder_options =
      ::cartographer::mapping::CreateTrajectoryBuilderOptions(
          lua_parameter_dictionary.GetDictionary("trajectory_builder").get());

  std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId> expected_sensor_ids{
      {::cartographer::mapping::TrajectoryBuilderInterface::SensorId::SensorType::RANGE,
       kLidarName},
      {::cartographer::mapping::TrajectoryBuilderInterface::SensorId::SensorType::ODOMETRY,
       kOdometryName}};
  trajectory_id_ = map_builder_->AddTrajectoryBuilder(
      expected_sensor_ids, trajectory_builder_options,
      [this](int trajectory_id, ::cartographer::common::Time time,
             ::cartographer::transform::Rigid3d local_pose,
             ::cartographer::sensor::RangeData range_data_in_local,
             std::unique_ptr<
                 const ::cartographer::mapping::TrajectoryBuilderInterface::InsertionResult>) {
        OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
      });
  trajectory_builder_ = map_builder_->GetTrajectoryBuilder(trajectory_id_);

  constexpr double kExtrapolationEstimationTimeSec = 0.001;  // 1 ms
  ASSERT(extrapolators_.count(trajectory_id_) == 0, "Duplicate Trajectory");
  extrapolators_.emplace(
      std::piecewise_construct, std::forward_as_tuple(trajectory_id_),
      std::forward_as_tuple(
          ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
          trajectory_builder_options.trajectory_builder_2d_options().imu_gravity_time_constant()));

Provide Data:

  // world_T_lidar_rigid3 is the Rigid3 that convert from Lidar frame to world frame
  trajectory_builder_->AddSensorData(
      kOdometryName, ::cartographer::sensor::OdometryData{scan_time, *world_T_lidar_rigid3});
  trajectory_builder_->AddSensorData(
      kLidarName, ::cartographer::sensor::TimedPointCloudData{
                      scan_time, Vector3f::Zero(),
                      ::cartographer::sensor::TransformTimedPointCloud(
                          pointcloud.points /* vector of Vector3f in Lidar frame with 0 on z-axis */,
                          ::cartographer::transform::Rigid3f::Identity())});

Config

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
}

MAP_BUILDER.use_trajectory_builder_2d = true
MAP_BUILDER.use_trajectory_builder_3d = false
MAP_BUILDER.pose_graph.optimize_every_n_nodes = 2
MAP_BUILDER.pose_graph.global_constraint_search_after_n_seconds = 3
MAP_BUILDER.pose_graph.constraint_builder.max_constraint_distance = 100
TRAJECTORY_BUILDER_2D.use_imu_data = false

TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability = 0.6
TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.miss_probability = 0.4
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 50
kdaun commented 6 years ago

Thank you for the detailed overview. Can you please post the whole log as text?

And I see these Submaps seem to have correct orientation but in-consistent translation.

Can you elaborate on:

I0816 18:05:41.186154 27835 constraint_builder_2d.cc:282] 1 computations resulted in 0 additional constraints.

This indicates there was 1 candidate for loop closure evaluated which did not meet POSE_GRAPH.constraint_builder.min_score and therefore was not added as a constraint.

Check the Tuning Guide for more information.

thomasyoung commented 6 years ago

@kdaun Thanks for helping!

Following the Tuning Guide I tweaked the parameter a little:

MAP_BUILDER.use_trajectory_builder_2d = true
MAP_BUILDER.use_trajectory_builder_3d = false

TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability = 0.9
TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.miss_probability = 0.1
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 50
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1

TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 2e1

POSE_GRAPH.constraint_builder.min_score=0.3
POSE_GRAPH.constraint_builder.sampling_ratio = 0.7
POSE_GRAPH.optimize_every_n_nodes = 10
POSE_GRAPH.optimize_every_n_nodes = 2
POSE_GRAPH.global_constraint_search_after_n_seconds = 3
POSE_GRAPH.constraint_builder.max_constraint_distance = 100

The sub-maps seem to be in correct scale and orientation. The started to be several meters off. Later they were almost aligned perfectly before suddenly jumping away from each other as shown below. A corridor here is roughly 2 meters wide.

sm01 sm00 sm_0_1

Here is the log I got:

I0820 11:16:08.386246  4238 configuration_file_resolver.cc:41] Found 'apps/flatsim//carter.lua' for 'carter.lua'.
I0820 11:16:08.386456  4238 configuration_file_resolver.cc:41] Found '../com_github_googlecartographer_cartographer/configuration_files//map_builder.lua' for 'map_builder.lua'.
I0820 11:16:08.386487  4238 configuration_file_resolver.cc:41] Found '../com_github_googlecartographer_cartographer/configuration_files//map_builder.lua' for 'map_builder.lua'.
I0820 11:16:08.386540  4238 configuration_file_resolver.cc:41] Found '../com_github_googlecartographer_cartographer/configuration_files//pose_graph.lua' for 'pose_graph.lua'.
I0820 11:16:08.386565  4238 configuration_file_resolver.cc:41] Found '../com_github_googlecartographer_cartographer/configuration_files//pose_graph.lua' for 'pose_graph.lua'.
I0820 11:16:08.386692  4238 configuration_file_resolver.cc:41] Found '../com_github_googlecartographer_cartographer/configuration_files//trajectory_builder.lua' for 'trajectory_builder.lua'.
I0820 11:16:08.386718  4238 configuration_file_resolver.cc:41] Found '../com_github_googlecartographer_cartographer/configuration_files//trajectory_builder.lua' for 'trajectory_builder.lua'.
I0820 11:16:08.386775  4238 configuration_file_resolver.cc:41] Found '../com_github_googlecartographer_cartographer/configuration_files//trajectory_builder_2d.lua' for 'trajectory_builder_2d.lua'.
I0820 11:16:08.386798  4238 configuration_file_resolver.cc:41] Found '../com_github_googlecartographer_cartographer/configuration_files//trajectory_builder_2d.lua' for 'trajectory_builder_2d.lua'.
I0820 11:16:08.386903  4238 configuration_file_resolver.cc:41] Found '../com_github_googlecartographer_cartographer/configuration_files//trajectory_builder_3d.lua' for 'trajectory_builder_3d.lua'.
I0820 11:16:08.386929  4238 configuration_file_resolver.cc:41] Found '../com_github_googlecartographer_cartographer/configuration_files//trajectory_builder_3d.lua' for 'trajectory_builder_3d.lua'.
I0820 11:16:08.406352  4232 ordered_multi_queue.cc:172] All sensor data for trajectory 0 is available starting at '794205'.
I0820 11:16:08.406414  4232 local_trajectory_builder_2d.cc:315] Extrapolator not yet initialized.
W0820 11:16:08.428285  4229 pose_extrapolator.cc:168] Queue too short for velocity estimation. Queue duration: 0 ms
I0820 11:16:08.443886  4229 pose_graph_2d.cc:133] Inserted submap (0, 0).
I0820 11:16:14.048514  4242 pose_graph_2d.cc:493] Remaining work items in queue: 0
I0820 11:16:14.048607  4242 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:14.048646  4242 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:14.531646  4241 pose_graph_2d.cc:493] Remaining work items in queue: 0
I0820 11:16:14.531740  4241 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:14.531771  4241 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:14.948503  4240 pose_graph_2d.cc:493] Remaining work items in queue: 1
I0820 11:16:14.948546  4240 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:14.948561  4240 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:15.504904  4241 pose_graph_2d.cc:493] Remaining work items in queue: 1
I0820 11:16:15.504972  4241 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:15.505010  4241 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:16.059389  4242 pose_graph_2d.cc:493] Remaining work items in queue: 0
I0820 11:16:16.059446  4242 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:16.059463  4242 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:16.604918  4241 pose_graph_2d.cc:493] Remaining work items in queue: 1
I0820 11:16:16.604981  4241 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:16.605000  4241 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:16.887661  4242 pose_graph_2d.cc:493] Remaining work items in queue: 0
I0820 11:16:16.887758  4242 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:16.887799  4242 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:17.150410  4241 pose_graph_2d.cc:493] Remaining work items in queue: 1
I0820 11:16:17.150492  4241 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:17.150521  4241 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:17.371204  4242 pose_graph_2d.cc:493] Remaining work items in queue: 1
I0820 11:16:17.371279  4242 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:17.371305  4242 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:17.616118  4241 pose_graph_2d.cc:493] Remaining work items in queue: 1
I0820 11:16:17.616200  4241 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:17.616230  4241 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:17.814246  4242 pose_graph_2d.cc:493] Remaining work items in queue: 0
I0820 11:16:17.814337  4242 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:17.814371  4242 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:18.047089  4241 pose_graph_2d.cc:493] Remaining work items in queue: 1
I0820 11:16:18.047184  4241 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:18.047217  4241 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:18.286721  4239 pose_graph_2d.cc:493] Remaining work items in queue: 1
I0820 11:16:18.286798  4239 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:18.286828  4239 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:18.551259  4242 pose_graph_2d.cc:493] Remaining work items in queue: 1
I0820 11:16:18.551321  4242 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:18.551334  4242 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:18.797816  4241 pose_graph_2d.cc:493] Remaining work items in queue: 0
I0820 11:16:18.797897  4241 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:18.797930  4241 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:19.003482  4239 pose_graph_2d.cc:493] Remaining work items in queue: 1
I0820 11:16:19.003569  4239 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:19.003602  4239 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:19.229740  4226 pose_graph_2d.cc:133] Inserted submap (0, 1).
I0820 11:16:19.229877  4241 pose_graph_2d.cc:493] Remaining work items in queue: 0
I0820 11:16:19.229914  4241 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:19.229928  4241 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:19.732094  4239 pose_graph_2d.cc:493] Remaining work items in queue: 1
I0820 11:16:19.732215  4239 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:19.732246  4239 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:20.604620  4242 pose_graph_2d.cc:493] Remaining work items in queue: 0
I0820 11:16:20.604674  4242 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:20.604688  4242 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:21.415112  4240 pose_graph_2d.cc:493] Remaining work items in queue: 0
I0820 11:16:21.415197  4240 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:21.415235  4240 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:22.849040  4242 pose_graph_2d.cc:493] Remaining work items in queue: 0
I0820 11:16:22.849098  4242 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:22.849115  4242 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:23.391125  4233 collated_trajectory_builder.cc:81] odometry rate: 9.63 Hz 1.04e-01 s +/- 6.86e-02 s (pulsed at 99.83% real time)
I0820 11:16:23.391178  4233 collated_trajectory_builder.cc:81] range rate: 9.74 Hz 1.03e-01 s +/- 6.72e-02 s (pulsed at 98.26% real time)
I0820 11:16:23.665293  4240 pose_graph_2d.cc:493] Remaining work items in queue: 0
I0820 11:16:23.665364  4240 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:23.665387  4240 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:24.569769  4239 pose_graph_2d.cc:493] Remaining work items in queue: 0
I0820 11:16:24.569821  4239 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:24.569835  4239 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:25.389613  4241 pose_graph_2d.cc:493] Remaining work items in queue: 0
I0820 11:16:25.389681  4241 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:25.389703  4241 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:26.230587  4242 pose_graph_2d.cc:493] Remaining work items in queue: 0
I0820 11:16:26.230689  4242 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:26.230728  4242 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:27.071045  4240 pose_graph_2d.cc:493] Remaining work items in queue: 0
I0820 11:16:27.071123  4240 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:27.071146  4240 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:27.951095  4241 pose_graph_2d.cc:493] Remaining work items in queue: 1
I0820 11:16:27.951159  4241 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:27.951180  4241 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:28.831374  4239 pose_graph_2d.cc:493] Remaining work items in queue: 0
I0820 11:16:28.831471  4239 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:28.831506  4239 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:29.987287  4242 pose_graph_2d.cc:493] Remaining work items in queue: 0
I0820 11:16:29.987349  4242 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:29.987365  4242 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:31.106581  4241 pose_graph_2d.cc:493] Remaining work items in queue: 0
I0820 11:16:31.106644  4241 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:31.106660  4241 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:31.963465  4240 pose_graph_2d.cc:493] Remaining work items in queue: 1
I0820 11:16:31.963551  4240 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:31.963579  4240 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:32.838410  4241 pose_graph_2d.cc:493] Remaining work items in queue: 0
I0820 11:16:32.838521  4241 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:32.838554  4241 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:33.709697  4240 pose_graph_2d.cc:493] Remaining work items in queue: 1
I0820 11:16:33.709779  4240 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:33.709806  4240 constraint_builder_2d.cc:284] Score histogram:
Count: 0
I0820 11:16:34.606011  4228 pose_graph_2d.cc:133] Inserted submap (0, 2).
I0820 11:16:35.175624  4240 constraint_builder_2d.cc:267] Node (0, 100) with 202 points on submap (0, 0) differs by translation 0.02 rotation 0.005 with score 39.5%.
I0820 11:16:35.310346  4241 pose_graph_2d.cc:493] Remaining work items in queue: 1
I0820 11:16:35.576208  4242 constraint_builder_2d.cc:267] Node (0, 101) with 208 points on submap (0, 0) differs by translation 0.02 rotation 0.002 with score 39.5%.
I0820 11:16:35.576262  4242 constraint_builder_2d.cc:282] 2 computations resulted in 2 additional constraints.
I0820 11:16:35.576295  4242 constraint_builder_2d.cc:284] Score histogram:
Count: 2  Min: 0.394510  Max: 0.395414  Mean: 0.394962
[0.394510, 0.394600)              ##########    Count: 1 (50.000000%)   Total: 1 (50.000000%)
[0.394600, 0.394691)                            Count: 0 (0.000000%)    Total: 1 (50.000000%)
[0.394691, 0.394781)                            Count: 0 (0.000000%)    Total: 1 (50.000000%)
[0.394781, 0.394872)                            Count: 0 (0.000000%)    Total: 1 (50.000000%)
[0.394872, 0.394962)                            Count: 0 (0.000000%)    Total: 1 (50.000000%)
[0.394962, 0.395053)                            Count: 0 (0.000000%)    Total: 1 (50.000000%)
[0.395053, 0.395143)                            Count: 0 (0.000000%)    Total: 1 (50.000000%)
[0.395143, 0.395234)                            Count: 0 (0.000000%)    Total: 1 (50.000000%)
[0.395234, 0.395324)                            Count: 0 (0.000000%)    Total: 1 (50.000000%)
[0.395324, 0.395414]              ##########    Count: 1 (50.000000%)   Total: 2 (100.000000%)
I0820 11:16:35.867403  4239 constraint_builder_2d.cc:267] Node (0, 102) with 200 points on submap (0, 0) differs by translation 0.02 rotation 0.003 with score 41.6%.
I0820 11:16:36.452813  4239 pose_graph_2d.cc:493] Remaining work items in queue: 0
I0820 11:16:36.708395  4242 constraint_builder_2d.cc:267] Node (0, 104) with 200 points on submap (0, 0) differs by translation 0.04 rotation 0.002 with score 38.0%.
I0820 11:16:36.708451  4242 constraint_builder_2d.cc:282] 2 computations resulted in 2 additional constraints.
I0820 11:16:36.708484  4242 constraint_builder_2d.cc:284] Score histogram:
Count: 4  Min: 0.380282  Max: 0.416329  Mean: 0.396634
[0.380282, 0.383887)                   #####    Count: 1 (25.000000%)   Total: 1 (25.000000%)
[0.383887, 0.387492)                            Count: 0 (0.000000%)    Total: 1 (25.000000%)
[0.387492, 0.391096)                            Count: 0 (0.000000%)    Total: 1 (25.000000%)
[0.391096, 0.394701)                   #####    Count: 1 (25.000000%)   Total: 2 (50.000000%)
[0.394701, 0.398306)                   #####    Count: 1 (25.000000%)   Total: 3 (75.000000%)
[0.398306, 0.401911)                            Count: 0 (0.000000%)    Total: 3 (75.000000%)
[0.401911, 0.405515)                            Count: 0 (0.000000%)    Total: 3 (75.000000%)
[0.405515, 0.409120)                            Count: 0 (0.000000%)    Total: 3 (75.000000%)
[0.409120, 0.412725)                            Count: 0 (0.000000%)    Total: 3 (75.000000%)
[0.412725, 0.416329]                   #####    Count: 1 (25.000000%)   Total: 4 (100.000000%)
I0820 11:16:37.068981  4240 constraint_builder_2d.cc:267] Node (0, 105) with 203 points on submap (0, 0) differs by translation 0.04 rotation 0.011 with score 36.0%.
I0820 11:16:37.571311  4240 pose_graph_2d.cc:493] Remaining work items in queue: 1
I0820 11:16:37.819602  4241 constraint_builder_2d.cc:267] Node (0, 107) with 209 points on submap (0, 0) differs by translation 0.10 rotation 0.017 with score 35.1%.
I0820 11:16:37.819653  4241 constraint_builder_2d.cc:282] 2 computations resulted in 2 additional constraints.
I0820 11:16:37.819694  4241 constraint_builder_2d.cc:284] Score histogram:
Count: 6  Min: 0.350965  Max: 0.416329  Mean: 0.382980
[0.350965, 0.357502)                     ###    Count: 1 (16.666666%)   Total: 1 (16.666666%)
[0.357502, 0.364038)                     ###    Count: 1 (16.666666%)   Total: 2 (33.333332%)
[0.364038, 0.370575)                            Count: 0 (0.000000%)    Total: 2 (33.333332%)
[0.370575, 0.377111)                            Count: 0 (0.000000%)    Total: 2 (33.333332%)
[0.377111, 0.383647)                     ###    Count: 1 (16.666666%)   Total: 3 (50.000000%)
[0.383647, 0.390184)                            Count: 0 (0.000000%)    Total: 3 (50.000000%)
[0.390184, 0.396720)                 #######    Count: 2 (33.333332%)   Total: 5 (83.333336%)
[0.396720, 0.403257)                            Count: 0 (0.000000%)    Total: 5 (83.333336%)
[0.403257, 0.409793)                            Count: 0 (0.000000%)    Total: 5 (83.333336%)
[0.409793, 0.416329]                     ###    Count: 1 (16.666666%)   Total: 6 (100.000000%)
I0820 11:16:37.984714  4242 pose_graph_2d.cc:493] Remaining work items in queue: 2
I0820 11:16:38.336047  4240 constraint_builder_2d.cc:267] Node (0, 108) with 221 points on submap (0, 0) differs by translation 0.02 rotation 0.017 with score 30.8%.
I0820 11:16:38.336102  4240 constraint_builder_2d.cc:282] 1 computations resulted in 1 additional constraints.
I0820 11:16:38.336139  4240 constraint_builder_2d.cc:284] Score histogram:
Count: 7  Min: 0.308379  Max: 0.416329  Mean: 0.372322
[0.308379, 0.319174)                     ###    Count: 1 (14.285714%)   Total: 1 (14.285714%)
[0.319174, 0.329969)                            Count: 0 (0.000000%)    Total: 1 (14.285714%)
[0.329969, 0.340764)                            Count: 0 (0.000000%)    Total: 1 (14.285714%)
[0.340764, 0.351559)                     ###    Count: 1 (14.285714%)   Total: 2 (28.571428%)
[0.351559, 0.362354)                     ###    Count: 1 (14.285714%)   Total: 3 (42.857143%)
[0.362354, 0.373149)                            Count: 0 (0.000000%)    Total: 3 (42.857143%)
[0.373149, 0.383944)                     ###    Count: 1 (14.285714%)   Total: 4 (57.142857%)
[0.383944, 0.394739)                     ###    Count: 1 (14.285714%)   Total: 5 (71.428574%)
[0.394739, 0.405534)                     ###    Count: 1 (14.285714%)   Total: 6 (85.714287%)
[0.405534, 0.416329]                     ###    Count: 1 (14.285714%)   Total: 7 (100.000000%)
I0820 11:16:38.338914  4240 pose_graph_2d.cc:493] Remaining work items in queue: 1
I0820 11:16:38.338943  4240 constraint_builder_2d.cc:282] 0 computations resulted in 0 additional constraints.
I0820 11:16:38.338977  4240 constraint_builder_2d.cc:284] Score histogram:
Count: 7  Min: 0.308379  Max: 0.416329  Mean: 0.372322
[0.308379, 0.319174)                     ###    Count: 1 (14.285714%)   Total: 1 (14.285714%)
I0820 11:16:38.439116  4239 constraint_builder_2d.cc:284] Score histogram:
Count: 7  Min: 0.308379  Max: 0.416329  Mean: 0.372322
[0.308379, 0.319174)                     ###    Count: 1 (14.285714%)   Total: 1 (14.285714%)
[0.319174, 0.329969)                            Count: 0 (0.000000%)    Total: 1 (14.285714%)
[0.329969, 0.340764)                            Count: 0 (0.000000%)    Total: 1 (14.285714%)
[0.340764, 0.351559)                     ###    Count: 1 (14.285714%)   Total: 2 (28.571428%)
[0.351559, 0.362354)                     ###    Count: 1 (14.285714%)   Total: 3 (42.857143%)
[0.362354, 0.373149)                            Count: 0 (0.000000%)    Total: 3 (42.857143%)
[0.373149, 0.383944)                     ###    Count: 1 (14.285714%)   Total: 4 (57.142857%)
[0.383944, 0.394739)                     ###    Count: 1 (14.285714%)   Total: 5 (71.428574%)
[0.394739, 0.405534)                     ###    Count: 1 (14.285714%)   Total: 6 (85.714287%)
[0.405534, 0.416329]                     ###    Count: 1 (14.285714%)   Total: 7 (100.000000%)

I also tried to visualize using PaintSubmapSlices:

  const double kPixelSize = 0.05;
  const auto submap_poses = map_builder_->pose_graph()->GetAllSubmapPoses();
  std::map<cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice> submap_slices;

  for (const auto& submap_id_pose : submap_poses) {
    CHECK(submap_id_pose.id.trajectory_id == trajectory_id_, "Wrong trajectory_id");
    cartographer::mapping::proto::SubmapQuery::Response response_proto;
    const std::string error = map_builder_->SubmapToProto(submap_id_pose.id, &response_proto);
    CHECK(error.empty(), "Error retrieving sub-map data.");

    auto submap_textures = absl::make_unique<::cartographer::io::SubmapTextures>();
    submap_textures->version = response_proto.submap_version();
    for (const auto& texture_proto : response_proto.textures()) {
      const std::string compressed_cells(texture_proto.cells().begin(),
                                         texture_proto.cells().end());
      CHECK(!compressed_cells.empty(), "Empty Texture Cell Data.");
      submap_textures->textures.emplace_back(::cartographer::io::SubmapTexture{
          ::cartographer::io::UnpackTextureData(compressed_cells, texture_proto.width(),
                                                texture_proto.height()),
          texture_proto.width(), texture_proto.height(), texture_proto.resolution(),
          ToRigid3d(texture_proto.slice_pose())});
    }
    CHECK(!response_proto.textures().empty(), "Empty Texture");

    // Prepares SubmapSlice
    ::cartographer::io::SubmapSlice& submap_slice = submap_slices[submap_id_pose.id];
    const auto fetched_texture = submap_textures->textures.begin();
    submap_slice.width = fetched_texture->width;
    submap_slice.height = fetched_texture->height;
    submap_slice.slice_pose = fetched_texture->slice_pose;
    submap_slice.resolution = fetched_texture->resolution;
    submap_slice.cairo_data.clear();
    submap_slice.surface = ::cartographer::io::DrawTexture(
        fetched_texture->pixels.intensity, fetched_texture->pixels.alpha, fetched_texture->width,
        fetched_texture->height, &submap_slice.cairo_data);
  }
  // Generates occupancy grid as CreateOccupancyGridMsg()
  cartographer::io::PaintSubmapSlicesResult painted_slices =
      PaintSubmapSlices(submap_slices, kPixelSize);

And here is what I got: carto-painted

kdaun commented 6 years ago

TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability = 0.9 TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.miss_probability = 0.1

These values don't look reasonable to me. With these update probabilities each update step is almost completely overwriting the previous updates in the submap (read here for more background). hit_probability = 0.55 and miss_probability = 0.49 are usually reasonable starting values.

POSE_GRAPH.optimize_every_n_nodes = 10 POSE_GRAPH.optimize_every_n_nodes = 2

This parameter is set twice in your configuration. Try setting it to the same value as TRAJECTORY_BUILDER_2D.submaps.num_range_data.

What exactly are the black/white maps showing?

The blue/cyan map shows a lot of single rays at the boundary of the map. If laser rangefinders observe free space, sometimes they can give you such ghost points. Especially in combination with the low value for the miss probability that can lead to artifacts similar to the artifacts shown in your map. Try adding min/max range parameters, e.g. TRAJECTORY_BUILDER_2D.min_range = 1.5 TRAJECTORY_BUILDER_2D.max_range = 20.

thomasyoung commented 6 years ago

@kdaun Thanks for suggestions! Now I am using following parameters:

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
}

MAP_BUILDER.use_trajectory_builder_2d = true
MAP_BUILDER.use_trajectory_builder_3d = false

TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 4e2
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 2e2
TRAJECTORY_BUILDER_2D.max_range = 20
TRAJECTORY_BUILDER_2D.min_range = 1.5
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 100
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.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true

POSE_GRAPH.constraint_builder.max_constraint_distance = 100
POSE_GRAPH.constraint_builder.min_score=0.2
POSE_GRAPH.constraint_builder.sampling_ratio = 0.7
POSE_GRAPH.global_constraint_search_after_n_seconds = 3
POSE_GRAPH.optimize_every_n_nodes = 100

Besides I have map_builder_options.set_collate_by_trajectory(true); The black/white maps are created using following code (roughly):

  const auto submap_list = map_builder_->pose_graph()->GetAllSubmapData();
  int count = 0;
  for (const auto& map_data : submap_list) {
    std::string label = absl::StrCat("submap_", count);
    count++;
    // All submaps are Submap2D with ProbabilityGrid inside.
    const std::shared_ptr<const ::cartographer::mapping::Submap> submap = map_data.data.submap;
    const ::cartographer::mapping::Submap2D* submap_2d =
        static_cast<const ::cartographer::mapping::Submap2D*>(submap.get());
    CHECK(submap_2d != nullptr, "Submap is not Submap2D");
    CHECK(submap_2d->grid()->GetGridType() == ::cartographer::mapping::GridType::PROBABILITY_GRID,
           "grid2d is not ProbabilityGrid");

      const ::cartographer::mapping::ProbabilityGrid* prob_grid =
          static_cast<const ::cartographer::mapping::ProbabilityGrid*>(submap_2d->grid());
      Eigen::Array2i offset;
      ::cartographer::mapping::CellLimits cell_limits;
      prob_grid->ComputeCroppedLimits(&offset, &cell_limits);
      submap_.resize(cell_limits.num_y_cells, cell_limits.num_x_cells); // rows-by-cols 4 channels image of uint8_t
      auto submap_write_iter = submap_.raw_begin(); // Go over each pixel row by row
      for (const Eigen::Array2i& xy_index :
           ::cartographer::mapping::XYIndexRangeIterator(cell_limits)) {
        uint8_t val = 0;
        if (prob_grid->IsKnown(xy_index + offset)) {
          // per comment in cartographer:
          // Converts a probability to a log odds integer. 0 means unknown, [kMinLogOdds,
          // kMaxLogOdds] is mapped to [1, 255].
          val = ::cartographer::mapping::ProbabilityToLogOddsInteger(
              prob_grid->GetProbability(xy_index + offset));
        }
        *submap_write_iter = val;
        submap_write_iter++;
        *submap_write_iter = val;
        submap_write_iter++;
        *submap_write_iter = val;
        submap_write_iter++;
        *submap_write_iter = val;
        submap_write_iter++;
      }

      auto const map_pose3d = map_data.data.pose;
      Rigid2d offset_pose2d = Rigid2d::Translation(kPixelSize * offset.matrix().cast<double>());

      Draw(submap_, Rigid3dToRidge2dOnXY(map_pose3d)* offset_pose2d, kPixelSize); // Draws to canvas with pose
  }

Following are the result I got (Visualized using my code above and PaintSubmapSlices() separately:

flatsim - cartographer cartographer

Here is the neighborhood that I am simulating flat Lidar scan from: map

We can see that the 2 submaps are off by couple of meters.

kdaun commented 6 years ago

Thank you for the update. Can you elaborate a little bit more on your system? How do you simulate the LiDAR returns and the odometry? With which frequency are they providing observations? The lots of long lines in your map look somewhat unusual to me.

For tuning your system I would suggest trying to tune local SLAM first, to avoid potential issues from the constraint detection. And then if local SLAM looks good, start tuning global SLAM. As your environment is small local SLAM should already give sufficient results. Therefore:

  1. Validate your input data is correct. Visualize simulated LiDAR + Odometry results
  2. Local SLAM only. Turn off the constraint search + correlative_scan_matching and create only a single submap TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = false TRAJECTORY_BUILDER_2D.submaps.num_range_data = 1e8 POSE_GRAPH.constraint_builder.sampling_ratio = 1e8 POSE_GRAPH.optimize_every_n_nodes = 1e8 And see if you can get a consistent map by tuning: TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.miss_probability TRAJECTORY_BUILDER_2D.num_accumulated_range_data TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight
  3. If Local SLAM works for you, you can start tuning the global SLAM as mentionend in the Tuning Guide.
thomasyoung commented 6 years ago

@kdaun Thanks for helping!

Believe the local SLAM is working fine as each submap looks reasonable. Turns out that submap->local_pose for every submap has 0 rotation. And following code gives translation of submaps that make them match together:

// Converts to 2d matrix as image
  for (int row = 0; row < rows; row++) {
    for (int col = 0; col < cols; col++) {
      // Compute corresponding location in probability grid
      const Eigen::Array2i carto_cell =
          offset_cells + Eigen::Array2i{cols - 1 - col, rows - 1 - row};
      image(row, col) = ::cartographer::mapping::ProbabilityToLogOddsInteger(
                        prob_grid->GetProbability(carto_cell));
}
}
  const auto map_limits = prob_grid->limits();
  const double resolution = map_limits.resolution();

  const double row_translation =
      map_limits.max().x()/resolution - (offset_cells.y() + cell_limits.num_y_cells);
  const double col_translation =
      map_limits.max().y()/resolution - (offset_cells.x() + cell_limits.num_x_cells);

But I could not find any relation between these translation and the one in `submap->local_pose`.