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() {
return ToProto(sparse_pose_graph_->GetConnectedTrajectories(),
trajectory_ids_);
return ToProto(sparse_pose_graph_->GetConnectedTrajectories());
}
string MapBuilder::SubmapToProto(const int trajectory_id,

5
cartographer/mapping/sparse_pose_graph.h
View File

@ -58,7 +58,7 @@ class SparsePoseGraph {
Eigen::Matrix<double, 6, 6> sqrt_Lambda_ij;
};
mapping::SubmapId submap_id; // 'i' in the paper.
mapping::SubmapId submap_id; // 'i' in the paper.
// Scan index.
int j;
@ -82,8 +82,7 @@ class SparsePoseGraph {
virtual void RunFinalOptimization() = 0;
// Get the current trajectory clusters.
virtual std::vector<std::vector<const Submaps*>>
GetConnectedTrajectories() = 0;
virtual std::vector<std::vector<int>> GetConnectedTrajectories() = 0;
// Returns the current optimized transforms for the given 'trajectory'.
virtual std::vector<transform::Rigid3d> GetSubmapTransforms(

84
cartographer/mapping/trajectory_connectivity.cc
View File

@ -28,35 +28,30 @@ namespace mapping {
TrajectoryConnectivity::TrajectoryConnectivity()
: lock_(), forest_(), connection_map_() {}
void TrajectoryConnectivity::Add(const Submaps* trajectory) {
CHECK(trajectory != nullptr);
void TrajectoryConnectivity::Add(const int trajectory_id) {
common::MutexLocker locker(&lock_);
forest_.emplace(trajectory, trajectory);
forest_.emplace(trajectory_id, trajectory_id);
}
void TrajectoryConnectivity::Connect(const Submaps* trajectory_a,
const Submaps* trajectory_b) {
CHECK(trajectory_a != nullptr);
CHECK(trajectory_b != nullptr);
void TrajectoryConnectivity::Connect(const int trajectory_id_a,
const int trajectory_id_b) {
common::MutexLocker locker(&lock_);
Union(trajectory_a, trajectory_b);
auto sorted_pair =
std::minmax(trajectory_a, trajectory_b, std::less<const Submaps*>());
Union(trajectory_id_a, trajectory_id_b);
auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b);
++connection_map_[sorted_pair];
}
void TrajectoryConnectivity::Union(const Submaps* const trajectory_a,
const Submaps* const trajectory_b) {
forest_.emplace(trajectory_a, trajectory_a);
forest_.emplace(trajectory_b, trajectory_b);
const Submaps* const representative_a = FindSet(trajectory_a);
const Submaps* const representative_b = FindSet(trajectory_b);
void TrajectoryConnectivity::Union(const int trajectory_id_a,
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);
const int representative_b = FindSet(trajectory_id_b);
forest_[representative_a] = representative_b;
}
const Submaps* TrajectoryConnectivity::FindSet(
const Submaps* const trajectory) {
auto it = forest_.find(trajectory);
int TrajectoryConnectivity::FindSet(const int trajectory_id) {
auto it = forest_.find(trajectory_id);
CHECK(it != forest_.end());
if (it->first != it->second) {
it->second = FindSet(it->second);
@ -64,28 +59,27 @@ const Submaps* TrajectoryConnectivity::FindSet(
return it->second;
}
bool TrajectoryConnectivity::TransitivelyConnected(
const Submaps* trajectory_a, const Submaps* trajectory_b) {
CHECK(trajectory_a != nullptr);
CHECK(trajectory_b != nullptr);
bool TrajectoryConnectivity::TransitivelyConnected(const int trajectory_id_a,
const int trajectory_id_b) {
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 FindSet(trajectory_a) == FindSet(trajectory_b);
return FindSet(trajectory_id_a) == FindSet(trajectory_id_b);
}
std::vector<std::vector<const Submaps*>>
TrajectoryConnectivity::ConnectedComponents() {
std::vector<std::vector<int>> TrajectoryConnectivity::ConnectedComponents() {
// 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_);
for (const auto& trajectory_entry : forest_) {
map[FindSet(trajectory_entry.first)].push_back(trajectory_entry.first);
for (const auto& trajectory_id_entry : forest_) {
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());
for (auto& pair : map) {
result.emplace_back(std::move(pair.second));
@ -93,33 +87,23 @@ TrajectoryConnectivity::ConnectedComponents() {
return result;
}
int TrajectoryConnectivity::ConnectionCount(const Submaps* trajectory_a,
const Submaps* trajectory_b) {
CHECK(trajectory_a != nullptr);
CHECK(trajectory_b != nullptr);
int TrajectoryConnectivity::ConnectionCount(const int trajectory_id_a,
const int trajectory_id_b) {
common::MutexLocker locker(&lock_);
const auto it = connection_map_.find(
std::minmax(trajectory_a, trajectory_b, std::less<const Submaps*>()));
const auto it =
connection_map_.find(std::minmax(trajectory_id_a, trajectory_id_b));
return it != connection_map_.end() ? it->second : 0;
}
proto::TrajectoryConnectivity ToProto(
std::vector<std::vector<const Submaps*>> connected_components,
std::unordered_map<const mapping::Submaps*, int> trajectory_indices) {
std::vector<std::vector<int>> connected_components) {
proto::TrajectoryConnectivity proto;
std::vector<std::vector<int>> connected_components_by_indices;
for (const auto& connected_component : connected_components) {
connected_components_by_indices.emplace_back();
for (const mapping::Submaps* trajectory : connected_component) {
connected_components_by_indices.back().push_back(
trajectory_indices.at(trajectory));
}
std::sort(connected_components_by_indices.back().begin(),
connected_components_by_indices.back().end());
std::sort(connected_components.back().begin(),
connected_components.back().end());
}
std::sort(connected_components_by_indices.begin(),
connected_components_by_indices.end());
for (const auto& connected_component : connected_components_by_indices) {
std::sort(connected_components.begin(), connected_components.end());
for (const auto& connected_component : connected_components) {
auto* proto_connected_component = proto.add_connected_component();
for (const int trajectory_id : connected_component) {
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;
// 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
// tracked. This function is invariant to the order of its arguments. Repeated
// calls to Connect increment the connectivity count.
void Connect(const Submaps* trajectory_a, const Submaps* trajectory_b)
EXCLUDES(lock_);
void Connect(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_);
// Determines if two trajectories have been (transitively) connected. If
// either trajectory is not being tracked, returns false. This function is
// invariant to the order of its arguments.
bool TransitivelyConnected(const Submaps* trajectory_a,
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)
bool TransitivelyConnected(int trajectory_id_a, int trajectory_id_b)
EXCLUDES(lock_);
// The trajectories, grouped by connectivity.
std::vector<std::vector<const Submaps*>> ConnectedComponents()
EXCLUDES(lock_);
// Return the number of _direct_ connections between 'trajectory_id_a' and
// 'trajectory_id_b'. If either trajectory is not being tracked, returns 0.
// 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:
// Find the representative and compresses the path to it.
const Submaps* FindSet(const Submaps* trajectory) REQUIRES(lock_);
void Union(const Submaps* trajectory_a, const Submaps* trajectory_b)
REQUIRES(lock_);
int FindSet(int trajectory_id) REQUIRES(lock_);
void Union(int trajectory_id_a, int trajectory_id_b) REQUIRES(lock_);
common::Mutex lock_;
// Tracks transitive connectivity using a disjoint set forest, i.e. each
// 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.
std::map<std::pair<const Submaps*, const Submaps*>, int> connection_map_
GUARDED_BY(lock_);
std::map<std::pair<int, int>, int> connection_map_ GUARDED_BY(lock_);
};
// Returns a proto encoding connected components according to
// 'trajectory_indices'.
// Returns a proto encoding connected components.
proto::TrajectoryConnectivity ToProto(
std::vector<std::vector<const Submaps*>> connected_components,
std::unordered_map<const mapping::Submaps*, int> trajectory_indices);
std::vector<std::vector<int>> connected_components);
// Returns the connected component containing 'trajectory_index'.
proto::TrajectoryConnectivity::ConnectedComponent FindConnectedComponent(

97
cartographer/mapping/trajectory_connectivity_test.cc
View File

@ -20,88 +20,60 @@
#include <memory>
#include <vector>
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
#include "cartographer/mapping_2d/submaps.h"
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping {
namespace {
class TrajectoryConnectivityTest : public ::testing::Test {
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())));
}
}
constexpr int kNumTrajectories = 10;
// Helper function to avoid .get() noise.
const Submaps* trajectory(int index) { return trajectories_.at(index).get(); }
TEST(TrajectoryConnectivityTest, TransitivelyConnected) {
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.
for (auto& trajectory_a : trajectories_) {
for (auto& trajectory_b : trajectories_) {
EXPECT_FALSE(trajectory_connectivity_.TransitivelyConnected(
trajectory_a.get(), trajectory_b.get()));
for (int trajectory_a; trajectory_a < kNumTrajectories; ++trajectory_a) {
for (int trajectory_b; trajectory_b < kNumTrajectories; ++trajectory_b) {
EXPECT_FALSE(trajectory_connectivity.TransitivelyConnected(trajectory_a,
trajectory_b));
}
}
// Connect some stuff up.
trajectory_connectivity_.Connect(trajectory(0), trajectory(1));
EXPECT_TRUE(trajectory_connectivity_.TransitivelyConnected(trajectory(0),
trajectory(1)));
trajectory_connectivity_.Connect(trajectory(8), trajectory(9));
EXPECT_TRUE(trajectory_connectivity_.TransitivelyConnected(trajectory(8),
trajectory(9)));
EXPECT_FALSE(trajectory_connectivity_.TransitivelyConnected(trajectory(0),
trajectory(9)));
trajectory_connectivity.Connect(0, 1);
EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(0, 1));
trajectory_connectivity.Connect(8, 9);
EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(8, 9));
EXPECT_FALSE(trajectory_connectivity.TransitivelyConnected(0, 9));
trajectory_connectivity_.Connect(trajectory(1), trajectory(8));
trajectory_connectivity.Connect(1, 8);
for (int i : {0, 1}) {
for (int j : {8, 9}) {
EXPECT_TRUE(trajectory_connectivity_.TransitivelyConnected(
trajectory(i), trajectory(j)));
EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(i, j));
}
}
}
TEST_F(TrajectoryConnectivityTest, EmptyConnectedComponents) {
auto connections = trajectory_connectivity_.ConnectedComponents();
TEST(TrajectoryConnectivityTest, EmptyConnectedComponents) {
TrajectoryConnectivity trajectory_connectivity;
auto connections = trajectory_connectivity.ConnectedComponents();
EXPECT_EQ(0, connections.size());
}
TEST_F(TrajectoryConnectivityTest, ConnectedComponents) {
TEST(TrajectoryConnectivityTest, ConnectedComponents) {
TrajectoryConnectivity trajectory_connectivity;
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) {
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());
// The clustering is arbitrary; we need to figure out which one is which.
const std::vector<const Submaps*>* zero_cluster = nullptr;
const std::vector<const Submaps*>* five_cluster = nullptr;
if (std::find(connections[0].begin(), connections[0].end(), trajectory(0)) !=
const std::vector<int>* zero_cluster = nullptr;
const std::vector<int>* five_cluster = nullptr;
if (std::find(connections[0].begin(), connections[0].end(), 0) !=
connections[0].end()) {
zero_cluster = &connections[0];
five_cluster = &connections[1];
@ -111,22 +83,21 @@ TEST_F(TrajectoryConnectivityTest, ConnectedComponents) {
}
for (int i = 0; i <= 9; ++i) {
EXPECT_EQ(i <= 4, std::find(zero_cluster->begin(), zero_cluster->end(),
trajectory(i)) != zero_cluster->end());
EXPECT_EQ(i > 4, std::find(five_cluster->begin(), five_cluster->end(),
trajectory(i)) != five_cluster->end());
i) != zero_cluster->end());
EXPECT_EQ(i > 4, std::find(five_cluster->begin(), five_cluster->end(), i) !=
five_cluster->end());
}
}
TEST_F(TrajectoryConnectivityTest, ConnectionCount) {
for (int i = 0; i < 10; ++i) {
trajectory_connectivity_.Connect(trajectory(0), trajectory(1));
TEST(TrajectoryConnectivityTest, ConnectionCount) {
TrajectoryConnectivity trajectory_connectivity;
for (int i = 0; i < kNumTrajectories; ++i) {
trajectory_connectivity.Connect(0, 1);
// Permute the arguments to check invariance.
EXPECT_EQ(i + 1, trajectory_connectivity_.ConnectionCount(trajectory(1),
trajectory(0)));
EXPECT_EQ(i + 1, trajectory_connectivity.ConnectionCount(1, 0));
}
for (int i = 1; i < 9; ++i) {
EXPECT_EQ(0, trajectory_connectivity_.ConnectionCount(trajectory(i),
trajectory(i + 1)));
EXPECT_EQ(0, trajectory_connectivity.ConnectionCount(i, i + 1));
}
}

43
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -99,6 +99,7 @@ void SparsePoseGraph::AddScan(
common::MutexLocker locker(&mutex_);
trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
const int trajectory_id = trajectory_ids_.at(trajectory);
const int j = trajectory_nodes_.size();
CHECK_LT(j, std::numeric_limits<int>::max());
@ -109,16 +110,15 @@ void SparsePoseGraph::AddScan(
trajectory_nodes_.push_back(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) {
submap_indices_.emplace(insertion_submaps.back(),
static_cast<int>(submap_indices_.size()));
submap_states_.emplace_back();
submap_states_.back().submap = insertion_submaps.back();
submap_states_.back().trajectory = trajectory;
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];
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 =
trajectory_nodes_[scan_index].constant_data->trajectory;
const mapping::Submaps* const submap_trajectory =
submap_states_[submap_index].trajectory;
const int scan_trajectory_id = trajectory_ids_.at(scan_trajectory);
const int submap_trajectory_id =
submap_states_[submap_index].id.trajectory_id;
// 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()) {
constraint_builder_.MaybeAddGlobalConstraint(
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);
} else {
const bool scan_and_submap_trajectories_connected =
reverse_connected_components_.count(scan_trajectory) > 0 &&
reverse_connected_components_.count(submap_trajectory) > 0 &&
reverse_connected_components_.at(scan_trajectory) ==
reverse_connected_components_.at(submap_trajectory);
if (scan_trajectory == submap_trajectory ||
reverse_connected_components_.count(scan_trajectory_id) > 0 &&
reverse_connected_components_.count(submap_trajectory_id) > 0 &&
reverse_connected_components_.at(scan_trajectory_id) ==
reverse_connected_components_.at(submap_trajectory_id);
if (scan_trajectory_id == submap_trajectory_id ||
scan_and_submap_trajectories_connected) {
constraint_builder_.MaybeAddConstraint(
submap_id, submap_states_[submap_index].submap, scan_index,
@ -386,8 +387,8 @@ void SparsePoseGraph::RunOptimization() {
connected_components_ = trajectory_connectivity_.ConnectedComponents();
reverse_connected_components_.clear();
for (size_t i = 0; i != connected_components_.size(); ++i) {
for (const auto* trajectory : connected_components_[i]) {
reverse_connected_components_.emplace(trajectory, i);
for (const int trajectory_id : connected_components_[i]) {
reverse_connected_components_.emplace(trajectory_id, i);
}
}
}
@ -417,8 +418,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
.inverse();
}
std::vector<std::vector<const mapping::Submaps*>>
SparsePoseGraph::GetConnectedTrajectories() {
std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
common::MutexLocker locker(&mutex_);
return connected_components_;
}
@ -433,17 +433,20 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
submap_transforms,
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_of_result_back = -1;
// Submaps for which we have optimized poses.
std::vector<transform::Rigid3d> result;
for (; flat_index != submap_states_.size(); ++flat_index) {
const auto& state = submap_states_[flat_index];
if (state.trajectory != trajectory) {
if (state.id.trajectory_id != trajectory_id) {
continue;
}
const int trajectory_id = trajectory_ids_.at(trajectory);
if (static_cast<size_t>(trajectory_id) >= submap_transforms.size() ||
result.size() >= submap_transforms.at(trajectory_id).size()) {
break;
@ -456,7 +459,7 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
// Extrapolate to the remaining submaps.
for (; flat_index != submap_states_.size(); ++flat_index) {
const auto& state = submap_states_[flat_index];
if (state.trajectory != trajectory) {
if (state.id.trajectory_id != trajectory_id) {
continue;
}
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);
void RunFinalOptimization() override;
std::vector<std::vector<const mapping::Submaps*>> GetConnectedTrajectories()
override;
std::vector<std::vector<int>> GetConnectedTrajectories() override;
std::vector<transform::Rigid3d> GetSubmapTransforms(
const mapping::Submaps* trajectory) EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps)
@ -112,9 +111,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// which are finished.
bool finished = false;
// The trajectory to which this SubmapState belongs.
const mapping::Submaps* trajectory = nullptr;
mapping::SubmapId id;
};
@ -205,10 +201,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
std::map<const mapping::Submaps*, int> num_submaps_in_trajectory_
GUARDED_BY(mutex_);
// Connectivity structure of our trajectories.
std::vector<std::vector<const mapping::Submaps*>> connected_components_;
// Trajectory to connected component ID.
std::map<const mapping::Submaps*, size_t> reverse_connected_components_;
// Connectivity structure of our trajectories by IDs.
std::vector<std::vector<int>> connected_components_;
// Trajectory ID to connected component ID.
std::map<int, size_t> reverse_connected_components_;
// 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(
submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, scan_index,
nullptr, /* scan_trajectory */
nullptr, /* submap_trajectory */
-1, /* scan_trajectory_id */
false, /* match_full_submap */
nullptr, /* trajectory_connectivity */
point_cloud, initial_relative_pose, constraint);
@ -90,8 +89,7 @@ void ConstraintBuilder::MaybeAddConstraint(
void ConstraintBuilder::MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
const int scan_index, const mapping::Submaps* scan_trajectory,
const mapping::Submaps* submap_trajectory,
const int scan_index, const int scan_trajectory_id,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud* const point_cloud) {
common::MutexLocker locker(&mutex_);
@ -102,8 +100,8 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
const int current_computation = current_computation_;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, scan_index, submap_trajectory,
scan_trajectory, true, /* match_full_submap */
ComputeConstraint(submap_id, submap, scan_index, scan_trajectory_id,
true, /* match_full_submap */
trajectory_connectivity, point_cloud,
transform::Rigid2d::Identity(), constraint);
FinishComputation(current_computation);
@ -168,8 +166,7 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
void ConstraintBuilder::ComputeConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
const int scan_index, const mapping::Submaps* scan_trajectory,
const mapping::Submaps* submap_trajectory, bool match_full_submap,
const int scan_index, const int scan_trajectory_id, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud* const point_cloud,
const transform::Rigid2d& initial_relative_pose,
@ -194,7 +191,10 @@ void ConstraintBuilder::ComputeConstraint(
filtered_point_cloud, options_.global_localization_min_score(),
&score, &pose_estimate)) {
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 {
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_2d/scan_matching/ceres_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/fast_correlative_scan_matcher.h"
#include "cartographer/sensor/point_cloud.h"
@ -86,16 +86,14 @@ class ConstraintBuilder {
// 'submap_id' and the 'point_cloud' for 'scan_index'. This performs
// full-submap matching.
//
// The scan at 'scan_index' should be from trajectory 'scan_trajectory', and
// the 'submap' should be from 'submap_trajectory'. The
// 'trajectory_connectivity' is updated if the full-submap match succeeds.
// The scan at 'scan_index' should be from trajectory 'scan_trajectory_id'.
// The 'trajectory_connectivity' is updated if the full-submap match succeeds.
//
// The pointees of 'submap' and 'point_cloud' must stay valid until all
// computations are finished.
void MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* submap,
int scan_index, const mapping::Submaps* scan_trajectory,
const mapping::Submaps* submap_trajectory,
int scan_index, int scan_trajectory_id,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud* point_cloud);
@ -134,13 +132,12 @@ class ConstraintBuilder {
// Runs in a background thread and does computations for an additional
// constraint, assuming 'submap' and 'point_cloud' do not change anymore.
// 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'.
// As output, it may create a new Constraint in 'constraint'.
void ComputeConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* submap,
int scan_index, const mapping::Submaps* scan_trajectory,
const mapping::Submaps* submap_trajectory, bool match_full_submap,
int scan_index, int scan_trajectory_id, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud* point_cloud,
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_);
trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
const int trajectory_id = trajectory_ids_.at(trajectory);
const int j = trajectory_nodes_.size();
CHECK_LT(j, std::numeric_limits<int>::max());
@ -106,16 +107,15 @@ void SparsePoseGraph::AddScan(
transform::Rigid3d::Identity()});
trajectory_nodes_.push_back(
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) {
submap_indices_.emplace(insertion_submaps.back(),
static_cast<int>(submap_indices_.size()));
submap_states_.emplace_back();
submap_states_.back().submap = insertion_submaps.back();
submap_states_.back().trajectory = trajectory;
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];
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 =
trajectory_nodes_[scan_index].constant_data->trajectory;
const mapping::Submaps* const submap_trajectory =
submap_states_[submap_index].trajectory;
const int scan_trajectory_id = trajectory_ids_.at(scan_trajectory);
const int submap_trajectory_id =
submap_states_[submap_index].id.trajectory_id;
// 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()) {
constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap_states_[submap_index].submap, scan_index,
scan_trajectory, submap_trajectory, &trajectory_connectivity_,
trajectory_nodes_);
scan_trajectory_id, &trajectory_connectivity_, trajectory_nodes_);
} else {
const bool scan_and_submap_trajectories_connected =
reverse_connected_components_.count(scan_trajectory) > 0 &&
reverse_connected_components_.count(submap_trajectory) > 0 &&
reverse_connected_components_.at(scan_trajectory) ==
reverse_connected_components_.at(submap_trajectory);
if (scan_trajectory == submap_trajectory ||
reverse_connected_components_.count(scan_trajectory_id) > 0 &&
reverse_connected_components_.count(submap_trajectory_id) > 0 &&
reverse_connected_components_.at(scan_trajectory_id) ==
reverse_connected_components_.at(submap_trajectory_id);
if (scan_trajectory_id == submap_trajectory_id ||
scan_and_submap_trajectories_connected) {
constraint_builder_.MaybeAddConstraint(
submap_id, submap_states_[submap_index].submap, scan_index,
@ -379,8 +379,8 @@ void SparsePoseGraph::RunOptimization() {
connected_components_ = trajectory_connectivity_.ConnectedComponents();
reverse_connected_components_.clear();
for (size_t i = 0; i != connected_components_.size(); ++i) {
for (const auto* trajectory : connected_components_[i]) {
reverse_connected_components_.emplace(trajectory, i);
for (const int trajectory_id : connected_components_[i]) {
reverse_connected_components_.emplace(trajectory_id, i);
}
}
}
@ -410,8 +410,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
.inverse();
}
std::vector<std::vector<const mapping::Submaps*>>
SparsePoseGraph::GetConnectedTrajectories() {
std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
common::MutexLocker locker(&mutex_);
return connected_components_;
}
@ -426,17 +425,20 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
submap_transforms,
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_of_result_back = -1;
// Submaps for which we have optimized poses.
std::vector<transform::Rigid3d> result;
for (; flat_index != submap_states_.size(); ++flat_index) {
const auto& state = submap_states_[flat_index];
if (state.trajectory != trajectory) {
if (state.id.trajectory_id != trajectory_id) {
continue;
}
const int trajectory_id = trajectory_ids_.at(trajectory);
if (static_cast<size_t>(trajectory_id) >= submap_transforms.size() ||
result.size() >= submap_transforms.at(trajectory_id).size()) {
break;
@ -449,7 +451,7 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
// Extrapolate to the remaining submaps.
for (; flat_index != submap_states_.size(); ++flat_index) {
const auto& state = submap_states_[flat_index];
if (state.trajectory != trajectory) {
if (state.id.trajectory_id != trajectory_id) {
continue;
}
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);
void RunFinalOptimization() override;
std::vector<std::vector<const mapping::Submaps*>> GetConnectedTrajectories()
override;
std::vector<std::vector<int>> GetConnectedTrajectories() override;
std::vector<transform::Rigid3d> GetSubmapTransforms(
const mapping::Submaps* trajectory) EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps)
@ -114,9 +113,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// which are finished.
bool finished = false;
// The trajectory to which this SubmapState belongs.
const Submaps* trajectory = nullptr;
mapping::SubmapId id;
};
@ -205,10 +201,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
std::map<const mapping::Submaps*, int> num_submaps_in_trajectory_
GUARDED_BY(mutex_);
// Connectivity structure of our trajectories.
std::vector<std::vector<const mapping::Submaps*>> connected_components_;
// Trajectory to connected component ID.
std::map<const mapping::Submaps*, size_t> reverse_connected_components_;
// Connectivity structure of our trajectories by IDs.
std::vector<std::vector<int>> connected_components_;
// Trajectory ID to connected component ID.
std::map<int, size_t> reverse_connected_components_;
// 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,
[=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, scan_index,
nullptr, /* scan_trajectory */
nullptr, /* submap_trajectory */
-1, /* scan_trajectory_id */
false, /* match_full_submap */
nullptr, /* trajectory_connectivity */
point_cloud, initial_relative_pose, constraint);
@ -112,8 +111,7 @@ void ConstraintBuilder::MaybeAddConstraint(
void ConstraintBuilder::MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap,
const int scan_index, const mapping::Submaps* scan_trajectory,
const mapping::Submaps* submap_trajectory,
const int scan_index, const int scan_trajectory_id,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const std::vector<mapping::TrajectoryNode>& trajectory_nodes) {
const auto submap_nodes = ComputeSubmapNodes(
@ -129,8 +127,8 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
[=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, scan_index, submap_trajectory,
scan_trajectory, true, /* match_full_submap */
ComputeConstraint(submap_id, submap, scan_index, scan_trajectory_id,
true, /* match_full_submap */
trajectory_connectivity, point_cloud,
transform::Rigid3d::Identity(), constraint);
FinishComputation(current_computation);
@ -200,8 +198,7 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
void ConstraintBuilder::ComputeConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap,
const int scan_index, const mapping::Submaps* scan_trajectory,
const mapping::Submaps* submap_trajectory, bool match_full_submap,
const int scan_index, const int scan_trajectory_id, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::CompressedPointCloud* const compressed_point_cloud,
const transform::Rigid3d& initial_relative_pose,
@ -227,7 +224,10 @@ void ConstraintBuilder::ComputeConstraint(
initial_pose.rotation(), filtered_point_cloud, point_cloud,
options_.global_localization_min_score(), &score, &pose_estimate)) {
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 {
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
// 'scan_index'. This performs full-submap matching.
//
// The scan at 'scan_index' should be from trajectory 'scan_trajectory', and
// the 'submap' should be from 'submap_trajectory'. The
// 'trajectory_connectivity' is updated if the full-submap match succeeds.
// The scan at 'scan_index' should be from trajectory 'scan_trajectory_id'.
// The 'trajectory_connectivity' is updated if the full-submap match succeeds.
//
// The pointees of 'submap' and 'range_data_3d.returns' must stay valid until
// all computations are finished.
void MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const Submap* submap, int scan_index,
const mapping::Submaps* scan_trajectory,
const mapping::Submaps* submap_trajectory,
int scan_trajectory_id,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const std::vector<mapping::TrajectoryNode>& trajectory_nodes);
@ -133,13 +131,12 @@ class ConstraintBuilder {
// Runs in a background thread and does computations for an additional
// constraint, assuming 'submap' and 'point_cloud' do not change anymore.
// 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'.
// As output, it may create a new Constraint in 'constraint'.
void ComputeConstraint(
const mapping::SubmapId& submap_id, const Submap* submap, int scan_index,
const mapping::Submaps* scan_trajectory,
const mapping::Submaps* submap_trajectory, bool match_full_submap,
int scan_trajectory_id, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::CompressedPointCloud* const compressed_point_cloud,
const transform::Rigid3d& initial_relative_pose,