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

Use per-trajectory SubmapData in OptimizationProblem. (#272)

master
Wolfgang Hess 2017-05-10 16:28:58 +02:00 committed by GitHub
parent 94e8eec41d
commit 926b0320cb
8 changed files with 167 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -54,31 +54,32 @@ SparsePoseGraph::~SparsePoseGraph() {
void SparsePoseGraph::GrowSubmapTransformsAsNeeded( void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
const std::vector<const mapping::Submap*>& insertion_submaps) { const std::vector<const mapping::Submap*>& insertion_submaps) {
CHECK(!insertion_submaps.empty()); CHECK(!insertion_submaps.empty());
CHECK_LT(optimization_problem_.submap_data().size(), const mapping::SubmapId first_submap_id = GetSubmapId(insertion_submaps[0]);
std::numeric_limits<int>::max()); const int trajectory_id = first_submap_id.trajectory_id;
const int next_submap_index = optimization_problem_.submap_data().size(); CHECK_GE(trajectory_id, 0);
// Verify that we have an index for the first submap. const auto& submap_data = optimization_problem_.submap_data();
const int first_submap_index = GetSubmapIndex(insertion_submaps[0]);
CHECK_LE(first_submap_index, next_submap_index);
if (insertion_submaps.size() == 1) { if (insertion_submaps.size() == 1) {
// If we don't already have an entry for this submap, add one. // If we don't already have an entry for the first submap, add one.
if (first_submap_index == next_submap_index) { CHECK_EQ(first_submap_id.submap_index, 0);
optimization_problem_.AddSubmap( if (static_cast<size_t>(trajectory_id) >= submap_data.size() ||
submap_states_[first_submap_index].id.trajectory_id, submap_data[trajectory_id].empty()) {
transform::Rigid2d::Identity()); optimization_problem_.AddSubmap(trajectory_id,
transform::Rigid2d::Identity());
} }
return; return;
} }
CHECK_EQ(2, insertion_submaps.size()); CHECK_EQ(2, insertion_submaps.size());
const int next_submap_index = submap_data.at(trajectory_id).size();
// CHECK that we have a index for the second submap. // CHECK that we have a index for the second submap.
const int second_submap_index = GetSubmapIndex(insertion_submaps[1]); const mapping::SubmapId second_submap_id = GetSubmapId(insertion_submaps[1]);
CHECK_LE(second_submap_index, next_submap_index); CHECK_EQ(second_submap_id.trajectory_id, trajectory_id);
CHECK_LE(second_submap_id.submap_index, next_submap_index);
// Extrapolate if necessary. // Extrapolate if necessary.
if (second_submap_index == next_submap_index) { if (second_submap_id.submap_index == next_submap_index) {
const auto& first_submap_pose = const auto& first_submap_pose =
optimization_problem_.submap_data().at(first_submap_index).pose; submap_data.at(trajectory_id).at(first_submap_id.submap_index).pose;
optimization_problem_.AddSubmap( optimization_problem_.AddSubmap(
submap_states_[second_submap_index].id.trajectory_id, trajectory_id,
first_submap_pose * first_submap_pose *
sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0]) sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0])
.inverse() * .inverse() *
@ -160,8 +161,12 @@ void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
void SparsePoseGraph::ComputeConstraint(const int scan_index, void SparsePoseGraph::ComputeConstraint(const int scan_index,
const int submap_index) { const int submap_index) {
const mapping::SubmapId submap_id = submap_states_[submap_index].id;
const transform::Rigid2d relative_pose = const transform::Rigid2d relative_pose =
optimization_problem_.submap_data().at(submap_index).pose.inverse() * optimization_problem_.submap_data()
.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.pose.inverse() *
optimization_problem_.node_data().at(scan_index).point_cloud_pose; optimization_problem_.node_data().at(scan_index).point_cloud_pose;
const mapping::Submaps* const scan_trajectory = const mapping::Submaps* const scan_trajectory =
@ -169,8 +174,6 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
const mapping::Submaps* const submap_trajectory = const mapping::Submaps* const submap_trajectory =
submap_states_[submap_index].trajectory; submap_states_[submap_index].trajectory;
const mapping::SubmapId submap_id = submap_states_[submap_index].id;
// Only globally match against submaps not in this trajectory. // Only globally match against submaps not in this trajectory.
if (scan_trajectory != submap_trajectory && if (scan_trajectory != submap_trajectory &&
global_localization_samplers_[scan_trajectory]->Pulse()) { global_localization_samplers_[scan_trajectory]->Pulse()) {
@ -214,9 +217,12 @@ void SparsePoseGraph::ComputeConstraintsForScan(
const mapping::Submap* finished_submap, const transform::Rigid2d& pose, const mapping::Submap* finished_submap, const transform::Rigid2d& pose,
const kalman_filter::Pose2DCovariance& covariance) { const kalman_filter::Pose2DCovariance& covariance) {
GrowSubmapTransformsAsNeeded(insertion_submaps); GrowSubmapTransformsAsNeeded(insertion_submaps);
const int matching_index = GetSubmapIndex(matching_submap); const mapping::SubmapId matching_id = GetSubmapId(matching_submap);
const transform::Rigid2d optimized_pose = const transform::Rigid2d optimized_pose =
optimization_problem_.submap_data().at(matching_index).pose * optimization_problem_.submap_data()
.at(matching_id.trajectory_id)
.at(matching_id.submap_index)
.pose *
sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose; sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose;
CHECK_EQ(scan_index, optimization_problem_.node_data().size()); CHECK_EQ(scan_index, optimization_problem_.node_data().size());
const mapping::TrajectoryNode::ConstantData* const scan_data = const mapping::TrajectoryNode::ConstantData* const scan_data =
@ -358,16 +364,12 @@ void SparsePoseGraph::RunOptimization() {
// Extrapolate all point cloud poses that were added later. // Extrapolate all point cloud poses that were added later.
std::unordered_map<const mapping::Submaps*, transform::Rigid3d> std::unordered_map<const mapping::Submaps*, transform::Rigid3d>
extrapolation_transforms; extrapolation_transforms;
std::vector<transform::Rigid2d> submap_transforms;
for (const auto& submap_data : optimization_problem_.submap_data()) {
submap_transforms.push_back(submap_data.pose);
}
for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) { for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) {
const mapping::Submaps* trajectory = const mapping::Submaps* trajectory =
trajectory_nodes_[i].constant_data->trajectory; trajectory_nodes_[i].constant_data->trajectory;
if (extrapolation_transforms.count(trajectory) == 0) { if (extrapolation_transforms.count(trajectory) == 0) {
const auto new_submap_transforms = const auto new_submap_transforms = ExtrapolateSubmapTransforms(
ExtrapolateSubmapTransforms(submap_transforms, trajectory); optimization_problem_.submap_data(), trajectory);
const auto old_submap_transforms = const auto old_submap_transforms =
ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory); ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory);
CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size()); CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size());
@ -378,7 +380,7 @@ void SparsePoseGraph::RunOptimization() {
trajectory_nodes_[i].pose = trajectory_nodes_[i].pose =
extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose; extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose;
} }
optimized_submap_transforms_ = submap_transforms; optimized_submap_transforms_ = optimization_problem_.submap_data();
connected_components_ = trajectory_connectivity_.ConnectedComponents(); connected_components_ = trajectory_connectivity_.ConnectedComponents();
reverse_connected_components_.clear(); reverse_connected_components_.clear();
for (size_t i = 0; i != connected_components_.size(); ++i) { for (size_t i = 0; i != connected_components_.size(); ++i) {
@ -426,7 +428,8 @@ std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
} }
std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms( std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
const std::vector<transform::Rigid2d>& submap_transforms, const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
submap_transforms,
const mapping::Submaps* const trajectory) const { const mapping::Submaps* const trajectory) const {
std::vector<transform::Rigid3d> result; std::vector<transform::Rigid3d> result;
size_t flat_index = 0; size_t flat_index = 0;
@ -438,10 +441,13 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
if (state.trajectory != trajectory) { if (state.trajectory != trajectory) {
continue; continue;
} }
if (flat_index >= submap_transforms.size()) { const int trajectory_id = trajectory_ids_.at(trajectory);
if (static_cast<size_t>(trajectory_id) >= submap_transforms.size() ||
result.size() >= submap_transforms.at(trajectory_id).size()) {
break; break;
} }
result.push_back(transform::Embed3D(submap_transforms[flat_index])); result.push_back(transform::Embed3D(
submap_transforms.at(trajectory_id).at(result.size()).pose));
flat_index_of_result_back = flat_index; flat_index_of_result_back = flat_index;
} }

12
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -127,6 +127,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
return iterator->second; return iterator->second;
} }
mapping::SubmapId GetSubmapId(const mapping::Submap* submap) const
REQUIRES(mutex_) {
return submap_states_.at(GetSubmapIndex(submap)).id;
}
// Grows the optimization problem to have an entry for every element of // Grows the optimization problem to have an entry for every element of
// 'insertion_submaps'. // 'insertion_submaps'.
void GrowSubmapTransformsAsNeeded( void GrowSubmapTransformsAsNeeded(
@ -162,7 +167,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Adds extrapolated transforms, so that there are transforms for all submaps. // Adds extrapolated transforms, so that there are transforms for all submaps.
std::vector<transform::Rigid3d> ExtrapolateSubmapTransforms( std::vector<transform::Rigid3d> ExtrapolateSubmapTransforms(
const std::vector<transform::Rigid2d>& submap_transforms, const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
submap_transforms,
const mapping::Submaps* trajectory) const REQUIRES(mutex_); const mapping::Submaps* trajectory) const REQUIRES(mutex_);
const mapping::proto::SparsePoseGraphOptions options_; const mapping::proto::SparsePoseGraphOptions options_;
@ -212,8 +218,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
std::vector<mapping::TrajectoryNode> trajectory_nodes_ GUARDED_BY(mutex_); std::vector<mapping::TrajectoryNode> trajectory_nodes_ GUARDED_BY(mutex_);
// Current submap transforms used for displaying data. // Current submap transforms used for displaying data.
std::vector<transform::Rigid2d> optimized_submap_transforms_ std::vector<std::vector<sparse_pose_graph::SubmapData>>
GUARDED_BY(mutex_); optimized_submap_transforms_ GUARDED_BY(mutex_);
// Map from submap pointers to trajectory IDs. // Map from submap pointers to trajectory IDs.
std::unordered_map<const mapping::Submaps*, int> trajectory_ids_ std::unordered_map<const mapping::Submaps*, int> trajectory_ids_

49
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
View File

@ -16,6 +16,7 @@
#include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h" #include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h"
#include <algorithm>
#include <array> #include <array>
#include <cmath> #include <cmath>
#include <map> #include <map>
@ -77,7 +78,10 @@ void OptimizationProblem::AddTrajectoryNode(
void OptimizationProblem::AddSubmap(const int trajectory_id, void OptimizationProblem::AddSubmap(const int trajectory_id,
const transform::Rigid2d& submap_pose) { const transform::Rigid2d& submap_pose) {
submap_data_.push_back(SubmapData{trajectory_id, submap_pose}); CHECK_GE(trajectory_id, 0);
submap_data_.resize(
std::max(submap_data_.size(), static_cast<size_t>(trajectory_id) + 1));
submap_data_[trajectory_id].push_back(SubmapData{submap_pose});
} }
void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
@ -96,27 +100,31 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
// Set the starting point. // Set the starting point.
// TODO(hrapp): Move ceres data into SubmapData. // TODO(hrapp): Move ceres data into SubmapData.
std::deque<std::deque<std::array<double, 3>>> C_submaps; std::vector<std::deque<std::array<double, 3>>> C_submaps(submap_data_.size());
std::vector<std::array<double, 3>> C_point_clouds(node_data_.size()); std::vector<std::array<double, 3>> C_point_clouds(node_data_.size());
for (size_t i = 0; i != submap_data_.size(); ++i) { for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
while (static_cast<int>(C_submaps.size()) <= ++trajectory_id) {
submap_data_[i].trajectory_id) { for (size_t submap_index = 0;
C_submaps.emplace_back(); submap_index != submap_data_[trajectory_id].size(); ++submap_index) {
if (trajectory_id == 0 && submap_index == 0) {
// Fix the pose of the first submap of the first trajectory.
C_submaps[trajectory_id].push_back(
FromPose(transform::Rigid2d::Identity()));
problem.AddParameterBlock(C_submaps[trajectory_id].back().data(), 3);
problem.SetParameterBlockConstant(
C_submaps[trajectory_id].back().data());
} else {
C_submaps[trajectory_id].push_back(
FromPose(submap_data_[trajectory_id][submap_index].pose));
problem.AddParameterBlock(C_submaps[trajectory_id].back().data(), 3);
}
} }
C_submaps[submap_data_[i].trajectory_id].push_back(
FromPose(submap_data_[i].pose));
problem.AddParameterBlock(
C_submaps[submap_data_[i].trajectory_id].back().data(), 3);
} }
for (size_t j = 0; j != node_data_.size(); ++j) { for (size_t j = 0; j != node_data_.size(); ++j) {
C_point_clouds[j] = FromPose(node_data_[j].point_cloud_pose); C_point_clouds[j] = FromPose(node_data_[j].point_cloud_pose);
problem.AddParameterBlock(C_point_clouds[j].data(), 3); problem.AddParameterBlock(C_point_clouds[j].data(), 3);
} }
// Fix the pose of the first submap.
problem.SetParameterBlockConstant(
C_submaps[submap_data_[0].trajectory_id].front().data());
// Add cost functions for intra- and inter-submap constraints. // Add cost functions for intra- and inter-submap constraints.
for (const Constraint& constraint : constraints) { for (const Constraint& constraint : constraints) {
CHECK_GE(constraint.j, 0); CHECK_GE(constraint.j, 0);
@ -179,9 +187,13 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
} }
// Store the result. // Store the result.
for (auto& submap_data : submap_data_) { for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
submap_data.pose = ToPose(C_submaps[submap_data.trajectory_id].front()); ++trajectory_id) {
C_submaps[submap_data.trajectory_id].pop_front(); for (size_t submap_index = 0;
submap_index != submap_data_[trajectory_id].size(); ++submap_index) {
submap_data_[trajectory_id][submap_index].pose =
ToPose(C_submaps[trajectory_id][submap_index]);
}
} }
for (size_t j = 0; j != node_data_.size(); ++j) { for (size_t j = 0; j != node_data_.size(); ++j) {
@ -193,7 +205,8 @@ const std::vector<NodeData>& OptimizationProblem::node_data() const {
return node_data_; return node_data_;
} }
const std::vector<SubmapData>& OptimizationProblem::submap_data() const { const std::vector<std::vector<SubmapData>>& OptimizationProblem::submap_data()
const {
return submap_data_; return submap_data_;
} }

6
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h
View File

@ -43,8 +43,6 @@ struct NodeData {
}; };
struct SubmapData { struct SubmapData {
// TODO(whess): Keep nodes per trajectory instead.
const int trajectory_id;
transform::Rigid2d pose; transform::Rigid2d pose;
}; };
@ -75,13 +73,13 @@ class OptimizationProblem {
void Solve(const std::vector<Constraint>& constraints); void Solve(const std::vector<Constraint>& constraints);
const std::vector<NodeData>& node_data() const; const std::vector<NodeData>& node_data() const;
const std::vector<SubmapData>& submap_data() const; const std::vector<std::vector<SubmapData>>& submap_data() const;
private: private:
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
std::map<const mapping::Submaps*, std::deque<mapping_3d::ImuData>> imu_data_; std::map<const mapping::Submaps*, std::deque<mapping_3d::ImuData>> imu_data_;
std::vector<NodeData> node_data_; std::vector<NodeData> node_data_;
std::vector<SubmapData> submap_data_; std::vector<std::vector<SubmapData>> submap_data_;
}; };
} // namespace sparse_pose_graph } // namespace sparse_pose_graph

72
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -55,33 +55,34 @@ SparsePoseGraph::~SparsePoseGraph() {
void SparsePoseGraph::GrowSubmapTransformsAsNeeded( void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
const std::vector<const Submap*>& insertion_submaps) { const std::vector<const Submap*>& insertion_submaps) {
CHECK(!insertion_submaps.empty()); CHECK(!insertion_submaps.empty());
CHECK_LT(optimization_problem_.submap_data().size(), const mapping::SubmapId first_submap_id = GetSubmapId(insertion_submaps[0]);
std::numeric_limits<int>::max()); const int trajectory_id = first_submap_id.trajectory_id;
const int next_submap_index = optimization_problem_.submap_data().size(); CHECK_GE(trajectory_id, 0);
// Verify that we have an index for the first submap. const auto& submap_data = optimization_problem_.submap_data();
const int first_submap_index = GetSubmapIndex(insertion_submaps[0]);
CHECK_LE(first_submap_index, next_submap_index);
if (insertion_submaps.size() == 1) { if (insertion_submaps.size() == 1) {
// If we don't already have an entry for this submap, add one. // If we don't already have an entry for the first submap, add one.
if (first_submap_index == next_submap_index) { CHECK_EQ(first_submap_id.submap_index, 0);
optimization_problem_.AddSubmap( if (static_cast<size_t>(trajectory_id) >= submap_data.size() ||
submap_states_[first_submap_index].id.trajectory_id, submap_data[trajectory_id].empty()) {
transform::Rigid3d::Identity()); optimization_problem_.AddSubmap(trajectory_id,
transform::Rigid3d::Identity());
} }
return; return;
} }
CHECK_EQ(2, insertion_submaps.size()); CHECK_EQ(2, insertion_submaps.size());
const int next_submap_index = submap_data.at(trajectory_id).size();
// CHECK that we have a index for the second submap. // CHECK that we have a index for the second submap.
const int second_submap_index = GetSubmapIndex(insertion_submaps[1]); const mapping::SubmapId second_submap_id = GetSubmapId(insertion_submaps[1]);
CHECK_LE(second_submap_index, next_submap_index); CHECK_EQ(second_submap_id.trajectory_id, trajectory_id);
CHECK_LE(second_submap_id.submap_index, next_submap_index);
// Extrapolate if necessary. // Extrapolate if necessary.
if (second_submap_index == next_submap_index) { if (second_submap_id.submap_index == next_submap_index) {
const auto& first_submap_pose = const auto& first_submap_pose =
optimization_problem_.submap_data().at(first_submap_index).pose; submap_data.at(trajectory_id).at(first_submap_id.submap_index).pose;
optimization_problem_.AddSubmap( optimization_problem_.AddSubmap(
submap_states_[second_submap_index].id.trajectory_id, trajectory_id, first_submap_pose *
first_submap_pose * insertion_submaps[0]->local_pose().inverse() * insertion_submaps[0]->local_pose().inverse() *
insertion_submaps[1]->local_pose()); insertion_submaps[1]->local_pose());
} }
} }
@ -160,8 +161,12 @@ void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
void SparsePoseGraph::ComputeConstraint(const int scan_index, void SparsePoseGraph::ComputeConstraint(const int scan_index,
const int submap_index) { const int submap_index) {
const mapping::SubmapId submap_id = submap_states_[submap_index].id;
const transform::Rigid3d relative_pose = const transform::Rigid3d relative_pose =
optimization_problem_.submap_data().at(submap_index).pose.inverse() * optimization_problem_.submap_data()
.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.pose.inverse() *
optimization_problem_.node_data().at(scan_index).point_cloud_pose; optimization_problem_.node_data().at(scan_index).point_cloud_pose;
const mapping::Submaps* const scan_trajectory = const mapping::Submaps* const scan_trajectory =
@ -169,8 +174,6 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
const mapping::Submaps* const submap_trajectory = const mapping::Submaps* const submap_trajectory =
submap_states_[submap_index].trajectory; submap_states_[submap_index].trajectory;
const mapping::SubmapId submap_id = submap_states_[submap_index].id;
// Only globally match against submaps not in this trajectory. // Only globally match against submaps not in this trajectory.
if (scan_trajectory != submap_trajectory && if (scan_trajectory != submap_trajectory &&
global_localization_samplers_[scan_trajectory]->Pulse()) { global_localization_samplers_[scan_trajectory]->Pulse()) {
@ -212,9 +215,12 @@ void SparsePoseGraph::ComputeConstraintsForScan(
const transform::Rigid3d& pose, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) { const kalman_filter::PoseCovariance& covariance) {
GrowSubmapTransformsAsNeeded(insertion_submaps); GrowSubmapTransformsAsNeeded(insertion_submaps);
const int matching_index = GetSubmapIndex(matching_submap); const mapping::SubmapId matching_id = GetSubmapId(matching_submap);
const transform::Rigid3d optimized_pose = const transform::Rigid3d optimized_pose =
optimization_problem_.submap_data().at(matching_index).pose * optimization_problem_.submap_data()
.at(matching_id.trajectory_id)
.at(matching_id.submap_index)
.pose *
matching_submap->local_pose().inverse() * pose; matching_submap->local_pose().inverse() * pose;
CHECK_EQ(scan_index, optimization_problem_.node_data().size()); CHECK_EQ(scan_index, optimization_problem_.node_data().size());
const mapping::TrajectoryNode::ConstantData* const scan_data = const mapping::TrajectoryNode::ConstantData* const scan_data =
@ -351,16 +357,12 @@ void SparsePoseGraph::RunOptimization() {
// Extrapolate all point cloud poses that were added later. // Extrapolate all point cloud poses that were added later.
std::unordered_map<const mapping::Submaps*, transform::Rigid3d> std::unordered_map<const mapping::Submaps*, transform::Rigid3d>
extrapolation_transforms; extrapolation_transforms;
std::vector<transform::Rigid3d> submap_transforms;
for (const auto& submap_data : optimization_problem_.submap_data()) {
submap_transforms.push_back(submap_data.pose);
}
for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) { for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) {
const mapping::Submaps* trajectory = const mapping::Submaps* trajectory =
trajectory_nodes_[i].constant_data->trajectory; trajectory_nodes_[i].constant_data->trajectory;
if (extrapolation_transforms.count(trajectory) == 0) { if (extrapolation_transforms.count(trajectory) == 0) {
const auto new_submap_transforms = const auto new_submap_transforms = ExtrapolateSubmapTransforms(
ExtrapolateSubmapTransforms(submap_transforms, trajectory); optimization_problem_.submap_data(), trajectory);
const auto old_submap_transforms = const auto old_submap_transforms =
ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory); ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory);
CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size()); CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size());
@ -371,7 +373,7 @@ void SparsePoseGraph::RunOptimization() {
trajectory_nodes_[i].pose = trajectory_nodes_[i].pose =
extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose; extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose;
} }
optimized_submap_transforms_ = submap_transforms; optimized_submap_transforms_ = optimization_problem_.submap_data();
connected_components_ = trajectory_connectivity_.ConnectedComponents(); connected_components_ = trajectory_connectivity_.ConnectedComponents();
reverse_connected_components_.clear(); reverse_connected_components_.clear();
for (size_t i = 0; i != connected_components_.size(); ++i) { for (size_t i = 0; i != connected_components_.size(); ++i) {
@ -419,7 +421,8 @@ std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
} }
std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms( std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
const std::vector<transform::Rigid3d>& submap_transforms, const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
submap_transforms,
const mapping::Submaps* const trajectory) const { const mapping::Submaps* const trajectory) const {
std::vector<transform::Rigid3d> result; std::vector<transform::Rigid3d> result;
size_t flat_index = 0; size_t flat_index = 0;
@ -431,10 +434,13 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
if (state.trajectory != trajectory) { if (state.trajectory != trajectory) {
continue; continue;
} }
if (flat_index >= submap_transforms.size()) { const int trajectory_id = trajectory_ids_.at(trajectory);
if (static_cast<size_t>(trajectory_id) >= submap_transforms.size() ||
result.size() >= submap_transforms.at(trajectory_id).size()) {
break; break;
} }
result.push_back(submap_transforms[flat_index]); result.push_back(
submap_transforms.at(trajectory_id).at(result.size()).pose);
flat_index_of_result_back = flat_index; flat_index_of_result_back = flat_index;
} }

12
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -129,6 +129,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
return iterator->second; return iterator->second;
} }
mapping::SubmapId GetSubmapId(const mapping::Submap* submap) const
REQUIRES(mutex_) {
return submap_states_.at(GetSubmapIndex(submap)).id;
}
// Grows the optimization problem to have an entry for every element of // Grows the optimization problem to have an entry for every element of
// 'insertion_submaps'. // 'insertion_submaps'.
void GrowSubmapTransformsAsNeeded( void GrowSubmapTransformsAsNeeded(
@ -162,7 +167,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Adds extrapolated transforms, so that there are transforms for all submaps. // Adds extrapolated transforms, so that there are transforms for all submaps.
std::vector<transform::Rigid3d> ExtrapolateSubmapTransforms( std::vector<transform::Rigid3d> ExtrapolateSubmapTransforms(
const std::vector<transform::Rigid3d>& submap_transforms, const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
submap_transforms,
const mapping::Submaps* trajectory) const REQUIRES(mutex_); const mapping::Submaps* trajectory) const REQUIRES(mutex_);
const mapping::proto::SparsePoseGraphOptions options_; const mapping::proto::SparsePoseGraphOptions options_;
@ -212,8 +218,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
std::vector<mapping::TrajectoryNode> trajectory_nodes_ GUARDED_BY(mutex_); std::vector<mapping::TrajectoryNode> trajectory_nodes_ GUARDED_BY(mutex_);
// Current submap transforms used for displaying data. // Current submap transforms used for displaying data.
std::vector<transform::Rigid3d> optimized_submap_transforms_ std::vector<std::vector<sparse_pose_graph::SubmapData>>
GUARDED_BY(mutex_); optimized_submap_transforms_ GUARDED_BY(mutex_);
// Map from submap pointers to trajectory IDs. // Map from submap pointers to trajectory IDs.
std::unordered_map<const mapping::Submaps*, int> trajectory_ids_ std::unordered_map<const mapping::Submaps*, int> trajectory_ids_

61
cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
View File

@ -16,6 +16,7 @@
#include "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h" #include "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h"
#include <algorithm>
#include <array> #include <array>
#include <cmath> #include <cmath>
#include <map> #include <map>
@ -93,7 +94,10 @@ void OptimizationProblem::AddTrajectoryNode(
void OptimizationProblem::AddSubmap(const int trajectory_id, void OptimizationProblem::AddSubmap(const int trajectory_id,
const transform::Rigid3d& submap_pose) { const transform::Rigid3d& submap_pose) {
submap_data_.push_back(SubmapData{trajectory_id, submap_pose}); CHECK_GE(trajectory_id, 0);
submap_data_.resize(
std::max(submap_data_.size(), static_cast<size_t>(trajectory_id) + 1));
submap_data_[trajectory_id].push_back(SubmapData{submap_pose});
} }
void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
@ -125,25 +129,31 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
}; };
// Set the starting point. // Set the starting point.
// TODO(hrapp): Move ceres data into SubmapData.
std::vector<std::deque<CeresPose>> C_submaps(nodes_per_trajectory.size());
std::deque<CeresPose> C_point_clouds;
// Tie the first submap to the origin.
CHECK(!submap_data_.empty()); CHECK(!submap_data_.empty());
C_submaps[submap_data_[0].trajectory_id].emplace_back( CHECK(!submap_data_[0].empty());
transform::Rigid3d::Identity(), translation_parameterization(), // TODO(hrapp): Move ceres data into SubmapData.
common::make_unique<ceres::AutoDiffLocalParameterization< std::vector<std::deque<CeresPose>> C_submaps(submap_data_.size());
ConstantYawQuaternionPlus, 4, 2>>(), std::deque<CeresPose> C_point_clouds;
&problem); for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
problem.SetParameterBlockConstant( ++trajectory_id) {
C_submaps[submap_data_[0].trajectory_id].back().translation()); for (size_t submap_index = 0;
submap_index != submap_data_[trajectory_id].size(); ++submap_index) {
for (size_t i = 1; i != submap_data_.size(); ++i) { if (trajectory_id == 0 && submap_index == 0) {
C_submaps[submap_data_[i].trajectory_id].emplace_back( // Tie the first submap of the first trajectory to the origin.
submap_data_[i].pose, translation_parameterization(), C_submaps[trajectory_id].emplace_back(
common::make_unique<ceres::QuaternionParameterization>(), &problem); transform::Rigid3d::Identity(), translation_parameterization(),
common::make_unique<ceres::AutoDiffLocalParameterization<
ConstantYawQuaternionPlus, 4, 2>>(),
&problem);
problem.SetParameterBlockConstant(
C_submaps[trajectory_id].back().translation());
} else {
C_submaps[trajectory_id].emplace_back(
submap_data_[trajectory_id][submap_index].pose,
translation_parameterization(),
common::make_unique<ceres::QuaternionParameterization>(), &problem);
}
}
} }
for (size_t j = 0; j != node_data_.size(); ++j) { for (size_t j = 0; j != node_data_.size(); ++j) {
C_point_clouds.emplace_back( C_point_clouds.emplace_back(
@ -248,9 +258,13 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
} }
// Store the result. // Store the result.
for (auto& submap_data : submap_data_) { for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
submap_data.pose = C_submaps[submap_data.trajectory_id].front().ToRigid(); ++trajectory_id) {
C_submaps[submap_data.trajectory_id].pop_front(); for (size_t submap_index = 0;
submap_index != submap_data_[trajectory_id].size(); ++submap_index) {
submap_data_[trajectory_id][submap_index].pose =
C_submaps[trajectory_id][submap_index].ToRigid();
}
} }
for (size_t j = 0; j != node_data_.size(); ++j) { for (size_t j = 0; j != node_data_.size(); ++j) {
@ -262,7 +276,8 @@ const std::vector<NodeData>& OptimizationProblem::node_data() const {
return node_data_; return node_data_;
} }
const std::vector<SubmapData>& OptimizationProblem::submap_data() const { const std::vector<std::vector<SubmapData>>& OptimizationProblem::submap_data()
const {
return submap_data_; return submap_data_;
} }

6
cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h
View File

@ -43,8 +43,6 @@ struct NodeData {
}; };
struct SubmapData { struct SubmapData {
// TODO(whess): Keep nodes per trajectory instead.
const int trajectory_id;
transform::Rigid3d pose; transform::Rigid3d pose;
}; };
@ -77,14 +75,14 @@ class OptimizationProblem {
void Solve(const std::vector<Constraint>& constraints); void Solve(const std::vector<Constraint>& constraints);
const std::vector<NodeData>& node_data() const; const std::vector<NodeData>& node_data() const;
const std::vector<SubmapData>& submap_data() const; const std::vector<std::vector<SubmapData>>& submap_data() const;
private: private:
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
FixZ fix_z_; FixZ fix_z_;
std::map<const mapping::Submaps*, std::deque<ImuData>> imu_data_; std::map<const mapping::Submaps*, std::deque<ImuData>> imu_data_;
std::vector<NodeData> node_data_; std::vector<NodeData> node_data_;
std::vector<SubmapData> submap_data_; std::vector<std::vector<SubmapData>> submap_data_;
double gravity_constant_ = 9.8; double gravity_constant_ = 9.8;
}; };