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

Support extrapolating a single submap transform. (#338)

master
Wolfgang Hess 2017-06-14 15:10:36 +02:00 committed by GitHub
parent 79eeb7d77a
commit c1e1d03636
7 changed files with 63 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cartographer/mapping/map_builder.cc
View File

@ -113,20 +113,20 @@ string MapBuilder::SubmapToProto(const int trajectory_id,
" trajectories.";
}
const std::vector<transform::Rigid3d> submap_transforms =
sparse_pose_graph_->GetSubmapTransforms(trajectory_id);
if (submap_index < 0 ||
static_cast<size_t>(submap_index) >= submap_transforms.size()) {
const int num_submaps = sparse_pose_graph_->num_submaps(trajectory_id);
if (submap_index < 0 || submap_index >= num_submaps) {
return "Requested submap " + std::to_string(submap_index) +
" from trajectory " + std::to_string(trajectory_id) +
" but there are only " + std::to_string(submap_transforms.size()) +
" but there are only " + std::to_string(num_submaps) +
" submaps in this trajectory.";
}
const Submap* const submap =
trajectory_builders_.at(trajectory_id)->submaps()->Get(submap_index);
response->set_submap_version(submap->num_range_data());
submap->ToResponseProto(submap_transforms[submap_index], response);
const auto submap_pose = sparse_pose_graph_->GetSubmapTransform(
SubmapId{trajectory_id, submap_index});
submap->ToResponseProto(submap_pose, response);
return "";
}

7
cartographer/mapping/sparse_pose_graph.cc
View File

@ -86,9 +86,12 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
}
if (!single_trajectory_nodes.empty()) {
for (const auto& transform : GetSubmapTransforms(trajectory_id)) {
const int num_submaps_in_trajectory = num_submaps(trajectory_id);
for (int submap_index = 0; submap_index != num_submaps_in_trajectory;
++submap_index) {
const SubmapId submap_id{static_cast<int>(trajectory_id), submap_index};
*trajectory_proto->add_submap()->mutable_pose() =
transform::ToProto(transform);
transform::ToProto(GetSubmapTransform(submap_id));
}
}
}

12
cartographer/mapping/sparse_pose_graph.h
View File

