Fix formatting. (#561)

cartographer/mapping/connected_components.cc

@@ -34,7 +34,7 @@ void ConnectedComponents::Add(const int trajectory_id) {
 }
 
 void ConnectedComponents::Connect(const int trajectory_id_a,
-                                     const int trajectory_id_b) {
+                                  const int trajectory_id_b) {
   common::MutexLocker locker(&lock_);
   Union(trajectory_id_a, trajectory_id_b);
   auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b);
@@ -42,7 +42,7 @@ void ConnectedComponents::Connect(const int trajectory_id_a,
 }
 
 void ConnectedComponents::Union(const int trajectory_id_a,
-                                   const int trajectory_id_b) {
+                                const int trajectory_id_b) {
   forest_.emplace(trajectory_id_a, trajectory_id_a);
   forest_.emplace(trajectory_id_b, trajectory_id_b);
   const int representative_a = FindSet(trajectory_id_a);

cartographer/mapping/connected_components_test.cc

@@ -37,7 +37,7 @@ TEST(ConnectedComponentsTest, TransitivelyConnected) {
          ++trajectory_b) {
       EXPECT_EQ(trajectory_a == trajectory_b,
                 connected_components.TransitivelyConnected(trajectory_a,
-                                                              trajectory_b));
+                                                           trajectory_b));
     }
   }
 

cartographer/mapping/pose_extrapolator.cc

@@ -95,10 +95,8 @@ void PoseExtrapolator::AddOdometryData(
   }
   // TODO(whess): Improve by using more than just the last two odometry poses.
   // Compute extrapolation in the tracking frame.
-  const sensor::OdometryData& odometry_data_oldest =
+  const sensor::OdometryData& odometry_data_oldest = odometry_data_.front();
-      odometry_data_.front();
+  const sensor::OdometryData& odometry_data_newest = odometry_data_.back();
-  const sensor::OdometryData& odometry_data_newest =
-      odometry_data_.back();
   const double odometry_time_delta =
       common::ToSeconds(odometry_data_oldest.time - odometry_data_newest.time);
   const transform::Rigid3d odometry_pose_delta =

cartographer/mapping/trajectory_connectivity_state.cc

@@ -40,8 +40,10 @@ void TrajectoryConnectivityState::Connect(const int trajectory_id_a,
     // the two connected components with the connection time. This is to quickly
     // change to a more efficient loop closure search (by constraining the
     // search window) when connected components are joined.
-    std::vector<int> component_a = connected_components_.GetComponent(trajectory_id_a);
+    std::vector<int> component_a =
-    std::vector<int> component_b = connected_components_.GetComponent(trajectory_id_b);
+        connected_components_.GetComponent(trajectory_id_a);
+    std::vector<int> component_b =
+        connected_components_.GetComponent(trajectory_id_b);
     for (const auto id_a : component_a) {
       for (const auto id_b : component_b) {
         auto id_pair = std::minmax(id_a, id_b);
@@ -53,8 +55,7 @@ void TrajectoryConnectivityState::Connect(const int trajectory_id_a,
 }
 
 bool TrajectoryConnectivityState::TransitivelyConnected(
-    const int trajectory_id_a,
+    const int trajectory_id_a, const int trajectory_id_b) {
-    const int trajectory_id_b) {
   return connected_components_.TransitivelyConnected(trajectory_id_a,
                                                      trajectory_id_b);
 }
@@ -64,8 +65,7 @@ std::vector<std::vector<int>> TrajectoryConnectivityState::Components() {
 }
 
 common::Time TrajectoryConnectivityState::LastConnectionTime(
-    const int trajectory_id_a,
+    const int trajectory_id_a, const int trajectory_id_b) {
-    const int trajectory_id_b) {
   auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b);
   return last_connection_time_map_[sorted_pair];
 }

cartographer/mapping/trajectory_connectivity_state.h

@@ -43,16 +43,13 @@ class TrajectoryConnectivityState {
   // tracked. This function is invariant to the order of its arguments. Repeated
   // calls to Connect increment the connectivity count and update the last
   // connected time.
-  void Connect(int trajectory_id_a,
+  void Connect(int trajectory_id_a, int trajectory_id_b, common::Time time);
-               int trajectory_id_b,
-               common::Time time);
 
   // Determines if two trajectories have been (transitively) connected. If
   // either trajectory is not being tracked, returns false, except when it is
   // the same trajectory, where it returns true. This function is invariant to
   // the order of its arguments.
-  bool TransitivelyConnected(int trajectory_id_a,
+  bool TransitivelyConnected(int trajectory_id_a, int trajectory_id_b);
-                             int trajectory_id_b);
 
   // The trajectory IDs, grouped by connectivity.
   std::vector<std::vector<int>> Components();
@@ -71,7 +68,6 @@ class TrajectoryConnectivityState {
   // bipartite trajectories entries for these components are updated with the
   // new connection time.
   std::map<std::pair<int, int>, common::Time> last_connection_time_map_;
-
 };
 
 }  // namespace mapping

cartographer/mapping/trajectory_connectivity_state_test.cc

@@ -87,5 +87,5 @@ TEST(TrajectoryConnectivityStateTest, JoinTwoComponents) {
   EXPECT_EQ(state.LastConnectionTime(0, 1), common::FromUniversal(123456));
 }
 
-}  // namespace cartographer
 }  // namespace mapping
