From 4c3059e0881e662ec971e518a7b71aaae47c725f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20M=C3=BCnst?= Date: Mon, 26 Jun 2017 14:32:44 +0200 Subject: [PATCH] Improve code comments. (#243) --- .../mapping_2d/local_trajectory_builder.cc | 2 ++ cartographer/mapping_2d/sparse_pose_graph.cc | 4 ++++ .../sparse_pose_graph/constraint_builder.cc | 14 +++++++++----- .../mapping_3d/kalman_local_trajectory_builder.cc | 2 ++ .../optimizing_local_trajectory_builder.cc | 2 ++ cartographer/mapping_3d/sparse_pose_graph.cc | 4 ++++ .../sparse_pose_graph/constraint_builder.cc | 10 +++++++--- 7 files changed, 30 insertions(+), 8 deletions(-) diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index 46a32c2..a6ac95b 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -175,6 +175,8 @@ LocalTrajectoryBuilder::AddHorizontalRangeData( return nullptr; } + // Querying the active submaps must be done here before calling + // InsertRangeData() since the queried values are valid for next insertion. std::vector> insertion_submaps; for (std::shared_ptr submap : active_submaps_.submaps()) { insertion_submaps.push_back(submap); diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 1bf0e15..af6f10c 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -123,6 +123,8 @@ void SparsePoseGraph::AddScan( .at(mapping::SubmapId{ trajectory_id, submap_data_.num_indices(trajectory_id) - 1}) .submap != insertion_submaps.back()) { + // We grow 'submap_data_' as needed. This code assumes that the first + // time we see a new submap is as 'insertion_submaps.back()'. const mapping::SubmapId submap_id = submap_data_.Append(trajectory_id, SubmapData()); submap_data_.at(submap_id).submap = insertion_submaps.back(); @@ -246,6 +248,8 @@ void SparsePoseGraph::ComputeConstraintsForScan( matching_id.trajectory_id, scan_data->time, pose, optimized_pose); for (size_t i = 0; i < insertion_submaps.size(); ++i) { const mapping::SubmapId submap_id = submap_ids[i]; + // Even if this was the last scan added to 'submap_id', the submap will only + // be marked as finished in 'submap_data_' further below. CHECK(submap_data_.at(submap_id).state == SubmapState::kActive); submap_data_.at(submap_id).node_ids.emplace(node_id); const transform::Rigid2d constraint_transform = diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index 3d3c95d..ad1b8b6 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -172,14 +172,18 @@ void ConstraintBuilder::ComputeConstraint( const sensor::PointCloud filtered_point_cloud = adaptive_voxel_filter_.Filter(*point_cloud); - // The 'constraint_transform' (i <- j) is computed from: - // - a 'filtered_point_cloud' in j, - // - the initial guess 'initial_pose' for (map <- j), - // - the result 'pose_estimate' of Match() (map <- j). - // - the ComputeSubmapPose() (map <- i) + // The 'constraint_transform' (submap i <- scan j) is computed from: + // - a 'filtered_point_cloud' in scan j, + // - the initial guess 'initial_pose' for (map <- scan j), + // - the result 'pose_estimate' of Match() (map <- scan j). + // - the ComputeSubmapPose() (map <- submap i) float score = 0.; transform::Rigid2d pose_estimate = transform::Rigid2d::Identity(); + // Compute 'pose_estimate' in three stages: + // 1. Fast estimate using the fast correlative scan matcher. + // 2. Prune if the score is too low. + // 3. Refine. if (match_full_submap) { if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( filtered_point_cloud, options_.global_localization_min_score(), diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc index 6d03b7b..1ea3f1a 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc @@ -204,6 +204,8 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap( if (motion_filter_.IsSimilar(time, pose_observation)) { return nullptr; } + // Querying the active submaps must be done here before calling + // InsertRangeData() since the queried values are valid for next insertion. std::vector> insertion_submaps; for (std::shared_ptr submap : active_submaps_.submaps()) { insertion_submaps.push_back(submap); diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc index 29300bf..471bee8 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc @@ -405,6 +405,8 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap( if (motion_filter_.IsSimilar(time, pose_observation)) { return nullptr; } + // Querying the active submaps must be done here before calling + // InsertRangeData() since the queried values are valid for next insertion. std::vector> insertion_submaps; for (std::shared_ptr submap : active_submaps_.submaps()) { insertion_submaps.push_back(submap); diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 223024a..25635c4 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -118,6 +118,8 @@ void SparsePoseGraph::AddScan( .at(mapping::SubmapId{ trajectory_id, submap_data_.num_indices(trajectory_id) - 1}) .submap != insertion_submaps.back()) { + // We grow 'submap_data_' as needed. This code assumes that the first + // time we see a new submap is as 'insertion_submaps.back()'. const mapping::SubmapId submap_id = submap_data_.Append(trajectory_id, SubmapData()); submap_data_.at(submap_id).submap = insertion_submaps.back(); @@ -261,6 +263,8 @@ void SparsePoseGraph::ComputeConstraintsForScan( scan_data->time, optimized_pose); for (size_t i = 0; i < insertion_submaps.size(); ++i) { const mapping::SubmapId submap_id = submap_ids[i]; + // Even if this was the last scan added to 'submap_id', the submap will only + // be marked as finished in 'submap_data_' further below. CHECK(submap_data_.at(submap_id).state == SubmapState::kActive); submap_data_.at(submap_id).node_ids.emplace(node_id); const transform::Rigid3d constraint_transform = diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index 33b9506..e366412 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -179,12 +179,16 @@ void ConstraintBuilder::ComputeConstraint( const sensor::PointCloud filtered_point_cloud = adaptive_voxel_filter_.Filter(point_cloud); - // The 'constraint_transform' (submap 'i' <- scan 'j') is computed from the - // initial guess 'initial_pose' for (submap 'i' <- scan 'j') and a - // 'filtered_point_cloud' in 'j'. + // The 'constraint_transform' (submap i <- scan j) is computed from: + // - a 'filtered_point_cloud' in scan j and + // - the initial guess 'initial_pose' (submap i <- scan j). float score = 0.; transform::Rigid3d pose_estimate; + // Compute 'pose_estimate' in three stages: + // 1. Fast estimate using the fast correlative scan matcher. + // 2. Prune if the score is too low. + // 3. Refine. if (match_full_submap) { if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( initial_pose.rotation(), filtered_point_cloud, point_cloud,