Fix formatting. (#561)

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,7 +179,8 @@ 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 +
+              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
@@ -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,14 +379,16 @@ void SparsePoseGraph::WaitForAllComputations() {
       common::FromSeconds(1.))) {
     std::ostringstream progress_info;
     progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
-                  << 100. * (constraint_builder_.GetNumFinishedScans() -
+                  << 100. *
+                         (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(
+      [this, &notification](
           const sparse_pose_graph::ConstraintBuilder::Result& result) {
         common::MutexLocker locker(&mutex_);
         constraints_.insert(constraints_.end(), result.begin(), result.end());

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc

@@ -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,7 +199,8 @@ 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 +
+              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
@@ -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_);