+}  // namespace cartographer

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -179,8 +179,9 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
           node_id.trajectory_id, submap_id.trajectory_id);
   if (node_id.trajectory_id == submap_id.trajectory_id ||
       scan_time <
-          last_connection_time + common::FromSeconds(
+          last_connection_time +
-              options_.global_constraint_search_after_n_seconds())) {
+              common::FromSeconds(
+                  options_.global_constraint_search_after_n_seconds())) {
     // If the scan and the submap belong to the same trajectory or if there has
     // been a recent global constraint that ties that scan's trajectory to the
     // submap's trajectory, it suffices to do a match constrained to a local
@@ -308,10 +309,8 @@ void SparsePoseGraph::ComputeConstraintsForScan(
 }
 
 common::Time SparsePoseGraph::GetLatestScanTime(
-    const mapping::NodeId& node_id,
+    const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) const {
-    const mapping::SubmapId& submap_id) const {
+  common::Time time = trajectory_nodes_.at(node_id).constant_data->time;
-  common::Time time =
-      trajectory_nodes_.at(node_id).constant_data->time;
   const SubmapData& submap_data = submap_data_.at(submap_id);
   if (!submap_data.node_ids.empty()) {
     const mapping::NodeId last_submap_node_id =
@@ -380,19 +379,21 @@ void SparsePoseGraph::WaitForAllComputations() {
       common::FromSeconds(1.))) {
     std::ostringstream progress_info;
     progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
-                  << 100. * (constraint_builder_.GetNumFinishedScans() -
+                  << 100. *
-                             num_finished_scans_at_start) /
+                         (constraint_builder_.GetNumFinishedScans() -
+                          num_finished_scans_at_start) /
                          (num_trajectory_nodes_ - num_finished_scans_at_start)
                   << "%...";
     std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
   }
   std::cout << "\r\x1b[KOptimizing: Done.     " << std::endl;
-  constraint_builder_.WhenDone([this, &notification](
+  constraint_builder_.WhenDone(
-      const sparse_pose_graph::ConstraintBuilder::Result& result) {
+      [this, &notification](
-    common::MutexLocker locker(&mutex_);
+          const sparse_pose_graph::ConstraintBuilder::Result& result) {
-    constraints_.insert(constraints_.end(), result.begin(), result.end());
+        common::MutexLocker locker(&mutex_);
-    notification = true;
+        constraints_.insert(constraints_.end(), result.begin(), result.end());
-  });
+        notification = true;
+      });
   locker.Await([&notification]() { return notification; });
 }
 

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc

@@ -76,7 +76,7 @@ void ConstraintBuilder::MaybeAddConstraint(
     ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
         submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) {
           ComputeConstraint(submap_id, submap, node_id,
-                            false,   /* match_full_submap */
+                            false, /* match_full_submap */
                             constant_data, initial_relative_pose, constraint);
           FinishComputation(current_computation);
         });
