Improve code comments. (#243)

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<std::shared_ptr<const Submap>> insertion_submaps;
   for (std::shared_ptr<Submap> submap : active_submaps_.submaps()) {
     insertion_submaps.push_back(submap);

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 =

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:
+  // The 'constraint_transform' (submap i <- scan j) is computed from:
-  // - a 'filtered_point_cloud' in j,
+  // - a 'filtered_point_cloud' in scan j,
-  // - the initial guess 'initial_pose' for (map <- j),
+  // - the initial guess 'initial_pose' for (map <- scan j),
-  // - the result 'pose_estimate' of Match() (map <- j).
+  // - the result 'pose_estimate' of Match() (map <- scan j).
-  // - the ComputeSubmapPose() (map <- i)
+  // - 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(),

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<std::shared_ptr<const Submap>> insertion_submaps;
   for (std::shared_ptr<Submap> submap : active_submaps_.submaps()) {
     insertion_submaps.push_back(submap);

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<std::shared_ptr<const Submap>> insertion_submaps;
   for (std::shared_ptr<Submap> submap : active_submaps_.submaps()) {
     insertion_submaps.push_back(submap);

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 =

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
+  // The 'constraint_transform' (submap i <- scan j) is computed from:
-  // initial guess 'initial_pose' for (submap 'i' <- scan 'j') and a
+  // - a 'filtered_point_cloud' in scan j and
-  // 'filtered_point_cloud' in 'j'.
+  // - 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,