12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix formatting. (#561)

master
Wolfgang Hess 2017-10-04 15:58:48 +02:00 committed by GitHub
parent aaaf5ac546
commit 51f5a18462
10 changed files with 55 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer/mapping/connected_components.cc
View File

@ -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);

2
cartographer/mapping/connected_components_test.cc
View File

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

6
cartographer/mapping/pose_extrapolator.cc
View File

@ -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 =

12
cartographer/mapping/trajectory_connectivity_state.cc
View File

@ -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_b = connected_components_.GetComponent(trajectory_id_b);
std::vector<int> component_a =
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_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<std::vector<int>> 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];
}

8
cartographer/mapping/trajectory_connectivity_state.h
View File

@ -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<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

2
cartographer/mapping/trajectory_connectivity_state_test.cc
View File

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

29
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -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, &notification](
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, &notification](
const sparse_pose_graph::ConstraintBuilder::Result& result) {
common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end());
notification = true;
});
locker.Await([&notification]() { return notification; });
}

9
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc
View File

@ -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);
});
}

43
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -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 =

4
cartographer/transform/rigid_transform_test.cc
View File

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