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

Introduce TrajectoryConnectivityState. (#513) 

* Introduce TrajectoryConnectivityState.

This class will be used to track the connectivity state (including the
last connection time) between pairs of trajectories.
master
Christoph Schütte 2017-09-11 14:43:55 +02:00 committed by GitHub
parent 84da6d75bc
commit 24c2b499dd
9 changed files with 314 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
cartographer/mapping/connected_components.cc
View File

@ -60,7 +60,7 @@ int ConnectedComponents::FindSet(const int trajectory_id) {
}
bool ConnectedComponents::TransitivelyConnected(const int trajectory_id_a,
const int trajectory_id_b) {
const int trajectory_id_b) {
if (trajectory_id_a == trajectory_id_b) {
return true;
}
@ -91,8 +91,19 @@ std::vector<std::vector<int>> ConnectedComponents::Components() {
return result;
}
std::vector<int> ConnectedComponents::GetComponent(const int trajectory_id) {
const int set_id = FindSet(trajectory_id);
std::vector<int> trajectory_ids;
for (const auto& entry : forest_) {
if (FindSet(entry.first) == set_id) {
trajectory_ids.push_back(entry.first);
}
}
return trajectory_ids;
}
int ConnectedComponents::ConnectionCount(const int trajectory_id_a,
const int trajectory_id_b) {
const int trajectory_id_b) {
common::MutexLocker locker(&lock_);
const auto it =
connection_map_.find(std::minmax(trajectory_id_a, trajectory_id_b));

4
cartographer/mapping/connected_components.h
View File

@ -63,6 +63,10 @@ class ConnectedComponents {
// The trajectory IDs, grouped by connectivity.
std::vector<std::vector<int>> Components() EXCLUDES(lock_);
// The list of trajectory IDs that belong to the same connected component as
// 'trajectory_id'.
std::vector<int> GetComponent(int trajectory_id) EXCLUDES(lock_);
private:
// Find the representative and compresses the path to it.
int FindSet(int trajectory_id) REQUIRES(lock_);

74
cartographer/mapping/trajectory_connectivity_state.cc Normal file
View File

@ -0,0 +1,74 @@
/*
* Copyright 2017 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer/mapping/trajectory_connectivity_state.h"
namespace cartographer {
namespace mapping {
void TrajectoryConnectivityState::Add(const int trajectory_id) {
connected_components_.Add(trajectory_id);
}
void TrajectoryConnectivityState::Connect(const int trajectory_id_a,
const int trajectory_id_b,
const common::Time time) {
if (TransitivelyConnected(trajectory_id_a, trajectory_id_b)) {
// The trajectories are transitively connected, i.e. they belong to the same
// connected component. In this case we only update the last connection time
// of those two trajectories.
auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b);
if (last_connection_time_map_[sorted_pair] < time) {
last_connection_time_map_[sorted_pair] = time;
}
} else {
// The connection between these two trajectories is about to join to
// connected components. Here we update all bipartite trajectory pairs for
// 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);
for (const auto id_a : component_a) {
for (const auto id_b : component_b) {
auto id_pair = std::minmax(id_a, id_b);
last_connection_time_map_[id_pair] = time;
}
}
}
connected_components_.Connect(trajectory_id_a, trajectory_id_b);
}
bool TrajectoryConnectivityState::TransitivelyConnected(
const int trajectory_id_a,
const int trajectory_id_b) {
return connected_components_.TransitivelyConnected(trajectory_id_a,
trajectory_id_b);
}
std::vector<std::vector<int>> TrajectoryConnectivityState::Components() {
return connected_components_.Components();
}
common::Time TrajectoryConnectivityState::LastConnectionTime(
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];
}
} // namespace mapping
} // namespace cartographer

80
cartographer/mapping/trajectory_connectivity_state.h Normal file
View File

@ -0,0 +1,80 @@
/*
* Copyright 2017 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_CONNECTIVITY_STATE_H_
#define CARTOGRAPHER_MAPPING_TRAJECTORY_CONNECTIVITY_STATE_H_
#include "cartographer/common/time.h"
#include "cartographer/mapping/connected_components.h"
namespace cartographer {
namespace mapping {
// A class that tracks the connectivity state between trajectories. Compared to
// ConnectedComponents it tracks additionally the last time that a global
// constraint connected to trajectories.
//
// This class is thread-compatible.
class TrajectoryConnectivityState {
public:
TrajectoryConnectivityState() {}
TrajectoryConnectivityState(const TrajectoryConnectivityState&) = delete;
TrajectoryConnectivityState& operator=(const TrajectoryConnectivityState&) =
delete;
// Add a trajectory which is initially connected to only itself.
void Add(int trajectory_id);
// Connect two trajectories. If either trajectory is untracked, it will be
// 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);
// 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);
// The trajectory IDs, grouped by connectivity.
std::vector<std::vector<int>> Components();
// Return the last connection count between the two trajectories. If either of
// the trajectories is untracked or they have never been connected returns the
// beginning of time.
common::Time LastConnectionTime(int trajectory_id_a, int trajectory_id_b);
private:
ConnectedComponents connected_components_;
// Tracks the last time a direct connection between two trajectories has
// been added. The exception is when a connection between two trajectories
// connects two formerly unconnected connected components. In this case all
// 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
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_TRAJECTORY_CONNECTIVITY_STATE_H_

91
cartographer/mapping/trajectory_connectivity_state_test.cc Normal file
View File

@ -0,0 +1,91 @@
/*
* Copyright 2017 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer/mapping/trajectory_connectivity_state.h"
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping {
TEST(TrajectoryConnectivityStateTest, UnknownTrajectory) {
TrajectoryConnectivityState state;
state.Add(0);
// Nothing is transitively connected to an unknown trajectory.
EXPECT_FALSE(state.TransitivelyConnected(0, 1));
EXPECT_EQ(state.LastConnectionTime(0, 1), common::Time());
}
TEST(TrajectoryConnectivityStateTest, NotConnected) {
TrajectoryConnectivityState state;
state.Add(0);
state.Add(1);
EXPECT_FALSE(state.TransitivelyConnected(0, 1));
EXPECT_EQ(state.LastConnectionTime(0, 1), common::Time());
}
TEST(TrajectoryConnectivityStateTest, Connected) {
TrajectoryConnectivityState state;
state.Add(0);
state.Add(1);
state.Connect(0, 1, common::FromUniversal(123456));
EXPECT_TRUE(state.TransitivelyConnected(0, 1));
EXPECT_EQ(state.LastConnectionTime(0, 1), common::FromUniversal(123456));
}
TEST(TrajectoryConnectivityStateTest, UpdateConnectionTime) {
TrajectoryConnectivityState state;
state.Add(0);
state.Add(1);
state.Connect(0, 1, common::FromUniversal(123456));
state.Connect(0, 1, common::FromUniversal(234567));
EXPECT_TRUE(state.TransitivelyConnected(0, 1));
EXPECT_EQ(state.LastConnectionTime(0, 1), common::FromUniversal(234567));
// Connections with an earlier connection time do not update the last
// connection time.
state.Connect(0, 1, common::FromUniversal(123456));
EXPECT_EQ(state.LastConnectionTime(0, 1), common::FromUniversal(234567));
}
TEST(TrajectoryConnectivityStateTest, JoinTwoComponents) {
TrajectoryConnectivityState state;
state.Add(0);
state.Add(1);
state.Add(2);
state.Add(3);
state.Connect(0, 1, common::FromUniversal(123456));
state.Connect(2, 3, common::FromUniversal(123456));
// Connect the two disjoint connected components.
state.Connect(0, 2, common::FromUniversal(234567));
EXPECT_TRUE(state.TransitivelyConnected(0, 2));
EXPECT_TRUE(state.TransitivelyConnected(1, 3));
// All bipartite trajectory pairs between the two connected components should
// have the updated connection time.
EXPECT_EQ(state.LastConnectionTime(0, 2), common::FromUniversal(234567));
EXPECT_EQ(state.LastConnectionTime(0, 3), common::FromUniversal(234567));
EXPECT_EQ(state.LastConnectionTime(1, 3), common::FromUniversal(234567));
// A pair of trajectory IDs belonging to the same connected component should
// be unaffected.
EXPECT_EQ(state.LastConnectionTime(0, 1), common::FromUniversal(123456));
}
} // namespace cartographer
} // namespace mapping

32
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -104,7 +104,7 @@ void SparsePoseGraph::AddScan(
trajectory_nodes_.Append(
trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose});
++num_trajectory_nodes_;
connected_components_.Add(trajectory_id);
trajectory_connectivity_state_.Add(trajectory_id);
// Test if the 'insertion_submap.back()' is one we never saw before.
if (trajectory_id >= submap_data_.num_trajectories() ||
@ -180,7 +180,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
trajectory_nodes_.at(node_id).constant_data.get());
} else {
if (connected_components_.TransitivelyConnected(
if (trajectory_connectivity_state_.TransitivelyConnected(
node_id.trajectory_id, submap_id.trajectory_id)) {
const transform::Rigid2d initial_relative_pose =
optimization_problem_.submap_data()
@ -300,6 +300,22 @@ void SparsePoseGraph::ComputeConstraintsForScan(
}
}
void SparsePoseGraph::UpdateTrajectoryConnectivity(
const sparse_pose_graph::ConstraintBuilder::Result& result) {
for (const Constraint& constraint : result) {
CHECK_EQ(constraint.tag,
mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
mapping::NodeId last_submap_node_id =
*submap_data_.at(constraint.submap_id).node_ids.rbegin();
common::Time time =
std::max(trajectory_nodes_.at(constraint.node_id).constant_data->time,
trajectory_nodes_.at(last_submap_node_id).constant_data->time);
trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
constraint.submap_id.trajectory_id,
time);
}
}
void SparsePoseGraph::HandleWorkQueue() {
constraint_builder_.WhenDone(
[this](const sparse_pose_graph::ConstraintBuilder::Result& result) {
@ -308,15 +324,7 @@ void SparsePoseGraph::HandleWorkQueue() {
constraints_.insert(constraints_.end(), result.begin(), result.end());
}
RunOptimization();
// Update the trajectory connectivity structure with the new
// constraints.
for (const Constraint& constraint : result) {
CHECK_EQ(constraint.tag,
mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
connected_components_.Connect(constraint.node_id.trajectory_id,
constraint.submap_id.trajectory_id);
}
UpdateTrajectoryConnectivity(result);
common::MutexLocker locker(&mutex_);
num_scans_since_last_loop_closure_ = 0;
@ -503,7 +511,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
}
std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
return connected_components_.Components();
return trajectory_connectivity_state_.Components();
}
int SparsePoseGraph::num_submaps(const int trajectory_id) {

8
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -32,9 +32,9 @@
#include "cartographer/common/mutex.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/connected_components.h"
#include "cartographer/mapping/pose_graph_trimmer.h"
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/trajectory_connectivity_state.h"
#include "cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h"
#include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h"
#include "cartographer/mapping_2d/submaps.h"
@ -153,6 +153,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// optimization being run at a time.
void RunOptimization() EXCLUDES(mutex_);
// Updates the trajectory connectivity structure with the new constraints.
void UpdateTrajectoryConnectivity(
const sparse_pose_graph::ConstraintBuilder::Result& result);
// Computes the local to global frame transform based on the given optimized
// 'submap_transforms'.
transform::Rigid3d ComputeLocalToGlobalTransform(
@ -172,7 +176,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
GUARDED_BY(mutex_);
// How our various trajectories are related.
mapping::ConnectedComponents connected_components_;
mapping::TrajectoryConnectivityState trajectory_connectivity_state_;
// We globally localize a fraction of the scans from each trajectory.
std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>

32
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -103,7 +103,7 @@ void SparsePoseGraph::AddScan(
trajectory_nodes_.Append(
trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose});
++num_trajectory_nodes_;
connected_components_.Add(trajectory_id);
trajectory_connectivity_state_.Add(trajectory_id);
// Test if the 'insertion_submap.back()' is one we never saw before.
if (trajectory_id >= submap_data_.num_trajectories() ||
@ -213,7 +213,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
initial_relative_pose.rotation());
} else {
if (connected_components_.TransitivelyConnected(
if (trajectory_connectivity_state_.TransitivelyConnected(
node_id.trajectory_id, submap_id.trajectory_id)) {
constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
@ -320,6 +320,22 @@ void SparsePoseGraph::ComputeConstraintsForScan(
}
}
void SparsePoseGraph::UpdateTrajectoryConnectivity(
const sparse_pose_graph::ConstraintBuilder::Result& result) {
for (const Constraint& constraint : result) {
CHECK_EQ(constraint.tag,
mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
const mapping::NodeId last_submap_node_id =
*submap_data_.at(constraint.submap_id).node_ids.rbegin();
const common::Time time =
std::max(trajectory_nodes_.at(constraint.node_id).constant_data->time,
trajectory_nodes_.at(last_submap_node_id).constant_data->time);
trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
constraint.submap_id.trajectory_id,
time);
}
}
void SparsePoseGraph::HandleWorkQueue() {
constraint_builder_.WhenDone(
[this](const sparse_pose_graph::ConstraintBuilder::Result& result) {
@ -328,15 +344,7 @@ void SparsePoseGraph::HandleWorkQueue() {
constraints_.insert(constraints_.end(), result.begin(), result.end());
}
RunOptimization();
// Update the trajectory connectivity structure with the new
// constraints.
for (const Constraint& constraint : result) {
CHECK_EQ(constraint.tag,
mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
connected_components_.Connect(constraint.node_id.trajectory_id,
constraint.submap_id.trajectory_id);
}
UpdateTrajectoryConnectivity(result);
common::MutexLocker locker(&mutex_);
num_scans_since_last_loop_closure_ = 0;
@ -542,7 +550,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
}
std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
return connected_components_.Components();
return trajectory_connectivity_state_.Components();
}
int SparsePoseGraph::num_submaps(const int trajectory_id) {

8
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -32,9 +32,9 @@
#include "cartographer/common/mutex.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/connected_components.h"
#include "cartographer/mapping/pose_graph_trimmer.h"
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/trajectory_connectivity_state.h"
#include "cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h"
#include "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h"
#include "cartographer/mapping_3d/submaps.h"
@ -168,6 +168,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// poses.
void LogResidualHistograms() REQUIRES(mutex_);
// Updates the trajectory connectivity structure with the new constraints.
void UpdateTrajectoryConnectivity(
const sparse_pose_graph::ConstraintBuilder::Result& result);
const mapping::proto::SparsePoseGraphOptions options_;
common::Mutex mutex_;
@ -177,7 +181,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
GUARDED_BY(mutex_);
// How our various trajectories are related.
mapping::ConnectedComponents connected_components_;
mapping::TrajectoryConnectivityState trajectory_connectivity_state_;
// We globally localize a fraction of the scans from each trajectory.
std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>