Improve code structure for extrapolation of poses. (#337)

This is in preparation of extrapolating single submap poses
as needed.
master
Wolfgang Hess 2017-06-14 12:49:17 +02:00 committed by GitHub
parent 30de09bee8
commit 79eeb7d77a
4 changed files with 77 additions and 84 deletions

View File

@ -377,17 +377,16 @@ void SparsePoseGraph::RunOptimization() {
node_data[trajectory_id][node_index].point_cloud_pose); node_data[trajectory_id][node_index].point_cloud_pose);
} }
// Extrapolate all point cloud poses that were added later. // Extrapolate all point cloud poses that were added later.
const auto new_submap_transforms = ExtrapolateSubmapTransforms( const auto local_to_new_global = ComputeLocalToGlobalTransform(
optimization_problem_.submap_data(), trajectory_id); optimization_problem_.submap_data(), trajectory_id);
const auto old_submap_transforms = ExtrapolateSubmapTransforms( const auto local_to_old_global = ComputeLocalToGlobalTransform(
optimized_submap_transforms_, trajectory_id); optimized_submap_transforms_, trajectory_id);
CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size()); const transform::Rigid3d old_global_to_new_global =
const transform::Rigid3d extrapolation_transform = local_to_new_global * local_to_old_global.inverse();
new_submap_transforms.back() * old_submap_transforms.back().inverse();
for (; node_index < num_nodes; ++node_index) { for (; node_index < num_nodes; ++node_index) {
const mapping::NodeId node_id{trajectory_id, node_index}; const mapping::NodeId node_id{trajectory_id, node_index};
trajectory_nodes_.at(node_id).pose = trajectory_nodes_.at(node_id).pose =
extrapolation_transform * trajectory_nodes_.at(node_id).pose; old_global_to_new_global * trajectory_nodes_.at(node_id).pose;
} }
} }
optimized_submap_transforms_ = optimization_problem_.submap_data(); optimized_submap_transforms_ = optimization_problem_.submap_data();
@ -419,19 +418,8 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
const int trajectory_id) { const int trajectory_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
if (submap_data_.num_trajectories() <= trajectory_id || return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
submap_data_.num_indices(trajectory_id) == 0) { trajectory_id);
return transform::Rigid3d::Identity();
}
const auto extrapolated_submap_transforms =
ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory_id);
return extrapolated_submap_transforms.back() *
submap_data_
.at(mapping::SubmapId{
trajectory_id,
static_cast<int>(extrapolated_submap_transforms.size()) - 1})
.submap->local_pose()
.inverse();
} }
std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() { std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
@ -442,14 +430,6 @@ std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms( std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
const int trajectory_id) { const int trajectory_id) {
common::MutexLocker locker(&mutex_); 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 int trajectory_id) const {
if (trajectory_id >= submap_data_.num_trajectories()) { if (trajectory_id >= submap_data_.num_trajectories()) {
return {transform::Rigid3d::Identity()}; return {transform::Rigid3d::Identity()};
} }
@ -461,22 +441,20 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
++submap_index) { ++submap_index) {
const mapping::SubmapId submap_id{trajectory_id, submap_index}; const mapping::SubmapId submap_id{trajectory_id, submap_index};
const auto& submap_data = submap_data_.at(submap_id); const auto& submap_data = submap_data_.at(submap_id);
if (static_cast<size_t>(trajectory_id) < submap_transforms.size() && if (static_cast<size_t>(trajectory_id) <
result.size() < submap_transforms.at(trajectory_id).size()) { optimized_submap_transforms_.size() &&
result.size() < optimized_submap_transforms_.at(trajectory_id).size()) {
// Submaps for which we have optimized poses. // Submaps for which we have optimized poses.
result.push_back( result.push_back(
Embed3D(submap_transforms.at(trajectory_id).at(result.size()).pose)); transform::Embed3D(optimized_submap_transforms_.at(trajectory_id)
} else if (result.empty()) { .at(result.size())
result.push_back(transform::Rigid3d::Identity()); .pose));
} else { } else {
// Extrapolate to the remaining submaps. Accessing 'local_pose' in // Extrapolate to the remaining submaps. Accessing 'local_pose' in Submap
// Submaps is okay, since the member is const. // is okay, since the member is const.
const mapping::SubmapId previous_submap_id{ result.push_back(ComputeLocalToGlobalTransform(
trajectory_id, static_cast<int>(result.size()) - 1}; optimized_submap_transforms_, trajectory_id) *
result.push_back( submap_data.submap->local_pose());
result.back() *
submap_data_.at(previous_submap_id).submap->local_pose().inverse() *
submap_data.submap->local_pose());
} }
} }
@ -486,6 +464,24 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
return result; return result;
} }
transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
submap_transforms,
const int trajectory_id) const {
if (trajectory_id >= static_cast<int>(submap_transforms.size()) ||
submap_transforms.at(trajectory_id).empty()) {
return transform::Rigid3d::Identity();
}
const mapping::SubmapId last_optimized_submap_id{
trajectory_id,
static_cast<int>(submap_transforms.at(trajectory_id).size() - 1)};
// Accessing 'local_pose' in Submap is okay, since the member is const.
return transform::Embed3D(submap_transforms.at(trajectory_id).back().pose) *
submap_data_.at(last_optimized_submap_id)
.submap->local_pose()
.inverse();
}
SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent) SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent)
: parent_(parent) {} : parent_(parent) {}

