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

Move trajectory id mapping into the SparsePoseGraph. (#270)

master
Holger Rapp 2017-05-10 08:23:57 +02:00 committed by GitHub
parent 524b613f2c
commit 45de59b116
7 changed files with 47 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
cartographer/mapping/sparse_pose_graph.cc
View File

@ -36,15 +36,6 @@ proto::SparsePoseGraph::Constraint::Tag ToProto(
LOG(FATAL) << "Unsupported tag.";
}
std::unordered_map<const Submaps*, int> ComputeTrajectoryIds(
const std::vector<const Submaps*>& trajectories) {
std::unordered_map<const Submaps*, int> result;
for (const auto& trajectory : trajectories) {
result.emplace(trajectory, result.size());
}
return result;
}
void GroupTrajectoryNodes(
const std::vector<TrajectoryNode>& trajectory_nodes,
const std::unordered_map<const Submaps*, int>& trajectory_ids,
@ -100,21 +91,14 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
proto::SparsePoseGraph SparsePoseGraph::ToProto() {
proto::SparsePoseGraph proto;
const auto trajectory_nodes = GetTrajectoryNodes();
std::vector<const Submaps*> submap_pointers;
for (const auto& trajectory_node : trajectory_nodes) {
submap_pointers.push_back(trajectory_node.constant_data->trajectory);
}
const auto trajectory_ids = ComputeTrajectoryIds(submap_pointers);
std::vector<std::vector<TrajectoryNode>> grouped_nodes;
std::vector<std::pair<int, int>> grouped_node_indices;
GroupTrajectoryNodes(trajectory_nodes, trajectory_ids, &grouped_nodes,
GroupTrajectoryNodes(GetTrajectoryNodes(), trajectory_ids(), &grouped_nodes,
&grouped_node_indices);
std::vector<std::pair<int, int>> grouped_submap_indices;
GroupSubmapStates(GetSubmapStates(), trajectory_ids, &grouped_submap_indices);
GroupSubmapStates(GetSubmapStates(), trajectory_ids(),
&grouped_submap_indices);
for (const auto& constraint : constraints()) {
auto* const constraint_proto = proto.add_constraint();

8
cartographer/mapping/sparse_pose_graph.h
View File

@ -35,11 +35,6 @@ namespace mapping {
proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
common::LuaParameterDictionary* const parameter_dictionary);
// Construct a mapping from trajectory (ie Submaps*) to an integer. These
// values are used to track trajectory identity between scans and submaps.
std::unordered_map<const Submaps*, int> ComputeTrajectoryIds(
const std::vector<const Submaps*>& trajectories);
// TrajectoryNodes are provided in a flat vector, but serialization requires
// that we group them by trajectory. This groups the elements of
// 'trajectory_nodes' into 'grouped_nodes' (so that (*grouped_nodes)[i]
@ -132,6 +127,9 @@ class SparsePoseGraph {
// Returns the collection of constraints.
virtual std::vector<Constraint> constraints() = 0;
// Returns the mapping from Submaps* to trajectory IDs.
virtual const std::unordered_map<const Submaps*, int>& trajectory_ids() = 0;
};
// Like TrajectoryNodes, SubmapStates arrive in a flat vector, but need to be

20
cartographer/mapping/sparse_pose_graph_test.cc
View File

@ -56,21 +56,11 @@ TEST(SparsePoseGraphTest, TrajectoryFunctions) {
}
}
std::vector<const Submaps*> submap_pointers;
for (const auto& submap : submaps) {
submap_pointers.push_back(&submap);
}
const auto index_map = ComputeTrajectoryIds(submap_pointers);
ASSERT_EQ(submaps.size(), index_map.size());
for (const auto& kv : index_map) {
const auto pointer_iterator =
std::find(submap_pointers.begin(), submap_pointers.end(), kv.first);
ASSERT_TRUE(pointer_iterator != submap_pointers.end());
const auto pointer_index = pointer_iterator - submap_pointers.begin();
EXPECT_EQ(kv.second, pointer_index);
}
const std::unordered_map<const Submaps*, int> index_map = {{&submaps[0], 0},
{&submaps[1], 1},
{&submaps[2], 2},
{&submaps[3], 3},
{&submaps[4], 4}};
std::vector<std::vector<TrajectoryNode>> grouped_nodes;
std::vector<std::pair<int, int>> new_indices;
GroupTrajectoryNodes(trajectory_nodes, index_map, &grouped_nodes,

8
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -97,6 +97,7 @@ void SparsePoseGraph::AddScan(
GetLocalToGlobalTransform(trajectory) * transform::Embed3D(pose));
common::MutexLocker locker(&mutex_);
trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
const int j = trajectory_nodes_.size();
CHECK_LT(j, std::numeric_limits<int>::max());
@ -392,9 +393,16 @@ std::vector<SparsePoseGraph::SubmapState> SparsePoseGraph::GetSubmapStates() {
}
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
common::MutexLocker locker(&mutex_);
return constraints_;
}
const std::unordered_map<const mapping::Submaps*, int>&
SparsePoseGraph::trajectory_ids() {
common::MutexLocker locker(&mutex_);
return trajectory_ids_;
}
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
const mapping::Submaps* const submaps) {
const auto extrapolated_submap_transforms = GetSubmapTransforms(submaps);

13
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -91,9 +91,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
EXCLUDES(mutex_) override;
std::vector<mapping::TrajectoryNode> GetTrajectoryNodes() override
EXCLUDES(mutex_);
std::vector<SubmapState> GetSubmapStates() override EXCLUDES(mutex_);
std::vector<Constraint> constraints() override;
protected:
std::vector<SubmapState> GetSubmapStates() override EXCLUDES(mutex_);
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
const std::unordered_map<const mapping::Submaps*, int>& trajectory_ids()
override EXCLUDES(mutex_);
private:
// Handles a new work item.
@ -168,7 +171,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Current optimization problem.
sparse_pose_graph::OptimizationProblem optimization_problem_;
sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
std::vector<Constraint> constraints_;
std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
// Submaps get assigned an index and state as soon as they are seen, even
// before they take part in the background computations.
@ -190,6 +193,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Current submap transforms used for displaying data.
std::vector<transform::Rigid2d> optimized_submap_transforms_
GUARDED_BY(mutex_);
// Map from submap pointers to trajectory IDs.
std::unordered_map<const mapping::Submaps*, int> trajectory_ids_
GUARDED_BY(mutex_);
};
} // namespace mapping_2d

7
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -395,9 +395,16 @@ SparsePoseGraph::GetSubmapStates() {
}
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
common::MutexLocker locker(&mutex_);
return constraints_;
}
const std::unordered_map<const mapping::Submaps*, int>&
SparsePoseGraph::trajectory_ids() {
common::MutexLocker locker(&mutex_);
return trajectory_ids_;
}
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
const mapping::Submaps* const submaps) {
const auto extrapolated_submap_transforms = GetSubmapTransforms(submaps);

14
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -93,8 +93,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
EXCLUDES(mutex_) override;
std::vector<mapping::TrajectoryNode> GetTrajectoryNodes() override
EXCLUDES(mutex_);
std::vector<SubmapState> GetSubmapStates() override;
std::vector<Constraint> constraints() override;
protected:
std::vector<SubmapState> GetSubmapStates() EXCLUDES(mutex_) override;
std::vector<Constraint> constraints() EXCLUDES(mutex_) override;
const std::unordered_map<const mapping::Submaps*, int>& trajectory_ids()
EXCLUDES(mutex_) override;
private:
// This is 'mapping::SubmapState', but with the 3D versions of 'submap' and
@ -187,7 +191,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Current optimization problem.
sparse_pose_graph::OptimizationProblem optimization_problem_;
sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
std::vector<Constraint> constraints_;
std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
// Submaps get assigned an index and state as soon as they are seen, even
// before they take part in the background computations.
@ -209,6 +213,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Current submap transforms used for displaying data.
std::vector<transform::Rigid3d> optimized_submap_transforms_
GUARDED_BY(mutex_);
// Map from submap pointers to trajectory IDs.
std::unordered_map<const mapping::Submaps*, int> trajectory_ids_
GUARDED_BY(mutex_);
};
} // namespace mapping_3d