@ -49,8 +49,8 @@ class SparsePoseGraph {
double rotation_weight;
};
mapping::SubmapId submap_id; // 'i' in the paper.
mapping::NodeId node_id; // 'j' in the paper.
SubmapId submap_id; // 'i' in the paper.
NodeId node_id; // 'j' in the paper.
// Pose of the scan 'j' relative to submap 'i'.
Pose pose;
@ -77,9 +77,11 @@ class SparsePoseGraph {
// Gets the current trajectory clusters.
virtual std::vector<std::vector<int>> GetConnectedTrajectories() = 0;
// Returns the current optimized transforms for the given 'trajectory_id'.
virtual std::vector<transform::Rigid3d> GetSubmapTransforms(
int trajectory_id) = 0;
// Return the number of submaps for the given 'trajectory_id'.
virtual int num_submaps(int trajectory_id) = 0;
// Returns the current optimized transform for the given 'submap_id'.
virtual transform::Rigid3d GetSubmapTransform(const SubmapId& submap_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

51
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -427,41 +427,32 @@ std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
return connected_components_;
}
std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
const int trajectory_id) {
int SparsePoseGraph::num_submaps(const int trajectory_id) {
common::MutexLocker locker(&mutex_);
if (trajectory_id >= submap_data_.num_trajectories()) {
return {transform::Rigid3d::Identity()};
return 0;
}
return submap_data_.num_indices(trajectory_id);
}
// Submaps for which we have optimized poses.
std::vector<transform::Rigid3d> result;
for (int submap_index = 0;
submap_index != submap_data_.num_indices(trajectory_id);
++submap_index) {
const mapping::SubmapId submap_id{trajectory_id, submap_index};
const auto& submap_data = submap_data_.at(submap_id);
if (static_cast<size_t>(trajectory_id) <
optimized_submap_transforms_.size() &&
result.size() < optimized_submap_transforms_.at(trajectory_id).size()) {
// Submaps for which we have optimized poses.
result.push_back(
transform::Embed3D(optimized_submap_transforms_.at(trajectory_id)
.at(result.size())
.pose));
} else {
// Extrapolate to the remaining submaps. Accessing 'local_pose' in Submap
// is okay, since the member is const.
result.push_back(ComputeLocalToGlobalTransform(
optimized_submap_transforms_, trajectory_id) *
submap_data.submap->local_pose());
transform::Rigid3d SparsePoseGraph::GetSubmapTransform(
const mapping::SubmapId& submap_id) {
common::MutexLocker locker(&mutex_);
// We already have an optimized pose.
if (submap_id.trajectory_id <
static_cast<int>(optimized_submap_transforms_.size()) &&
submap_id.submap_index < static_cast<int>(optimized_submap_transforms_
.at(submap_id.trajectory_id)
.size())) {
return transform::Embed3D(
optimized_submap_transforms_.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.pose);
}
}
if (result.empty()) {
result.push_back(transform::Rigid3d::Identity());
}
return result;
// We have to extrapolate.
return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
submap_id.trajectory_id) *
submap_data_.at(submap_id).submap->local_pose();
}
transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(

3
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -82,7 +82,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() override;
std::vector<transform::Rigid3d> GetSubmapTransforms(int trajectory_id)
int num_submaps(int trajectory_id) EXCLUDES(mutex_) override;
transform::Rigid3d GetSubmapTransform(const mapping::SubmapId& submap_id)
EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
EXCLUDES(mutex_) override;

49
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -430,40 +430,31 @@ std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
return connected_components_;
}
std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
const int trajectory_id) {
int SparsePoseGraph::num_submaps(const int trajectory_id) {
common::MutexLocker locker(&mutex_);
if (trajectory_id >= submap_data_.num_trajectories()) {
return {transform::Rigid3d::Identity()};
return 0;
}
return submap_data_.num_indices(trajectory_id);
}
// Submaps for which we have optimized poses.
std::vector<transform::Rigid3d> result;
for (int submap_index = 0;
submap_index != submap_data_.num_indices(trajectory_id);
++submap_index) {
const mapping::SubmapId submap_id{trajectory_id, submap_index};
const auto& submap_data = submap_data_.at(submap_id);
if (static_cast<size_t>(trajectory_id) <
optimized_submap_transforms_.size() &&
result.size() < optimized_submap_transforms_.at(trajectory_id).size()) {
// Submaps for which we have optimized poses.
result.push_back(optimized_submap_transforms_.at(trajectory_id)
.at(result.size())
.pose);
} else {
// Extrapolate to the remaining submaps. Accessing 'local_pose' in Submap
// is okay, since the member is const.
result.push_back(ComputeLocalToGlobalTransform(
optimized_submap_transforms_, trajectory_id) *
submap_data.submap->local_pose());
transform::Rigid3d SparsePoseGraph::GetSubmapTransform(
const mapping::SubmapId& submap_id) {
common::MutexLocker locker(&mutex_);
// We already have an optimized pose.
if (submap_id.trajectory_id <
static_cast<int>(optimized_submap_transforms_.size()) &&
submap_id.submap_index < static_cast<int>(optimized_submap_transforms_
.at(submap_id.trajectory_id)
.size())) {
return optimized_submap_transforms_.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.pose;
}
}
if (result.empty()) {
result.push_back(transform::Rigid3d::Identity());
}
return result;
// We have to extrapolate.
return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
submap_id.trajectory_id) *
submap_data_.at(submap_id).submap->local_pose();
}
transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(

3
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -81,7 +81,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
}
void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() override;
std::vector<transform::Rigid3d> GetSubmapTransforms(int trajectory_id)
int num_submaps(int trajectory_id) EXCLUDES(mutex_) override;
transform::Rigid3d GetSubmapTransform(const mapping::SubmapId& submap_id)
EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
EXCLUDES(mutex_) override;