View File

@ -149,8 +149,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// optimization being run at a time. // optimization being run at a time.
void RunOptimization() EXCLUDES(mutex_); void RunOptimization() EXCLUDES(mutex_);
// Adds extrapolated transforms, so that there are transforms for all submaps. // Computes the local to global frame transform based on the given optimized
std::vector<transform::Rigid3d> ExtrapolateSubmapTransforms( // 'submap_transforms'.
transform::Rigid3d ComputeLocalToGlobalTransform(
const std::vector<std::vector<sparse_pose_graph::SubmapData>>& const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
submap_transforms, submap_transforms,
int trajectory_id) const REQUIRES(mutex_); int trajectory_id) const REQUIRES(mutex_);

View File

@ -385,17 +385,16 @@ void SparsePoseGraph::RunOptimization() {
node_data[trajectory_id][node_index].point_cloud_pose; node_data[trajectory_id][node_index].point_cloud_pose;
} }
// Extrapolate all point cloud poses that were added later. // Extrapolate all point cloud poses that were added later.
const auto new_submap_transforms = ExtrapolateSubmapTransforms( const auto local_to_new_global = ComputeLocalToGlobalTransform(
optimization_problem_.submap_data(), trajectory_id); optimization_problem_.submap_data(), trajectory_id);
const auto old_submap_transforms = ExtrapolateSubmapTransforms( const auto local_to_old_global = ComputeLocalToGlobalTransform(
optimized_submap_transforms_, trajectory_id); optimized_submap_transforms_, trajectory_id);
CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size()); const transform::Rigid3d old_global_to_new_global =
const transform::Rigid3d extrapolation_transform = local_to_new_global * local_to_old_global.inverse();
new_submap_transforms.back() * old_submap_transforms.back().inverse();
for (; node_index < num_nodes; ++node_index) { for (; node_index < num_nodes; ++node_index) {
const mapping::NodeId node_id{trajectory_id, node_index}; const mapping::NodeId node_id{trajectory_id, node_index};
trajectory_nodes_.at(node_id).pose = trajectory_nodes_.at(node_id).pose =
extrapolation_transform * trajectory_nodes_.at(node_id).pose; old_global_to_new_global * trajectory_nodes_.at(node_id).pose;
} }
} }
optimized_submap_transforms_ = optimization_problem_.submap_data(); optimized_submap_transforms_ = optimization_problem_.submap_data();
@ -422,19 +421,8 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
const int trajectory_id) { const int trajectory_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
if (submap_data_.num_trajectories() <= trajectory_id || return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
submap_data_.num_indices(trajectory_id) == 0) { trajectory_id);
return transform::Rigid3d::Identity();
}
const auto extrapolated_submap_transforms =
ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory_id);
return extrapolated_submap_transforms.back() *
submap_data_
.at(mapping::SubmapId{
trajectory_id,
static_cast<int>(extrapolated_submap_transforms.size()) - 1})
.submap->local_pose()
.inverse();
} }
std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() { std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
@ -445,14 +433,6 @@ std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms( std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
const int trajectory_id) { const int trajectory_id) {
common::MutexLocker locker(&mutex_); 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 int trajectory_id) const {
if (trajectory_id >= submap_data_.num_trajectories()) { if (trajectory_id >= submap_data_.num_trajectories()) {
return {transform::Rigid3d::Identity()}; return {transform::Rigid3d::Identity()};
} }
@ -464,22 +444,19 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
++submap_index) { ++submap_index) {
const mapping::SubmapId submap_id{trajectory_id, submap_index}; const mapping::SubmapId submap_id{trajectory_id, submap_index};
const auto& submap_data = submap_data_.at(submap_id); const auto& submap_data = submap_data_.at(submap_id);
if (static_cast<size_t>(trajectory_id) < submap_transforms.size() && if (static_cast<size_t>(trajectory_id) <
result.size() < submap_transforms.at(trajectory_id).size()) { optimized_submap_transforms_.size() &&
result.size() < optimized_submap_transforms_.at(trajectory_id).size()) {
// Submaps for which we have optimized poses. // Submaps for which we have optimized poses.
result.push_back( result.push_back(optimized_submap_transforms_.at(trajectory_id)
submap_transforms.at(trajectory_id).at(result.size()).pose); .at(result.size())
} else if (result.empty()) { .pose);
result.push_back(transform::Rigid3d::Identity());
} else { } else {
// Extrapolate to the remaining submaps. Accessing 'local_pose' in Submaps // Extrapolate to the remaining submaps. Accessing 'local_pose' in Submap
// is okay, since the member is const. // is okay, since the member is const.
const mapping::SubmapId previous_submap_id{ result.push_back(ComputeLocalToGlobalTransform(
trajectory_id, static_cast<int>(result.size()) - 1}; optimized_submap_transforms_, trajectory_id) *
result.push_back( submap_data.submap->local_pose());
result.back() *
submap_data_.at(previous_submap_id).submap->local_pose().inverse() *
submap_data.submap->local_pose());
} }
} }
@ -489,5 +466,23 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
return result; return result;
} }
transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
submap_transforms,
const int trajectory_id) const {
if (trajectory_id >= static_cast<int>(submap_transforms.size()) ||
submap_transforms.at(trajectory_id).empty()) {
return transform::Rigid3d::Identity();
}
const mapping::SubmapId last_optimized_submap_id{
trajectory_id,
static_cast<int>(submap_transforms.at(trajectory_id).size() - 1)};
// Accessing 'local_pose' in Submap is okay, since the member is const.
return submap_transforms.at(trajectory_id).back().pose *
submap_data_.at(last_optimized_submap_id)
.submap->local_pose()
.inverse();
}
} // namespace mapping_3d } // namespace mapping_3d
} // namespace cartographer } // namespace cartographer

View File

@ -146,8 +146,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// optimization being run at a time. // optimization being run at a time.
void RunOptimization() EXCLUDES(mutex_); void RunOptimization() EXCLUDES(mutex_);
// Adds extrapolated transforms, so that there are transforms for all submaps. // Computes the local to global frame transform based on the given optimized
std::vector<transform::Rigid3d> ExtrapolateSubmapTransforms( // 'submap_transforms'.
transform::Rigid3d ComputeLocalToGlobalTransform(
const std::vector<std::vector<sparse_pose_graph::SubmapData>>& const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
submap_transforms, submap_transforms,
int trajectory_id) const REQUIRES(mutex_); int trajectory_id) const REQUIRES(mutex_);