diff --git a/cartographer/mapping/connected_components.cc b/cartographer/mapping/connected_components.cc index 4290908..22a8115 100644 --- a/cartographer/mapping/connected_components.cc +++ b/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); diff --git a/cartographer/mapping/connected_components_test.cc b/cartographer/mapping/connected_components_test.cc index 999b0b3..5bd9495 100644 --- a/cartographer/mapping/connected_components_test.cc +++ b/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)); } } diff --git a/cartographer/mapping/pose_extrapolator.cc b/cartographer/mapping/pose_extrapolator.cc index 5436fae..e510d2f 100644 --- a/cartographer/mapping/pose_extrapolator.cc +++ 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 = - odometry_data_.front(); - const sensor::OdometryData& odometry_data_newest = - odometry_data_.back(); + const sensor::OdometryData& odometry_data_oldest = odometry_data_.front(); + 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 = diff --git a/cartographer/mapping/trajectory_connectivity_state.cc b/cartographer/mapping/trajectory_connectivity_state.cc index ee733af..eb71610 100644 --- a/cartographer/mapping/trajectory_connectivity_state.cc +++ b/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 component_a = connected_components_.GetComponent(trajectory_id_a); - std::vector component_b = connected_components_.GetComponent(trajectory_id_b); + std::vector component_a = + connected_components_.GetComponent(trajectory_id_a); + std::vector 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_b) { + const int trajectory_id_a, const int trajectory_id_b) { return connected_components_.TransitivelyConnected(trajectory_id_a, trajectory_id_b); } @@ -64,8 +65,7 @@ std::vector> TrajectoryConnectivityState::Components() { } common::Time TrajectoryConnectivityState::LastConnectionTime( - const int trajectory_id_a, - const int trajectory_id_b) { + const int trajectory_id_a, const int trajectory_id_b) { auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b); return last_connection_time_map_[sorted_pair]; } diff --git a/cartographer/mapping/trajectory_connectivity_state.h b/cartographer/mapping/trajectory_connectivity_state.h index ab94963..9b95bdb 100644 --- a/cartographer/mapping/trajectory_connectivity_state.h +++ b/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, - int trajectory_id_b, - common::Time time); + void Connect(int trajectory_id_a, 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, - int trajectory_id_b); + bool TransitivelyConnected(int trajectory_id_a, int trajectory_id_b); // The trajectory IDs, grouped by connectivity. std::vector> Components(); @@ -71,7 +68,6 @@ class TrajectoryConnectivityState { // bipartite trajectories entries for these components are updated with the // new connection time. std::map, common::Time> last_connection_time_map_; - }; } // namespace mapping diff --git a/cartographer/mapping/trajectory_connectivity_state_test.cc b/cartographer/mapping/trajectory_connectivity_state_test.cc index 517f01d..68ed26c 100644 --- a/cartographer/mapping/trajectory_connectivity_state_test.cc +++ b/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 diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index cc2d432..438dd0a 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/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( - options_.global_constraint_search_after_n_seconds())) { + 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 // 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::SubmapId& submap_id) const { - common::Time time = - trajectory_nodes_.at(node_id).constant_data->time; + const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) const { + 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() - - num_finished_scans_at_start) / + << 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, ¬ification]( - const sparse_pose_graph::ConstraintBuilder::Result& result) { - common::MutexLocker locker(&mutex_); - constraints_.insert(constraints_.end(), result.begin(), result.end()); - notification = true; - }); + constraint_builder_.WhenDone( + [this, ¬ification]( + const sparse_pose_graph::ConstraintBuilder::Result& result) { + common::MutexLocker locker(&mutex_); + constraints_.insert(constraints_.end(), result.begin(), result.end()); + notification = true; + }); locker.Await([¬ification]() { return notification; }); } diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index 33a5131..90b7034 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/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, - true, /* match_full_submap */ - constant_data, - transform::Rigid2d::Identity(), constraint); + ComputeConstraint( + submap_id, submap, node_id, true, /* match_full_submap */ + constant_data, transform::Rigid2d::Identity(), constraint); FinishComputation(current_computation); }); } diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 0675878..32e0294 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/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( - options_.global_constraint_search_after_n_seconds())) { + 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 // 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: - // - // submap <- global map 2 <- global map 1 <- tracking - // (agreeing on gravity) - // - // Since they possibly came from two disconnected trajectories, the only - // guaranteed connection between the tracking and the submap frames is - // an agreement on the direction of gravity. Therefore, excluding yaw, - // 'initial_relative_pose.rotation()' is a good estimate of the relative - // orientation of the point cloud in the submap frame. Finding the correct - // yaw component will be handled by the matching procedure in the - // FastCorrelativeScanMatcher, and the given yaw is essentially ignored. - constraint_builder_.MaybeAddGlobalConstraint( - submap_id, submap_data_.at(submap_id).submap.get(), node_id, - trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes, - initial_relative_pose.rotation()); + // In this situation, 'initial_relative_pose' is: + // + // submap <- global map 2 <- global map 1 <- tracking + // (agreeing on gravity) + // + // Since they possibly came from two disconnected trajectories, the only + // guaranteed connection between the tracking and the submap frames is + // an agreement on the direction of gravity. Therefore, excluding yaw, + // 'initial_relative_pose.rotation()' is a good estimate of the relative + // orientation of the point cloud in the submap frame. Finding the correct + // yaw component will be handled by the matching procedure in the + // FastCorrelativeScanMatcher, and the given yaw is essentially ignored. + constraint_builder_.MaybeAddGlobalConstraint( + submap_id, submap_data_.at(submap_id).submap.get(), node_id, + trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes, + initial_relative_pose.rotation()); } } @@ -327,10 +328,8 @@ void SparsePoseGraph::ComputeConstraintsForScan( } common::Time SparsePoseGraph::GetLatestScanTime( - const mapping::NodeId& node_id, - const mapping::SubmapId& submap_id) const { - common::Time time = - trajectory_nodes_.at(node_id).constant_data->time; + const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) const { + 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 = diff --git a/cartographer/transform/rigid_transform_test.cc b/cartographer/transform/rigid_transform_test.cc index c33e569..c1115e8 100644 --- a/cartographer/transform/rigid_transform_test.cc +++ b/cartographer/transform/rigid_transform_test.cc @@ -28,9 +28,7 @@ namespace { template class RigidTransformTest : public ::testing::Test { protected: - T eps() { - return std::numeric_limits::epsilon(); - } + T eps() { return std::numeric_limits::epsilon(); } Rigid2 GetRandomRigid2() { const T x = T(0.7) * distribution_(prng_);