@@ -94,10 +94,9 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
   const int current_computation = current_computation_;
   ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
       submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) {
-        ComputeConstraint(submap_id, submap, node_id,
+        ComputeConstraint(
-                          true, /* match_full_submap */
+            submap_id, submap, node_id, true, /* match_full_submap */
-                          constant_data,
+            constant_data, transform::Rigid2d::Identity(), constraint);
-                          transform::Rigid2d::Identity(), constraint);
         FinishComputation(current_computation);
       });
 }

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -199,8 +199,9 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
           node_id.trajectory_id, submap_id.trajectory_id);
   if (node_id.trajectory_id == submap_id.trajectory_id ||
       scan_time <
-          last_connection_time + common::FromSeconds(
+          last_connection_time +
-              options_.global_constraint_search_after_n_seconds())) {
+              common::FromSeconds(
+                  options_.global_constraint_search_after_n_seconds())) {
     // If the scan and the submap belong to the same trajectory or if there has
     // been a recent global constraint that ties that scan's trajectory to the
     // submap's trajectory, it suffices to do a match constrained to a local
@@ -210,22 +211,22 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
         trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
         initial_relative_pose);
   } else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
-      // In this situation, 'initial_relative_pose' is:
+    // In this situation, 'initial_relative_pose' is:
-      //
+    //
-      // submap <- global map 2 <- global map 1 <- tracking
+    // submap <- global map 2 <- global map 1 <- tracking
-      //               (agreeing on gravity)
+    //               (agreeing on gravity)
-      //
+    //
-      // Since they possibly came from two disconnected trajectories, the only
+    // Since they possibly came from two disconnected trajectories, the only
-      // guaranteed connection between the tracking and the submap frames is
+    // guaranteed connection between the tracking and the submap frames is
-      // an agreement on the direction of gravity. Therefore, excluding yaw,
+    // an agreement on the direction of gravity. Therefore, excluding yaw,
-      // 'initial_relative_pose.rotation()' is a good estimate of the relative
+    // 'initial_relative_pose.rotation()' is a good estimate of the relative
-      // orientation of the point cloud in the submap frame. Finding the correct
+    // orientation of the point cloud in the submap frame. Finding the correct
-      // yaw component will be handled by the matching procedure in the
+    // yaw component will be handled by the matching procedure in the
-      // FastCorrelativeScanMatcher, and the given yaw is essentially ignored.
+    // FastCorrelativeScanMatcher, and the given yaw is essentially ignored.
-      constraint_builder_.MaybeAddGlobalConstraint(
+    constraint_builder_.MaybeAddGlobalConstraint(
-          submap_id, submap_data_.at(submap_id).submap.get(), node_id,
+        submap_id, submap_data_.at(submap_id).submap.get(), node_id,
-          trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
+        trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
-          initial_relative_pose.rotation());
+        initial_relative_pose.rotation());
   }
 }
 
@@ -327,10 +328,8 @@ void SparsePoseGraph::ComputeConstraintsForScan(
 }
 
 common::Time SparsePoseGraph::GetLatestScanTime(
-    const mapping::NodeId& node_id,
+    const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) const {
-    const mapping::SubmapId& submap_id) const {
+  common::Time time = trajectory_nodes_.at(node_id).constant_data->time;
-  common::Time time =
-      trajectory_nodes_.at(node_id).constant_data->time;
   const SubmapData& submap_data = submap_data_.at(submap_id);
   if (!submap_data.node_ids.empty()) {
     const mapping::NodeId last_submap_node_id =

cartographer/transform/rigid_transform_test.cc

@@ -28,9 +28,7 @@ namespace {
 template <typename T>
 class RigidTransformTest : public ::testing::Test {
  protected:
-  T eps() {
+  T eps() { return std::numeric_limits<T>::epsilon(); }
-    return std::numeric_limits<T>::epsilon();
-  }
 
   Rigid2<T> GetRandomRigid2() {
     const T x = T(0.7) * distribution_(prng_);