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

ConstantData now has a trajectory_id. (#289) 

Related to #256.

PAIR=wohe
master
Holger Rapp 2017-05-16 11:42:18 +02:00 committed by GitHub
parent 5effc4dac7
commit 8612f5e6e5
7 changed files with 67 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cartographer/mapping/sparse_pose_graph.cc
View File

@ -93,9 +93,8 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
}
if (!trajectory_nodes.empty()) {
const Submaps* const trajectory =
trajectory_nodes[0].constant_data->trajectory;
for (const auto& transform : GetSubmapTransforms(trajectory)) {
for (const auto& transform : GetSubmapTransforms(
trajectory_nodes[0].constant_data->trajectory_id)) {
*trajectory_proto->add_submap()->mutable_pose() =
transform::ToProto(transform);
}

5
cartographer/mapping/sparse_pose_graph.h
View File

@ -72,9 +72,14 @@ class SparsePoseGraph {
virtual std::vector<std::vector<int>> GetConnectedTrajectories() = 0;
// Returns the current optimized transforms for the given 'trajectory'.
// TODO(hrapp): Remove this version.
virtual std::vector<transform::Rigid3d> GetSubmapTransforms(
const Submaps* trajectory) = 0;
// Returns the current optimized transforms for the given 'trajectory_id'.
virtual std::vector<transform::Rigid3d> GetSubmapTransforms(
int trajectory_id) = 0;
// Returns the transform converting data in the local map frame (i.e. the
// continuous, non-loop-closed frame) into the global map frame (i.e. the
// discontinuous, loop-closed frame).

4
cartographer/mapping/trajectory_node.h
View File

@ -41,9 +41,7 @@ struct TrajectoryNode {
sensor::CompressedRangeData range_data_3d;
// Trajectory this node belongs to.
// TODO(macmason): The naming here is confusing because 'trajectory'
// doesn't seem like a good name for a Submaps*. Sort this out.
const Submaps* trajectory;
int trajectory_id;
// Transform from the 3D 'tracking' frame to the 'pose' frame of the range
// data, which contains roll, pitch and height for 2D. In 3D this is always

52
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -105,8 +105,8 @@ void SparsePoseGraph::AddScan(
constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
time, range_data_in_pose,
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), trajectory,
tracking_to_pose});
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}),
trajectory_id, tracking_to_pose});
trajectory_nodes_.push_back(mapping::TrajectoryNode{
&constant_node_data_.back(), optimized_pose,
});
@ -174,9 +174,8 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
.at(node_id.node_index)
.point_cloud_pose;
const mapping::Submaps* const scan_trajectory =
trajectory_nodes_[scan_index].constant_data->trajectory;
const int scan_trajectory_id = trajectory_ids_.at(scan_trajectory);
const int scan_trajectory_id =
trajectory_nodes_[scan_index].constant_data->trajectory_id;
// Only globally match against submaps not in this trajectory.
if (scan_trajectory_id != submap_id.trajectory_id &&
@ -242,8 +241,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
++num_nodes_in_trajectory_[matching_id.trajectory_id];
const mapping::TrajectoryNode::ConstantData* const scan_data =
trajectory_nodes_[scan_index].constant_data;
CHECK_EQ(trajectory_ids_.at(scan_data->trajectory),
matching_id.trajectory_id);
CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
optimization_problem_.AddTrajectoryNode(
matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
for (const mapping::Submap* submap : insertion_submaps) {
@ -396,23 +394,21 @@ void SparsePoseGraph::RunOptimization() {
.point_cloud_pose);
}
// Extrapolate all point cloud poses that were added later.
std::unordered_map<const mapping::Submaps*, transform::Rigid3d>
extrapolation_transforms;
std::unordered_map<int, transform::Rigid3d> extrapolation_transforms;
for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) {
const mapping::Submaps* trajectory =
trajectory_nodes_[i].constant_data->trajectory;
if (extrapolation_transforms.count(trajectory) == 0) {
const int trajectory_id = trajectory_nodes_[i].constant_data->trajectory_id;
if (extrapolation_transforms.count(trajectory_id) == 0) {
const auto new_submap_transforms = ExtrapolateSubmapTransforms(
optimization_problem_.submap_data(), trajectory);
const auto old_submap_transforms =
ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory);
optimization_problem_.submap_data(), trajectory_id);
const auto old_submap_transforms = ExtrapolateSubmapTransforms(
optimized_submap_transforms_, trajectory_id);
CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size());
extrapolation_transforms[trajectory] =
extrapolation_transforms[trajectory_id] =
transform::Rigid3d(new_submap_transforms.back() *
old_submap_transforms.back().inverse());
}
trajectory_nodes_[i].pose =
extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose;
extrapolation_transforms[trajectory_id] * trajectory_nodes_[i].pose;
}
optimized_submap_transforms_ = optimization_problem_.submap_data();
connected_components_ = trajectory_connectivity_.ConnectedComponents();
@ -430,8 +426,7 @@ SparsePoseGraph::GetTrajectoryNodes() {
std::vector<std::vector<mapping::TrajectoryNode>> result(
trajectory_ids_.size());
for (const auto& node : trajectory_nodes_) {
result.at(trajectory_ids_.at(node.constant_data->trajectory))
.push_back(node);
result.at(node.constant_data->trajectory_id).push_back(node);
}
return result;
}
@ -458,17 +453,24 @@ std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
const mapping::Submaps* const trajectory) {
common::MutexLocker locker(&mutex_);
return ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory);
if (trajectory_ids_.count(trajectory) == 0) {
return {transform::Rigid3d::Identity()};
}
return ExtrapolateSubmapTransforms(optimized_submap_transforms_,
trajectory_ids_.at(trajectory));
}
std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
const int trajectory_id) {
common::MutexLocker locker(&mutex_);
return ExtrapolateSubmapTransforms(optimized_submap_transforms_,
trajectory_id);
}
std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
submap_transforms,
const mapping::Submaps* const trajectory) const {
if (trajectory_ids_.count(trajectory) == 0) {
return {transform::Rigid3d::Identity()};
}
const size_t trajectory_id = trajectory_ids_.at(trajectory);
const size_t trajectory_id) const {
if (trajectory_id >= submap_states_.size()) {
return {transform::Rigid3d::Identity()};
}

4
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -84,6 +84,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
std::vector<std::vector<int>> GetConnectedTrajectories() override;
std::vector<transform::Rigid3d> GetSubmapTransforms(
const mapping::Submaps* trajectory) EXCLUDES(mutex_) override;
std::vector<transform::Rigid3d> GetSubmapTransforms(int trajectory_id)
EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps)
EXCLUDES(mutex_) override;
std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
@ -153,7 +155,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
std::vector<transform::Rigid3d> ExtrapolateSubmapTransforms(
const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
submap_transforms,
const mapping::Submaps* trajectory) const REQUIRES(mutex_);
size_t trajectory_id) const REQUIRES(mutex_);
const mapping::proto::SparsePoseGraphOptions options_;
common::Mutex mutex_;

