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

Make TrajectoryConnectivity use trajectory IDs. (#275) 

Related to #256.
master
Holger Rapp 2017-05-11 13:36:12 +02:00 committed by GitHub
parent 95f0d013c6
commit e6a6bab351
13 changed files with 172 additions and 235 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cartographer/mapping/map_builder.cc
View File

@ -113,8 +113,7 @@ int MapBuilder::GetTrajectoryId(const Submaps* const trajectory) const {
} }
proto::TrajectoryConnectivity MapBuilder::GetTrajectoryConnectivity() { proto::TrajectoryConnectivity MapBuilder::GetTrajectoryConnectivity() {
return ToProto(sparse_pose_graph_->GetConnectedTrajectories(), return ToProto(sparse_pose_graph_->GetConnectedTrajectories());
trajectory_ids_);
} }
string MapBuilder::SubmapToProto(const int trajectory_id, string MapBuilder::SubmapToProto(const int trajectory_id,

3
cartographer/mapping/sparse_pose_graph.h
View File

@ -82,8 +82,7 @@ class SparsePoseGraph {
virtual void RunFinalOptimization() = 0; virtual void RunFinalOptimization() = 0;
// Get the current trajectory clusters. // Get the current trajectory clusters.
virtual std::vector<std::vector<const Submaps*>> virtual std::vector<std::vector<int>> GetConnectedTrajectories() = 0;
GetConnectedTrajectories() = 0;
// Returns the current optimized transforms for the given 'trajectory'. // Returns the current optimized transforms for the given 'trajectory'.
virtual std::vector<transform::Rigid3d> GetSubmapTransforms( virtual std::vector<transform::Rigid3d> GetSubmapTransforms(

84
cartographer/mapping/trajectory_connectivity.cc
View File

@ -28,35 +28,30 @@ namespace mapping {
TrajectoryConnectivity::TrajectoryConnectivity() TrajectoryConnectivity::TrajectoryConnectivity()
: lock_(), forest_(), connection_map_() {} : lock_(), forest_(), connection_map_() {}
void TrajectoryConnectivity::Add(const Submaps* trajectory) { void TrajectoryConnectivity::Add(const int trajectory_id) {
CHECK(trajectory != nullptr);
common::MutexLocker locker(&lock_); common::MutexLocker locker(&lock_);
forest_.emplace(trajectory, trajectory); forest_.emplace(trajectory_id, trajectory_id);
} }
void TrajectoryConnectivity::Connect(const Submaps* trajectory_a, void TrajectoryConnectivity::Connect(const int trajectory_id_a,
const Submaps* trajectory_b) { const int trajectory_id_b) {
CHECK(trajectory_a != nullptr);
CHECK(trajectory_b != nullptr);
common::MutexLocker locker(&lock_); common::MutexLocker locker(&lock_);
Union(trajectory_a, trajectory_b); Union(trajectory_id_a, trajectory_id_b);
auto sorted_pair = auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b);
std::minmax(trajectory_a, trajectory_b, std::less<const Submaps*>());
++connection_map_[sorted_pair]; ++connection_map_[sorted_pair];
} }
void TrajectoryConnectivity::Union(const Submaps* const trajectory_a, void TrajectoryConnectivity::Union(const int trajectory_id_a,
const Submaps* const trajectory_b) { const int trajectory_id_b) {
forest_.emplace(trajectory_a, trajectory_a); forest_.emplace(trajectory_id_a, trajectory_id_a);
forest_.emplace(trajectory_b, trajectory_b); forest_.emplace(trajectory_id_b, trajectory_id_b);
const Submaps* const representative_a = FindSet(trajectory_a); const int representative_a = FindSet(trajectory_id_a);
const Submaps* const representative_b = FindSet(trajectory_b); const int representative_b = FindSet(trajectory_id_b);
forest_[representative_a] = representative_b; forest_[representative_a] = representative_b;
} }
const Submaps* TrajectoryConnectivity::FindSet( int TrajectoryConnectivity::FindSet(const int trajectory_id) {
const Submaps* const trajectory) { auto it = forest_.find(trajectory_id);
auto it = forest_.find(trajectory);
CHECK(it != forest_.end()); CHECK(it != forest_.end());
if (it->first != it->second) { if (it->first != it->second) {
it->second = FindSet(it->second); it->second = FindSet(it->second);
@ -64,28 +59,27 @@ const Submaps* TrajectoryConnectivity::FindSet(
return it->second; return it->second;
} }
bool TrajectoryConnectivity::TransitivelyConnected( bool TrajectoryConnectivity::TransitivelyConnected(const int trajectory_id_a,
const Submaps* trajectory_a, const Submaps* trajectory_b) { const int trajectory_id_b) {
CHECK(trajectory_a != nullptr);
CHECK(trajectory_b != nullptr);
common::MutexLocker locker(&lock_); common::MutexLocker locker(&lock_);
if (forest_.count(trajectory_a) == 0 || forest_.count(trajectory_b) == 0) { if (forest_.count(trajectory_id_a) == 0 ||
forest_.count(trajectory_id_b) == 0) {
return false; return false;
} }
return FindSet(trajectory_a) == FindSet(trajectory_b); return FindSet(trajectory_id_a) == FindSet(trajectory_id_b);
} }
std::vector<std::vector<const Submaps*>> std::vector<std::vector<int>> TrajectoryConnectivity::ConnectedComponents() {
TrajectoryConnectivity::ConnectedComponents() {
// Map from cluster exemplar -> growing cluster. // Map from cluster exemplar -> growing cluster.
std::unordered_map<const Submaps*, std::vector<const Submaps*>> map; std::unordered_map<int, std::vector<int>> map;
common::MutexLocker locker(&lock_); common::MutexLocker locker(&lock_);
for (const auto& trajectory_entry : forest_) { for (const auto& trajectory_id_entry : forest_) {
map[FindSet(trajectory_entry.first)].push_back(trajectory_entry.first); map[FindSet(trajectory_id_entry.first)].push_back(
trajectory_id_entry.first);
} }
std::vector<std::vector<const Submaps*>> result; std::vector<std::vector<int>> result;
result.reserve(map.size()); result.reserve(map.size());
for (auto& pair : map) { for (auto& pair : map) {
result.emplace_back(std::move(pair.second)); result.emplace_back(std::move(pair.second));
@ -93,33 +87,23 @@ TrajectoryConnectivity::ConnectedComponents() {
return result; return result;
} }
int TrajectoryConnectivity::ConnectionCount(const Submaps* trajectory_a, int TrajectoryConnectivity::ConnectionCount(const int trajectory_id_a,
const Submaps* trajectory_b) { const int trajectory_id_b) {
CHECK(trajectory_a != nullptr);
CHECK(trajectory_b != nullptr);
common::MutexLocker locker(&lock_); common::MutexLocker locker(&lock_);
const auto it = connection_map_.find( const auto it =
std::minmax(trajectory_a, trajectory_b, std::less<const Submaps*>())); connection_map_.find(std::minmax(trajectory_id_a, trajectory_id_b));
return it != connection_map_.end() ? it->second : 0; return it != connection_map_.end() ? it->second : 0;
} }
proto::TrajectoryConnectivity ToProto( proto::TrajectoryConnectivity ToProto(
std::vector<std::vector<const Submaps*>> connected_components, std::vector<std::vector<int>> connected_components) {
std::unordered_map<const mapping::Submaps*, int> trajectory_indices) {
proto::TrajectoryConnectivity proto; proto::TrajectoryConnectivity proto;
std::vector<std::vector<int>> connected_components_by_indices;
for (const auto& connected_component : connected_components) { for (const auto& connected_component : connected_components) {
connected_components_by_indices.emplace_back(); std::sort(connected_components.back().begin(),
for (const mapping::Submaps* trajectory : connected_component) { connected_components.back().end());
connected_components_by_indices.back().push_back(
trajectory_indices.at(trajectory));
} }
std::sort(connected_components_by_indices.back().begin(), std::sort(connected_components.begin(), connected_components.end());
connected_components_by_indices.back().end()); for (const auto& connected_component : connected_components) {
}
std::sort(connected_components_by_indices.begin(),
connected_components_by_indices.end());
for (const auto& connected_component : connected_components_by_indices) {
auto* proto_connected_component = proto.add_connected_component(); auto* proto_connected_component = proto.add_connected_component();
for (const int trajectory_id : connected_component) { for (const int trajectory_id : connected_component) {
proto_connected_component->add_trajectory_id(trajectory_id); proto_connected_component->add_trajectory_id(trajectory_id);

39
cartographer/mapping/trajectory_connectivity.h
View File

@ -41,50 +41,43 @@ class TrajectoryConnectivity {
TrajectoryConnectivity& operator=(const TrajectoryConnectivity&) = delete; TrajectoryConnectivity& operator=(const TrajectoryConnectivity&) = delete;
// Add a trajectory which is initially connected to nothing. // Add a trajectory which is initially connected to nothing.
void Add(const Submaps* trajectory) EXCLUDES(lock_); void Add(int trajectory_id) EXCLUDES(lock_);
// Connect two trajectories. If either trajectory is untracked, it will be // Connect two trajectories. If either trajectory is untracked, it will be
// tracked. This function is invariant to the order of its arguments. Repeated // tracked. This function is invariant to the order of its arguments. Repeated
// calls to Connect increment the connectivity count. // calls to Connect increment the connectivity count.
void Connect(const Submaps* trajectory_a, const Submaps* trajectory_b) void Connect(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_);
EXCLUDES(lock_);
// Determines if two trajectories have been (transitively) connected. If // Determines if two trajectories have been (transitively) connected. If
// either trajectory is not being tracked, returns false. This function is // either trajectory is not being tracked, returns false. This function is
// invariant to the order of its arguments. // invariant to the order of its arguments.
bool TransitivelyConnected(const Submaps* trajectory_a, bool TransitivelyConnected(int trajectory_id_a, int trajectory_id_b)
const Submaps* trajectory_b) EXCLUDES(lock_);
// Return the number of _direct_ connections between trajectory_a and
// trajectory_b. If either trajectory is not being tracked, returns 0. This
// function is invariant to the order of its arguments.
int ConnectionCount(const Submaps* trajectory_a, const Submaps* trajectory_b)
EXCLUDES(lock_); EXCLUDES(lock_);
// The trajectories, grouped by connectivity. // Return the number of _direct_ connections between 'trajectory_id_a' and
std::vector<std::vector<const Submaps*>> ConnectedComponents() // 'trajectory_id_b'. If either trajectory is not being tracked, returns 0.
EXCLUDES(lock_); // This function is invariant to the order of its arguments.
int ConnectionCount(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_);
// The trajectory IDs, grouped by connectivity.
std::vector<std::vector<int>> ConnectedComponents() EXCLUDES(lock_);
private: private:
// Find the representative and compresses the path to it. // Find the representative and compresses the path to it.
const Submaps* FindSet(const Submaps* trajectory) REQUIRES(lock_); int FindSet(int trajectory_id) REQUIRES(lock_);
void Union(const Submaps* trajectory_a, const Submaps* trajectory_b) void Union(int trajectory_id_a, int trajectory_id_b) REQUIRES(lock_);
REQUIRES(lock_);
common::Mutex lock_; common::Mutex lock_;
// Tracks transitive connectivity using a disjoint set forest, i.e. each // Tracks transitive connectivity using a disjoint set forest, i.e. each
// entry points towards the representative for the given trajectory. // entry points towards the representative for the given trajectory.
std::map<const Submaps*, const Submaps*> forest_ GUARDED_BY(lock_); std::map<int, int> forest_ GUARDED_BY(lock_);
// Tracks the number of direct connections between a pair of trajectories. // Tracks the number of direct connections between a pair of trajectories.
std::map<std::pair<const Submaps*, const Submaps*>, int> connection_map_ std::map<std::pair<int, int>, int> connection_map_ GUARDED_BY(lock_);
GUARDED_BY(lock_);
}; };
// Returns a proto encoding connected components according to // Returns a proto encoding connected components.
// 'trajectory_indices'.
proto::TrajectoryConnectivity ToProto( proto::TrajectoryConnectivity ToProto(
std::vector<std::vector<const Submaps*>> connected_components, std::vector<std::vector<int>> connected_components);
std::unordered_map<const mapping::Submaps*, int> trajectory_indices);
// Returns the connected component containing 'trajectory_index'. // Returns the connected component containing 'trajectory_index'.
proto::TrajectoryConnectivity::ConnectedComponent FindConnectedComponent( proto::TrajectoryConnectivity::ConnectedComponent FindConnectedComponent(

97
cartographer/mapping/trajectory_connectivity_test.cc
View File

@ -20,88 +20,60 @@
#include <memory> #include <memory>
#include <vector> #include <vector>
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
#include "cartographer/mapping_2d/submaps.h"
#include "gtest/gtest.h" #include "gtest/gtest.h"
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
namespace { namespace {
class TrajectoryConnectivityTest : public ::testing::Test { constexpr int kNumTrajectories = 10;
protected:
TrajectoryConnectivityTest() {
for (int i = 0; i < 10; ++i) {
auto parameter_dictionary = common::MakeDictionary(R"text(
return {
resolution = 0.05,
half_length = 10.,
num_range_data = 10,
output_debug_images = false,
range_data_inserter = {
insert_free_space = true,
hit_probability = 0.53,
miss_probability = 0.495,
},
})text");
trajectories_.emplace_back(new mapping_2d::Submaps(
mapping_2d::CreateSubmapsOptions(parameter_dictionary.get())));
}
}
// Helper function to avoid .get() noise. TEST(TrajectoryConnectivityTest, TransitivelyConnected) {
const Submaps* trajectory(int index) { return trajectories_.at(index).get(); } TrajectoryConnectivity trajectory_connectivity;
TrajectoryConnectivity trajectory_connectivity_;
std::vector<std::unique_ptr<const Submaps>> trajectories_;
};
TEST_F(TrajectoryConnectivityTest, TransitivelyConnected) {
// Make sure nothing's connected until we connect some things. // Make sure nothing's connected until we connect some things.
for (auto& trajectory_a : trajectories_) { for (int trajectory_a; trajectory_a < kNumTrajectories; ++trajectory_a) {
for (auto& trajectory_b : trajectories_) { for (int trajectory_b; trajectory_b < kNumTrajectories; ++trajectory_b) {
EXPECT_FALSE(trajectory_connectivity_.TransitivelyConnected( EXPECT_FALSE(trajectory_connectivity.TransitivelyConnected(trajectory_a,
trajectory_a.get(), trajectory_b.get())); trajectory_b));
} }
} }
// Connect some stuff up. // Connect some stuff up.
trajectory_connectivity_.Connect(trajectory(0), trajectory(1)); trajectory_connectivity.Connect(0, 1);
EXPECT_TRUE(trajectory_connectivity_.TransitivelyConnected(trajectory(0), EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(0, 1));
trajectory(1))); trajectory_connectivity.Connect(8, 9);
trajectory_connectivity_.Connect(trajectory(8), trajectory(9)); EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(8, 9));
EXPECT_TRUE(trajectory_connectivity_.TransitivelyConnected(trajectory(8), EXPECT_FALSE(trajectory_connectivity.TransitivelyConnected(0, 9));
trajectory(9)));
EXPECT_FALSE(trajectory_connectivity_.TransitivelyConnected(trajectory(0),
trajectory(9)));
trajectory_connectivity_.Connect(trajectory(1), trajectory(8)); trajectory_connectivity.Connect(1, 8);
for (int i : {0, 1}) { for (int i : {0, 1}) {
for (int j : {8, 9}) { for (int j : {8, 9}) {
EXPECT_TRUE(trajectory_connectivity_.TransitivelyConnected( EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(i, j));
trajectory(i), trajectory(j)));
} }
} }
} }
TEST_F(TrajectoryConnectivityTest, EmptyConnectedComponents) { TEST(TrajectoryConnectivityTest, EmptyConnectedComponents) {
auto connections = trajectory_connectivity_.ConnectedComponents(); TrajectoryConnectivity trajectory_connectivity;
auto connections = trajectory_connectivity.ConnectedComponents();
EXPECT_EQ(0, connections.size()); EXPECT_EQ(0, connections.size());
} }
TEST_F(TrajectoryConnectivityTest, ConnectedComponents) { TEST(TrajectoryConnectivityTest, ConnectedComponents) {
TrajectoryConnectivity trajectory_connectivity;
for (int i = 0; i <= 4; ++i) { for (int i = 0; i <= 4; ++i) {
trajectory_connectivity_.Connect(trajectory(0), trajectory(i)); trajectory_connectivity.Connect(0, i);
} }
for (int i = 5; i <= 9; ++i) { for (int i = 5; i <= 9; ++i) {
trajectory_connectivity_.Connect(trajectory(5), trajectory(i)); trajectory_connectivity.Connect(5, i);
} }
auto connections = trajectory_connectivity_.ConnectedComponents(); auto connections = trajectory_connectivity.ConnectedComponents();
ASSERT_EQ(2, connections.size()); ASSERT_EQ(2, connections.size());
// The clustering is arbitrary; we need to figure out which one is which. // The clustering is arbitrary; we need to figure out which one is which.
const std::vector<const Submaps*>* zero_cluster = nullptr; const std::vector<int>* zero_cluster = nullptr;
const std::vector<const Submaps*>* five_cluster = nullptr; const std::vector<int>* five_cluster = nullptr;
if (std::find(connections[0].begin(), connections[0].end(), trajectory(0)) != if (std::find(connections[0].begin(), connections[0].end(), 0) !=
connections[0].end()) { connections[0].end()) {
zero_cluster = &connections[0]; zero_cluster = &connections[0];
five_cluster = &connections[1]; five_cluster = &connections[1];
@ -111,22 +83,21 @@ TEST_F(TrajectoryConnectivityTest, ConnectedComponents) {
} }
for (int i = 0; i <= 9; ++i) { for (int i = 0; i <= 9; ++i) {
EXPECT_EQ(i <= 4, std::find(zero_cluster->begin(), zero_cluster->end(), EXPECT_EQ(i <= 4, std::find(zero_cluster->begin(), zero_cluster->end(),
trajectory(i)) != zero_cluster->end()); i) != zero_cluster->end());
EXPECT_EQ(i > 4, std::find(five_cluster->begin(), five_cluster->end(), EXPECT_EQ(i > 4, std::find(five_cluster->begin(), five_cluster->end(), i) !=
trajectory(i)) != five_cluster->end()); five_cluster->end());
} }
} }
TEST_F(TrajectoryConnectivityTest, ConnectionCount) { TEST(TrajectoryConnectivityTest, ConnectionCount) {
for (int i = 0; i < 10; ++i) { TrajectoryConnectivity trajectory_connectivity;
trajectory_connectivity_.Connect(trajectory(0), trajectory(1)); for (int i = 0; i < kNumTrajectories; ++i) {
trajectory_connectivity.Connect(0, 1);
// Permute the arguments to check invariance. // Permute the arguments to check invariance.
EXPECT_EQ(i + 1, trajectory_connectivity_.ConnectionCount(trajectory(1), EXPECT_EQ(i + 1, trajectory_connectivity.ConnectionCount(1, 0));
trajectory(0)));
} }
for (int i = 1; i < 9; ++i) { for (int i = 1; i < 9; ++i) {
EXPECT_EQ(0, trajectory_connectivity_.ConnectionCount(trajectory(i), EXPECT_EQ(0, trajectory_connectivity.ConnectionCount(i, i + 1));
trajectory(i + 1)));
} }
} }

43
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -99,6 +99,7 @@ void SparsePoseGraph::AddScan(
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
trajectory_ids_.emplace(trajectory, trajectory_ids_.size()); trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
const int trajectory_id = trajectory_ids_.at(trajectory);
const int j = trajectory_nodes_.size(); const int j = trajectory_nodes_.size();
CHECK_LT(j, std::numeric_limits<int>::max()); CHECK_LT(j, std::numeric_limits<int>::max());
@ -109,16 +110,15 @@ void SparsePoseGraph::AddScan(
trajectory_nodes_.push_back(mapping::TrajectoryNode{ trajectory_nodes_.push_back(mapping::TrajectoryNode{
&constant_node_data_.back(), optimized_pose, &constant_node_data_.back(), optimized_pose,
}); });
trajectory_connectivity_.Add(trajectory); trajectory_connectivity_.Add(trajectory_id);
if (submap_indices_.count(insertion_submaps.back()) == 0) { if (submap_indices_.count(insertion_submaps.back()) == 0) {
submap_indices_.emplace(insertion_submaps.back(), submap_indices_.emplace(insertion_submaps.back(),
static_cast<int>(submap_indices_.size())); static_cast<int>(submap_indices_.size()));
submap_states_.emplace_back(); submap_states_.emplace_back();
submap_states_.back().submap = insertion_submaps.back(); submap_states_.back().submap = insertion_submaps.back();
submap_states_.back().trajectory = trajectory;
submap_states_.back().id = mapping::SubmapId{ submap_states_.back().id = mapping::SubmapId{
trajectory_ids_.at(trajectory), num_submaps_in_trajectory_[trajectory]}; trajectory_id, num_submaps_in_trajectory_[trajectory]};
++num_submaps_in_trajectory_[trajectory]; ++num_submaps_in_trajectory_[trajectory];
CHECK_EQ(submap_states_.size(), submap_indices_.size()); CHECK_EQ(submap_states_.size(), submap_indices_.size());
} }
@ -172,23 +172,24 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
const mapping::Submaps* const scan_trajectory = const mapping::Submaps* const scan_trajectory =
trajectory_nodes_[scan_index].constant_data->trajectory; trajectory_nodes_[scan_index].constant_data->trajectory;
const mapping::Submaps* const submap_trajectory = const int scan_trajectory_id = trajectory_ids_.at(scan_trajectory);
submap_states_[submap_index].trajectory; const int submap_trajectory_id =
submap_states_[submap_index].id.trajectory_id;
// Only globally match against submaps not in this trajectory. // Only globally match against submaps not in this trajectory.
if (scan_trajectory != submap_trajectory && if (scan_trajectory_id != submap_trajectory_id &&
global_localization_samplers_[scan_trajectory]->Pulse()) { global_localization_samplers_[scan_trajectory]->Pulse()) {
constraint_builder_.MaybeAddGlobalConstraint( constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap_states_[submap_index].submap, scan_index, submap_id, submap_states_[submap_index].submap, scan_index,
scan_trajectory, submap_trajectory, &trajectory_connectivity_, scan_trajectory_id, &trajectory_connectivity_,
&trajectory_nodes_[scan_index].constant_data->range_data_2d.returns); &trajectory_nodes_[scan_index].constant_data->range_data_2d.returns);
} else { } else {
const bool scan_and_submap_trajectories_connected = const bool scan_and_submap_trajectories_connected =
reverse_connected_components_.count(scan_trajectory) > 0 && reverse_connected_components_.count(scan_trajectory_id) > 0 &&
reverse_connected_components_.count(submap_trajectory) > 0 && reverse_connected_components_.count(submap_trajectory_id) > 0 &&
reverse_connected_components_.at(scan_trajectory) == reverse_connected_components_.at(scan_trajectory_id) ==
reverse_connected_components_.at(submap_trajectory); reverse_connected_components_.at(submap_trajectory_id);
if (scan_trajectory == submap_trajectory || if (scan_trajectory_id == submap_trajectory_id ||
scan_and_submap_trajectories_connected) { scan_and_submap_trajectories_connected) {
constraint_builder_.MaybeAddConstraint( constraint_builder_.MaybeAddConstraint(
submap_id, submap_states_[submap_index].submap, scan_index, submap_id, submap_states_[submap_index].submap, scan_index,
@ -386,8 +387,8 @@ void SparsePoseGraph::RunOptimization() {
connected_components_ = trajectory_connectivity_.ConnectedComponents(); connected_components_ = trajectory_connectivity_.ConnectedComponents();
reverse_connected_components_.clear(); reverse_connected_components_.clear();
for (size_t i = 0; i != connected_components_.size(); ++i) { for (size_t i = 0; i != connected_components_.size(); ++i) {
for (const auto* trajectory : connected_components_[i]) { for (const int trajectory_id : connected_components_[i]) {
reverse_connected_components_.emplace(trajectory, i); reverse_connected_components_.emplace(trajectory_id, i);
} }
} }
} }
@ -417,8 +418,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
.inverse(); .inverse();
} }
std::vector<std::vector<const mapping::Submaps*>> std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
SparsePoseGraph::GetConnectedTrajectories() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return connected_components_; return connected_components_;
} }
@ -433,17 +433,20 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
const std::vector<std::vector<sparse_pose_graph::SubmapData>>& const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
submap_transforms, submap_transforms,
const mapping::Submaps* const trajectory) const { const mapping::Submaps* const trajectory) const {
std::vector<transform::Rigid3d> result; if (trajectory_ids_.count(trajectory) == 0) {
return {transform::Rigid3d::Identity()};
}
const int trajectory_id = trajectory_ids_.at(trajectory);
size_t flat_index = 0; size_t flat_index = 0;
size_t flat_index_of_result_back = -1; size_t flat_index_of_result_back = -1;
// Submaps for which we have optimized poses. // Submaps for which we have optimized poses.
std::vector<transform::Rigid3d> result;
for (; flat_index != submap_states_.size(); ++flat_index) { for (; flat_index != submap_states_.size(); ++flat_index) {
const auto& state = submap_states_[flat_index]; const auto& state = submap_states_[flat_index];
if (state.trajectory != trajectory) { if (state.id.trajectory_id != trajectory_id) {
continue; continue;
} }
const int trajectory_id = trajectory_ids_.at(trajectory);
if (static_cast<size_t>(trajectory_id) >= submap_transforms.size() || if (static_cast<size_t>(trajectory_id) >= submap_transforms.size() ||
result.size() >= submap_transforms.at(trajectory_id).size()) { result.size() >= submap_transforms.at(trajectory_id).size()) {
break; break;
@ -456,7 +459,7 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
// Extrapolate to the remaining submaps. // Extrapolate to the remaining submaps.
for (; flat_index != submap_states_.size(); ++flat_index) { for (; flat_index != submap_states_.size(); ++flat_index) {
const auto& state = submap_states_[flat_index]; const auto& state = submap_states_[flat_index];
if (state.trajectory != trajectory) { if (state.id.trajectory_id != trajectory_id) {
continue; continue;
} }
if (result.empty()) { if (result.empty()) {

14
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -83,8 +83,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const Eigen::Vector3d& angular_velocity); const Eigen::Vector3d& angular_velocity);
void RunFinalOptimization() override; void RunFinalOptimization() override;
std::vector<std::vector<const mapping::Submaps*>> GetConnectedTrajectories() std::vector<std::vector<int>> GetConnectedTrajectories() override;
override;
std::vector<transform::Rigid3d> GetSubmapTransforms( std::vector<transform::Rigid3d> GetSubmapTransforms(
const mapping::Submaps* trajectory) EXCLUDES(mutex_) override; const mapping::Submaps* trajectory) EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps) transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps)
@ -112,9 +111,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// which are finished. // which are finished.
bool finished = false; bool finished = false;
// The trajectory to which this SubmapState belongs.
const mapping::Submaps* trajectory = nullptr;
mapping::SubmapId id; mapping::SubmapId id;
}; };
@ -205,10 +201,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
std::map<const mapping::Submaps*, int> num_submaps_in_trajectory_ std::map<const mapping::Submaps*, int> num_submaps_in_trajectory_
GUARDED_BY(mutex_); GUARDED_BY(mutex_);
// Connectivity structure of our trajectories. // Connectivity structure of our trajectories by IDs.
std::vector<std::vector<const mapping::Submaps*>> connected_components_; std::vector<std::vector<int>> connected_components_;
// Trajectory to connected component ID. // Trajectory ID to connected component ID.
std::map<const mapping::Submaps*, size_t> reverse_connected_components_; std::map<int, size_t> reverse_connected_components_;
// Data that are currently being shown. // Data that are currently being shown.
// //

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

@ -78,8 +78,7 @@ void ConstraintBuilder::MaybeAddConstraint(
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) { submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, scan_index, ComputeConstraint(submap_id, submap, scan_index,
nullptr, /* scan_trajectory */ -1, /* scan_trajectory_id */
nullptr, /* submap_trajectory */
false, /* match_full_submap */ false, /* match_full_submap */
nullptr, /* trajectory_connectivity */ nullptr, /* trajectory_connectivity */
point_cloud, initial_relative_pose, constraint); point_cloud, initial_relative_pose, constraint);
@ -90,8 +89,7 @@ void ConstraintBuilder::MaybeAddConstraint(
void ConstraintBuilder::MaybeAddGlobalConstraint( void ConstraintBuilder::MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* const submap, const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
const int scan_index, const mapping::Submaps* scan_trajectory, const int scan_index, const int scan_trajectory_id,
const mapping::Submaps* submap_trajectory,
mapping::TrajectoryConnectivity* trajectory_connectivity, mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud* const point_cloud) { const sensor::PointCloud* const point_cloud) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
@ -102,8 +100,8 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
const int current_computation = current_computation_; const int current_computation = current_computation_;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) { submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, scan_index, submap_trajectory, ComputeConstraint(submap_id, submap, scan_index, scan_trajectory_id,
scan_trajectory, true, /* match_full_submap */ true, /* match_full_submap */
trajectory_connectivity, point_cloud, trajectory_connectivity, point_cloud,
transform::Rigid2d::Identity(), constraint); transform::Rigid2d::Identity(), constraint);
FinishComputation(current_computation); FinishComputation(current_computation);
@ -168,8 +166,7 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
void ConstraintBuilder::ComputeConstraint( void ConstraintBuilder::ComputeConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* const submap, const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
const int scan_index, const mapping::Submaps* scan_trajectory, const int scan_index, const int scan_trajectory_id, bool match_full_submap,
const mapping::Submaps* submap_trajectory, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity, mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud* const point_cloud, const sensor::PointCloud* const point_cloud,
const transform::Rigid2d& initial_relative_pose, const transform::Rigid2d& initial_relative_pose,
@ -194,7 +191,10 @@ void ConstraintBuilder::ComputeConstraint(
filtered_point_cloud, options_.global_localization_min_score(), filtered_point_cloud, options_.global_localization_min_score(),
&score, &pose_estimate)) { &score, &pose_estimate)) {
CHECK_GT(score, options_.global_localization_min_score()); CHECK_GT(score, options_.global_localization_min_score());
trajectory_connectivity->Connect(scan_trajectory, submap_trajectory); CHECK_GE(scan_trajectory_id, 0);
CHECK_GE(submap_id.trajectory_id, 0);
trajectory_connectivity->Connect(scan_trajectory_id,
submap_id.trajectory_id);
} else { } else {
return; return;
} }

15
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h
View File

@ -35,7 +35,7 @@
#include "cartographer/mapping/trajectory_connectivity.h" #include "cartographer/mapping/trajectory_connectivity.h"
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
#include "cartographer/mapping_2d/submaps.h"
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
@ -86,16 +86,14 @@ class ConstraintBuilder {
// 'submap_id' and the 'point_cloud' for 'scan_index'. This performs // 'submap_id' and the 'point_cloud' for 'scan_index'. This performs
// full-submap matching. // full-submap matching.
// //
// The scan at 'scan_index' should be from trajectory 'scan_trajectory', and // The scan at 'scan_index' should be from trajectory 'scan_trajectory_id'.
// the 'submap' should be from 'submap_trajectory'. The // The 'trajectory_connectivity' is updated if the full-submap match succeeds.
// 'trajectory_connectivity' is updated if the full-submap match succeeds.
// //
// The pointees of 'submap' and 'point_cloud' must stay valid until all // The pointees of 'submap' and 'point_cloud' must stay valid until all
// computations are finished. // computations are finished.
void MaybeAddGlobalConstraint( void MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* submap, const mapping::SubmapId& submap_id, const mapping::Submap* submap,
int scan_index, const mapping::Submaps* scan_trajectory, int scan_index, int scan_trajectory_id,
const mapping::Submaps* submap_trajectory,
mapping::TrajectoryConnectivity* trajectory_connectivity, mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud* point_cloud); const sensor::PointCloud* point_cloud);
@ -134,13 +132,12 @@ class ConstraintBuilder {
// Runs in a background thread and does computations for an additional // Runs in a background thread and does computations for an additional
// constraint, assuming 'submap' and 'point_cloud' do not change anymore. // constraint, assuming 'submap' and 'point_cloud' do not change anymore.
// If 'match_full_submap' is true, and global localization succeeds, will // If 'match_full_submap' is true, and global localization succeeds, will
// connect 'scan_trajectory' and 'submap_trajectory' in // connect 'scan_trajectory_id' and 'submap_id.trajectory_id' in
// 'trajectory_connectivity'. // 'trajectory_connectivity'.
// As output, it may create a new Constraint in 'constraint'. // As output, it may create a new Constraint in 'constraint'.
void ComputeConstraint( void ComputeConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* submap, const mapping::SubmapId& submap_id, const mapping::Submap* submap,
int scan_index, const mapping::Submaps* scan_trajectory, int scan_index, int scan_trajectory_id, bool match_full_submap,
const mapping::Submaps* submap_trajectory, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity, mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud* point_cloud, const sensor::PointCloud* point_cloud,
const transform::Rigid2d& initial_relative_pose, const transform::Rigid2d& initial_relative_pose,

44
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -97,6 +97,7 @@ void SparsePoseGraph::AddScan(
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
trajectory_ids_.emplace(trajectory, trajectory_ids_.size()); trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
const int trajectory_id = trajectory_ids_.at(trajectory);
const int j = trajectory_nodes_.size(); const int j = trajectory_nodes_.size();
CHECK_LT(j, std::numeric_limits<int>::max()); CHECK_LT(j, std::numeric_limits<int>::max());
@ -106,16 +107,15 @@ void SparsePoseGraph::AddScan(
transform::Rigid3d::Identity()}); transform::Rigid3d::Identity()});
trajectory_nodes_.push_back( trajectory_nodes_.push_back(
mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose}); mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose});
trajectory_connectivity_.Add(trajectory); trajectory_connectivity_.Add(trajectory_id);
if (submap_indices_.count(insertion_submaps.back()) == 0) { if (submap_indices_.count(insertion_submaps.back()) == 0) {
submap_indices_.emplace(insertion_submaps.back(), submap_indices_.emplace(insertion_submaps.back(),
static_cast<int>(submap_indices_.size())); static_cast<int>(submap_indices_.size()));
submap_states_.emplace_back(); submap_states_.emplace_back();
submap_states_.back().submap = insertion_submaps.back(); submap_states_.back().submap = insertion_submaps.back();
submap_states_.back().trajectory = trajectory;
submap_states_.back().id = mapping::SubmapId{ submap_states_.back().id = mapping::SubmapId{
trajectory_ids_.at(trajectory), num_submaps_in_trajectory_[trajectory]}; trajectory_id, num_submaps_in_trajectory_[trajectory]};
++num_submaps_in_trajectory_[trajectory]; ++num_submaps_in_trajectory_[trajectory];
CHECK_EQ(submap_states_.size(), submap_indices_.size()); CHECK_EQ(submap_states_.size(), submap_indices_.size());
} }
@ -172,23 +172,23 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
const mapping::Submaps* const scan_trajectory = const mapping::Submaps* const scan_trajectory =
trajectory_nodes_[scan_index].constant_data->trajectory; trajectory_nodes_[scan_index].constant_data->trajectory;
const mapping::Submaps* const submap_trajectory = const int scan_trajectory_id = trajectory_ids_.at(scan_trajectory);
submap_states_[submap_index].trajectory; const int submap_trajectory_id =
submap_states_[submap_index].id.trajectory_id;
// Only globally match against submaps not in this trajectory. // Only globally match against submaps not in this trajectory.
if (scan_trajectory != submap_trajectory && if (scan_trajectory_id != submap_trajectory_id &&
global_localization_samplers_[scan_trajectory]->Pulse()) { global_localization_samplers_[scan_trajectory]->Pulse()) {
constraint_builder_.MaybeAddGlobalConstraint( constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap_states_[submap_index].submap, scan_index, submap_id, submap_states_[submap_index].submap, scan_index,
scan_trajectory, submap_trajectory, &trajectory_connectivity_, scan_trajectory_id, &trajectory_connectivity_, trajectory_nodes_);
trajectory_nodes_);
} else { } else {
const bool scan_and_submap_trajectories_connected = const bool scan_and_submap_trajectories_connected =
reverse_connected_components_.count(scan_trajectory) > 0 && reverse_connected_components_.count(scan_trajectory_id) > 0 &&
reverse_connected_components_.count(submap_trajectory) > 0 && reverse_connected_components_.count(submap_trajectory_id) > 0 &&
reverse_connected_components_.at(scan_trajectory) == reverse_connected_components_.at(scan_trajectory_id) ==
reverse_connected_components_.at(submap_trajectory); reverse_connected_components_.at(submap_trajectory_id);
if (scan_trajectory == submap_trajectory || if (scan_trajectory_id == submap_trajectory_id ||
scan_and_submap_trajectories_connected) { scan_and_submap_trajectories_connected) {
constraint_builder_.MaybeAddConstraint( constraint_builder_.MaybeAddConstraint(
submap_id, submap_states_[submap_index].submap, scan_index, submap_id, submap_states_[submap_index].submap, scan_index,
@ -379,8 +379,8 @@ void SparsePoseGraph::RunOptimization() {
connected_components_ = trajectory_connectivity_.ConnectedComponents(); connected_components_ = trajectory_connectivity_.ConnectedComponents();
reverse_connected_components_.clear(); reverse_connected_components_.clear();
for (size_t i = 0; i != connected_components_.size(); ++i) { for (size_t i = 0; i != connected_components_.size(); ++i) {
for (const auto* trajectory : connected_components_[i]) { for (const int trajectory_id : connected_components_[i]) {
reverse_connected_components_.emplace(trajectory, i); reverse_connected_components_.emplace(trajectory_id, i);
} }
} }
} }
@ -410,8 +410,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
.inverse(); .inverse();
} }
std::vector<std::vector<const mapping::Submaps*>> std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
SparsePoseGraph::GetConnectedTrajectories() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return connected_components_; return connected_components_;
} }
@ -426,17 +425,20 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
const std::vector<std::vector<sparse_pose_graph::SubmapData>>& const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
submap_transforms, submap_transforms,
const mapping::Submaps* const trajectory) const { const mapping::Submaps* const trajectory) const {
std::vector<transform::Rigid3d> result; if (trajectory_ids_.count(trajectory) == 0) {
return {transform::Rigid3d::Identity()};
}
const int trajectory_id = trajectory_ids_.at(trajectory);
size_t flat_index = 0; size_t flat_index = 0;
size_t flat_index_of_result_back = -1; size_t flat_index_of_result_back = -1;
// Submaps for which we have optimized poses. // Submaps for which we have optimized poses.
std::vector<transform::Rigid3d> result;
for (; flat_index != submap_states_.size(); ++flat_index) { for (; flat_index != submap_states_.size(); ++flat_index) {
const auto& state = submap_states_[flat_index]; const auto& state = submap_states_[flat_index];
if (state.trajectory != trajectory) { if (state.id.trajectory_id != trajectory_id) {
continue; continue;
} }
const int trajectory_id = trajectory_ids_.at(trajectory);
if (static_cast<size_t>(trajectory_id) >= submap_transforms.size() || if (static_cast<size_t>(trajectory_id) >= submap_transforms.size() ||
result.size() >= submap_transforms.at(trajectory_id).size()) { result.size() >= submap_transforms.at(trajectory_id).size()) {
break; break;
@ -449,7 +451,7 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
// Extrapolate to the remaining submaps. // Extrapolate to the remaining submaps.
for (; flat_index != submap_states_.size(); ++flat_index) { for (; flat_index != submap_states_.size(); ++flat_index) {
const auto& state = submap_states_[flat_index]; const auto& state = submap_states_[flat_index];
if (state.trajectory != trajectory) { if (state.id.trajectory_id != trajectory_id) {
continue; continue;
} }
if (result.empty()) { if (result.empty()) {

14
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -85,8 +85,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const Eigen::Vector3d& angular_velocity); const Eigen::Vector3d& angular_velocity);
void RunFinalOptimization() override; void RunFinalOptimization() override;
std::vector<std::vector<const mapping::Submaps*>> GetConnectedTrajectories() std::vector<std::vector<int>> GetConnectedTrajectories() override;
override;
std::vector<transform::Rigid3d> GetSubmapTransforms( std::vector<transform::Rigid3d> GetSubmapTransforms(
const mapping::Submaps* trajectory) EXCLUDES(mutex_) override; const mapping::Submaps* trajectory) EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps) transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps)
@ -114,9 +113,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// which are finished. // which are finished.
bool finished = false; bool finished = false;
// The trajectory to which this SubmapState belongs.
const Submaps* trajectory = nullptr;
mapping::SubmapId id; mapping::SubmapId id;
}; };
@ -205,10 +201,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
std::map<const mapping::Submaps*, int> num_submaps_in_trajectory_ std::map<const mapping::Submaps*, int> num_submaps_in_trajectory_
GUARDED_BY(mutex_); GUARDED_BY(mutex_);
// Connectivity structure of our trajectories. // Connectivity structure of our trajectories by IDs.
std::vector<std::vector<const mapping::Submaps*>> connected_components_; std::vector<std::vector<int>> connected_components_;
// Trajectory to connected component ID. // Trajectory ID to connected component ID.
std::map<const mapping::Submaps*, size_t> reverse_connected_components_; std::map<int, size_t> reverse_connected_components_;
// Data that are currently being shown. // Data that are currently being shown.
// //

18
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc
View File

@ -100,8 +100,7 @@ void ConstraintBuilder::MaybeAddConstraint(
submap_id, submap_nodes, &submap->high_resolution_hybrid_grid, submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
[=]() EXCLUDES(mutex_) { [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, scan_index, ComputeConstraint(submap_id, submap, scan_index,
nullptr, /* scan_trajectory */ -1, /* scan_trajectory_id */
nullptr, /* submap_trajectory */
false, /* match_full_submap */ false, /* match_full_submap */
nullptr, /* trajectory_connectivity */ nullptr, /* trajectory_connectivity */
point_cloud, initial_relative_pose, constraint); point_cloud, initial_relative_pose, constraint);
@ -112,8 +111,7 @@ void ConstraintBuilder::MaybeAddConstraint(
void ConstraintBuilder::MaybeAddGlobalConstraint( void ConstraintBuilder::MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap, const mapping::SubmapId& submap_id, const Submap* const submap,
const int scan_index, const mapping::Submaps* scan_trajectory, const int scan_index, const int scan_trajectory_id,
const mapping::Submaps* submap_trajectory,
mapping::TrajectoryConnectivity* trajectory_connectivity, mapping::TrajectoryConnectivity* trajectory_connectivity,
const std::vector<mapping::TrajectoryNode>& trajectory_nodes) { const std::vector<mapping::TrajectoryNode>& trajectory_nodes) {
const auto submap_nodes = ComputeSubmapNodes( const auto submap_nodes = ComputeSubmapNodes(
@ -129,8 +127,8 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap_nodes, &submap->high_resolution_hybrid_grid, submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
[=]() EXCLUDES(mutex_) { [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, scan_index, submap_trajectory, ComputeConstraint(submap_id, submap, scan_index, scan_trajectory_id,
scan_trajectory, true, /* match_full_submap */ true, /* match_full_submap */
trajectory_connectivity, point_cloud, trajectory_connectivity, point_cloud,
transform::Rigid3d::Identity(), constraint); transform::Rigid3d::Identity(), constraint);
FinishComputation(current_computation); FinishComputation(current_computation);
@ -200,8 +198,7 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
void ConstraintBuilder::ComputeConstraint( void ConstraintBuilder::ComputeConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap, const mapping::SubmapId& submap_id, const Submap* const submap,
const int scan_index, const mapping::Submaps* scan_trajectory, const int scan_index, const int scan_trajectory_id, bool match_full_submap,
const mapping::Submaps* submap_trajectory, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity, mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::CompressedPointCloud* const compressed_point_cloud, const sensor::CompressedPointCloud* const compressed_point_cloud,
const transform::Rigid3d& initial_relative_pose, const transform::Rigid3d& initial_relative_pose,
@ -227,7 +224,10 @@ void ConstraintBuilder::ComputeConstraint(
initial_pose.rotation(), filtered_point_cloud, point_cloud, initial_pose.rotation(), filtered_point_cloud, point_cloud,
options_.global_localization_min_score(), &score, &pose_estimate)) { options_.global_localization_min_score(), &score, &pose_estimate)) {
CHECK_GT(score, options_.global_localization_min_score()); CHECK_GT(score, options_.global_localization_min_score());
trajectory_connectivity->Connect(scan_trajectory, submap_trajectory); CHECK_GE(scan_trajectory_id, 0);
CHECK_GE(submap_id.trajectory_id, 0);
trajectory_connectivity->Connect(scan_trajectory_id,
submap_id.trajectory_id);
} else { } else {
return; return;
} }

13
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h
View File

@ -82,16 +82,14 @@ class ConstraintBuilder {
// 'submap_id' and the 'range_data_3d.returns' in 'trajectory_nodes' for // 'submap_id' and the 'range_data_3d.returns' in 'trajectory_nodes' for
// 'scan_index'. This performs full-submap matching. // 'scan_index'. This performs full-submap matching.
// //
// The scan at 'scan_index' should be from trajectory 'scan_trajectory', and // The scan at 'scan_index' should be from trajectory 'scan_trajectory_id'.
// the 'submap' should be from 'submap_trajectory'. The // The 'trajectory_connectivity' is updated if the full-submap match succeeds.
// 'trajectory_connectivity' is updated if the full-submap match succeeds.
// //
// The pointees of 'submap' and 'range_data_3d.returns' must stay valid until // The pointees of 'submap' and 'range_data_3d.returns' must stay valid until
// all computations are finished. // all computations are finished.
void MaybeAddGlobalConstraint( void MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const Submap* submap, int scan_index, const mapping::SubmapId& submap_id, const Submap* submap, int scan_index,
const mapping::Submaps* scan_trajectory, int scan_trajectory_id,
const mapping::Submaps* submap_trajectory,
mapping::TrajectoryConnectivity* trajectory_connectivity, mapping::TrajectoryConnectivity* trajectory_connectivity,
const std::vector<mapping::TrajectoryNode>& trajectory_nodes); const std::vector<mapping::TrajectoryNode>& trajectory_nodes);
@ -133,13 +131,12 @@ class ConstraintBuilder {
// Runs in a background thread and does computations for an additional // Runs in a background thread and does computations for an additional
// constraint, assuming 'submap' and 'point_cloud' do not change anymore. // constraint, assuming 'submap' and 'point_cloud' do not change anymore.
// If 'match_full_submap' is true, and global localization succeeds, will // If 'match_full_submap' is true, and global localization succeeds, will
// connect 'scan_trajectory' and 'submap_trajectory' in // connect 'scan_trajectory_id' and 'submap_id.trajectory_id' in
// 'trajectory_connectivity'. // 'trajectory_connectivity'.
// As output, it may create a new Constraint in 'constraint'. // As output, it may create a new Constraint in 'constraint'.
void ComputeConstraint( void ComputeConstraint(
const mapping::SubmapId& submap_id, const Submap* submap, int scan_index, const mapping::SubmapId& submap_id, const Submap* submap, int scan_index,
const mapping::Submaps* scan_trajectory, int scan_trajectory_id, bool match_full_submap,
const mapping::Submaps* submap_trajectory, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity, mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::CompressedPointCloud* const compressed_point_cloud, const sensor::CompressedPointCloud* const compressed_point_cloud,
const transform::Rigid3d& initial_relative_pose, const transform::Rigid3d& initial_relative_pose,