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

View File

@ -113,20 +113,20 @@ string MapBuilder::SubmapToProto(const int trajectory_id,
" trajectories."; " trajectories.";
} }
const std::vector<transform::Rigid3d> submap_transforms = const int num_submaps = sparse_pose_graph_->num_submaps(trajectory_id);
sparse_pose_graph_->GetSubmapTransforms(trajectory_id); if (submap_index < 0 || submap_index >= num_submaps) {
if (submap_index < 0 ||
static_cast<size_t>(submap_index) >= submap_transforms.size()) {
return "Requested submap " + std::to_string(submap_index) + return "Requested submap " + std::to_string(submap_index) +
" from trajectory " + std::to_string(trajectory_id) + " 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."; " submaps in this trajectory.";
} }
const Submap* const submap = const Submap* const submap =
trajectory_builders_.at(trajectory_id)->submaps()->Get(submap_index); trajectory_builders_.at(trajectory_id)->submaps()->Get(submap_index);
response->set_submap_version(submap->num_range_data()); 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 ""; return "";
} }

View File

@ -86,9 +86,12 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
} }
if (!single_trajectory_nodes.empty()) { 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() = *trajectory_proto->add_submap()->mutable_pose() =
transform::ToProto(transform); transform::ToProto(GetSubmapTransform(submap_id));
} }
} }
} }

View File

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

View File

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

View File

@ -82,7 +82,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override; void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
void RunFinalOptimization() override; void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() 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; EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
EXCLUDES(mutex_) override; EXCLUDES(mutex_) override;

View File

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

View File

@ -81,7 +81,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
} }
void RunFinalOptimization() override; void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() 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; EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
EXCLUDES(mutex_) override; EXCLUDES(mutex_) override;