51
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -103,7 +103,7 @@ void SparsePoseGraph::AddScan(
constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
sensor::Compress(range_data_in_tracking), trajectory,
sensor::Compress(range_data_in_tracking), trajectory_id,
transform::Rigid3d::Identity()});
trajectory_nodes_.push_back(
mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose});
@ -173,10 +173,8 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
.at(node_id.trajectory_id)
.at(node_id.node_index)
.point_cloud_pose;
const mapping::Submaps* const scan_trajectory =
trajectory_nodes_[scan_index].constant_data->trajectory;
const int scan_trajectory_id = trajectory_ids_.at(scan_trajectory);
const int scan_trajectory_id =
trajectory_nodes_[scan_index].constant_data->trajectory_id;
// Only globally match against submaps not in this trajectory.
if (scan_trajectory_id != submap_id.trajectory_id &&
@ -238,8 +236,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
++num_nodes_in_trajectory_[matching_id.trajectory_id];
const mapping::TrajectoryNode::ConstantData* const scan_data =
trajectory_nodes_[scan_index].constant_data;
CHECK_EQ(trajectory_ids_.at(scan_data->trajectory),
matching_id.trajectory_id);
CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
scan_data->time, optimized_pose);
for (const Submap* submap : insertion_submaps) {
@ -387,23 +384,21 @@ void SparsePoseGraph::RunOptimization() {
.point_cloud_pose;
}
// Extrapolate all point cloud poses that were added later.
std::unordered_map<const mapping::Submaps*, transform::Rigid3d>
extrapolation_transforms;
std::unordered_map<int, transform::Rigid3d> extrapolation_transforms;
for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) {
const mapping::Submaps* trajectory =
trajectory_nodes_[i].constant_data->trajectory;
if (extrapolation_transforms.count(trajectory) == 0) {
const int trajectory_id = trajectory_nodes_[i].constant_data->trajectory_id;
if (extrapolation_transforms.count(trajectory_id) == 0) {
const auto new_submap_transforms = ExtrapolateSubmapTransforms(
optimization_problem_.submap_data(), trajectory);
const auto old_submap_transforms =
ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory);
optimization_problem_.submap_data(), trajectory_id);
const auto old_submap_transforms = ExtrapolateSubmapTransforms(
optimized_submap_transforms_, trajectory_id);
CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size());
extrapolation_transforms[trajectory] =
extrapolation_transforms[trajectory_id] =
transform::Rigid3d(new_submap_transforms.back() *
old_submap_transforms.back().inverse());
}
trajectory_nodes_[i].pose =
extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose;
extrapolation_transforms[trajectory_id] * trajectory_nodes_[i].pose;
}
optimized_submap_transforms_ = optimization_problem_.submap_data();
connected_components_ = trajectory_connectivity_.ConnectedComponents();
@ -421,8 +416,7 @@ SparsePoseGraph::GetTrajectoryNodes() {
std::vector<std::vector<mapping::TrajectoryNode>> result(
trajectory_ids_.size());
for (const auto& node : trajectory_nodes_) {
result.at(trajectory_ids_.at(node.constant_data->trajectory))
.push_back(node);
result.at(node.constant_data->trajectory_id).push_back(node);
}
return result;
}
@ -449,17 +443,24 @@ std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
const mapping::Submaps* const trajectory) {
common::MutexLocker locker(&mutex_);
return ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory);
if (trajectory_ids_.count(trajectory) == 0) {
return {transform::Rigid3d::Identity()};
}
return ExtrapolateSubmapTransforms(optimized_submap_transforms_,
trajectory_ids_.at(trajectory));
}
std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
const int trajectory_id) {
common::MutexLocker locker(&mutex_);
return ExtrapolateSubmapTransforms(optimized_submap_transforms_,
trajectory_id);
}
std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
submap_transforms,
const mapping::Submaps* const trajectory) const {
if (trajectory_ids_.count(trajectory) == 0) {
return {transform::Rigid3d::Identity()};
}
const size_t trajectory_id = trajectory_ids_.at(trajectory);
const size_t trajectory_id) const {
if (trajectory_id >= submap_states_.size()) {
return {transform::Rigid3d::Identity()};
}

4
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -86,6 +86,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
std::vector<std::vector<int>> GetConnectedTrajectories() override;
std::vector<transform::Rigid3d> GetSubmapTransforms(
const mapping::Submaps* trajectory) EXCLUDES(mutex_) override;
std::vector<transform::Rigid3d> GetSubmapTransforms(int trajectory_id)
EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps)
EXCLUDES(mutex_) override;
std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
@ -153,7 +155,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
std::vector<transform::Rigid3d> ExtrapolateSubmapTransforms(
const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
submap_transforms,
const mapping::Submaps* trajectory) const REQUIRES(mutex_);
size_t trajectory_id) const REQUIRES(mutex_);
const mapping::proto::SparsePoseGraphOptions options_;
common::Mutex mutex_;