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

`submap_pose`s are now owned by the OptimizationProblem. (#269)

master
Holger Rapp 2017-05-09 17:24:48 +02:00 committed by GitHub
parent 20e9cde53d
commit 524b613f2c
9 changed files with 193 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -52,31 +52,37 @@ SparsePoseGraph::~SparsePoseGraph() {
} }
void SparsePoseGraph::GrowSubmapTransformsAsNeeded( void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
const std::vector<const mapping::Submap*>& submaps) { const std::vector<const mapping::Submap*>& insertion_submaps) {
CHECK(!submaps.empty()); CHECK(!insertion_submaps.empty());
CHECK_LT(submap_transforms_.size(), std::numeric_limits<int>::max()); CHECK_LT(optimization_problem_.submap_data().size(),
const int next_transform_index = submap_transforms_.size(); std::numeric_limits<int>::max());
const int next_submap_index = optimization_problem_.submap_data().size();
// Verify that we have an index for the first submap. // Verify that we have an index for the first submap.
const int first_transform_index = GetSubmapIndex(submaps[0]); const int first_submap_index = GetSubmapIndex(insertion_submaps[0]);
if (submaps.size() == 1) { CHECK_LE(first_submap_index, next_submap_index);
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 this submap, add one.
if (first_transform_index == next_transform_index) { if (first_submap_index == next_submap_index) {
submap_transforms_.push_back(transform::Rigid2d::Identity()); optimization_problem_.AddSubmap(
submap_states_[first_submap_index].trajectory,
transform::Rigid2d::Identity());
} }
return; return;
} }
CHECK_EQ(2, submaps.size()); CHECK_EQ(2, insertion_submaps.size());
// CHECK that we have a index for the second submap. // CHECK that we have a index for the second submap.
const int second_transform_index = GetSubmapIndex(submaps[1]); const int second_submap_index = GetSubmapIndex(insertion_submaps[1]);
CHECK_LE(second_transform_index, next_transform_index); CHECK_LE(second_submap_index, next_submap_index);
// Extrapolate if necessary. // Extrapolate if necessary.
if (second_transform_index == next_transform_index) { if (second_submap_index == next_submap_index) {
const auto& first_submap_transform = const auto& first_submap_pose =
submap_transforms_[first_transform_index]; optimization_problem_.submap_data().at(first_submap_index).pose;
submap_transforms_.push_back( optimization_problem_.AddSubmap(
first_submap_transform * submap_states_[second_submap_index].trajectory,
sparse_pose_graph::ComputeSubmapPose(*submaps[0]).inverse() * first_submap_pose *
sparse_pose_graph::ComputeSubmapPose(*submaps[1])); sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0])
.inverse() *
sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[1]));
} }
} }
@ -151,7 +157,7 @@ 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 transform::Rigid2d relative_pose = const transform::Rigid2d relative_pose =
submap_transforms_[submap_index].inverse() * optimization_problem_.submap_data().at(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 =
@ -203,7 +209,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
GrowSubmapTransformsAsNeeded(insertion_submaps); GrowSubmapTransformsAsNeeded(insertion_submaps);
const int matching_index = GetSubmapIndex(matching_submap); const int matching_index = GetSubmapIndex(matching_submap);
const transform::Rigid2d optimized_pose = const transform::Rigid2d optimized_pose =
submap_transforms_[matching_index] * optimization_problem_.submap_data().at(matching_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 =
@ -330,8 +336,10 @@ void SparsePoseGraph::RunFinalOptimization() {
} }
void SparsePoseGraph::RunOptimization() { void SparsePoseGraph::RunOptimization() {
if (!submap_transforms_.empty()) { if (optimization_problem_.submap_data().empty()) {
optimization_problem_.Solve(constraints_, &submap_transforms_); return;
}
optimization_problem_.Solve(constraints_);
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
const auto& node_data = optimization_problem_.node_data(); const auto& node_data = optimization_problem_.node_data();
@ -343,14 +351,18 @@ 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(submap_transforms_, trajectory); ExtrapolateSubmapTransforms(submap_transforms, trajectory);
const auto old_submap_transforms = ExtrapolateSubmapTransforms( const auto old_submap_transforms =
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());
extrapolation_transforms[trajectory] = extrapolation_transforms[trajectory] =
transform::Rigid3d(new_submap_transforms.back() * transform::Rigid3d(new_submap_transforms.back() *
@ -359,7 +371,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_ = submap_transforms;
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) {
@ -368,7 +380,6 @@ void SparsePoseGraph::RunOptimization() {
} }
} }
} }
}
std::vector<mapping::TrajectoryNode> SparsePoseGraph::GetTrajectoryNodes() { std::vector<mapping::TrajectoryNode> SparsePoseGraph::GetTrajectoryNodes() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);

7
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -105,9 +105,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
return iterator->second; return iterator->second;
} }
// Grows 'submap_transforms_' to have an entry for every element of 'submaps'. // Grows the optimization problem to have an entry for every element of
// 'insertion_submaps'.
void GrowSubmapTransformsAsNeeded( void GrowSubmapTransformsAsNeeded(
const std::vector<const mapping::Submap*>& submaps) REQUIRES(mutex_); const std::vector<const mapping::Submap*>& insertion_submaps)
REQUIRES(mutex_);
// Adds constraints for a scan, and starts scan matching in the background. // Adds constraints for a scan, and starts scan matching in the background.
void ComputeConstraintsForScan( void ComputeConstraintsForScan(
@ -167,7 +169,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
sparse_pose_graph::OptimizationProblem optimization_problem_; sparse_pose_graph::OptimizationProblem optimization_problem_;
sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_); sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
std::vector<Constraint> constraints_; std::vector<Constraint> constraints_;
std::vector<transform::Rigid2d> submap_transforms_; // (map <- submap)
// Submaps get assigned an index and state as soon as they are seen, even // Submaps get assigned an index and state as soon as they are seen, even
// before they take part in the background computations. // before they take part in the background computations.

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

@ -75,14 +75,17 @@ void OptimizationProblem::AddTrajectoryNode(
NodeData{trajectory, time, initial_point_cloud_pose, point_cloud_pose}); NodeData{trajectory, time, initial_point_cloud_pose, point_cloud_pose});
} }
void OptimizationProblem::AddSubmap(const mapping::Submaps* const trajectory,
const transform::Rigid2d& submap_pose) {
submap_data_.push_back(SubmapData{trajectory, submap_pose});
}
void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
options_.mutable_ceres_solver_options()->set_max_num_iterations( options_.mutable_ceres_solver_options()->set_max_num_iterations(
max_num_iterations); max_num_iterations);
} }
void OptimizationProblem::Solve( void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
const std::vector<Constraint>& constraints,
std::vector<transform::Rigid2d>* const submap_transforms) {
if (node_data_.empty()) { if (node_data_.empty()) {
// Nothing to optimize. // Nothing to optimize.
return; return;
@ -92,10 +95,10 @@ void OptimizationProblem::Solve(
ceres::Problem problem(problem_options); ceres::Problem problem(problem_options);
// Set the starting point. // Set the starting point.
std::vector<std::array<double, 3>> C_submaps(submap_transforms->size()); std::vector<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_transforms->size(); ++i) { for (size_t i = 0; i != submap_data_.size(); ++i) {
C_submaps[i] = FromPose((*submap_transforms)[i]); C_submaps[i] = FromPose(submap_data_[i].pose);
problem.AddParameterBlock(C_submaps[i].data(), 3); problem.AddParameterBlock(C_submaps[i].data(), 3);
} }
for (size_t j = 0; j != node_data_.size(); ++j) { for (size_t j = 0; j != node_data_.size(); ++j) {
@ -109,7 +112,7 @@ void OptimizationProblem::Solve(
// 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.i, 0); CHECK_GE(constraint.i, 0);
CHECK_LT(constraint.i, submap_transforms->size()); CHECK_LT(constraint.i, submap_data_.size());
CHECK_GE(constraint.j, 0); CHECK_GE(constraint.j, 0);
CHECK_LT(constraint.j, node_data_.size()); CHECK_LT(constraint.j, node_data_.size());
problem.AddResidualBlock( problem.AddResidualBlock(
@ -167,8 +170,8 @@ void OptimizationProblem::Solve(
} }
// Store the result. // Store the result.
for (size_t i = 0; i != submap_transforms->size(); ++i) { for (size_t i = 0; i != submap_data_.size(); ++i) {
(*submap_transforms)[i] = ToPose(C_submaps[i]); submap_data_[i].pose = ToPose(C_submaps[i]);
} }
for (size_t j = 0; j != node_data_.size(); ++j) { for (size_t j = 0; j != node_data_.size(); ++j) {
node_data_[j].point_cloud_pose = ToPose(C_point_clouds[j]); node_data_[j].point_cloud_pose = ToPose(C_point_clouds[j]);
@ -179,6 +182,10 @@ const std::vector<NodeData>& OptimizationProblem::node_data() const {
return node_data_; return node_data_;
} }
const std::vector<SubmapData>& OptimizationProblem::submap_data() const {
return submap_data_;
}
} // namespace sparse_pose_graph } // namespace sparse_pose_graph
} // namespace mapping_2d } // namespace mapping_2d
} // namespace cartographer } // namespace cartographer

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

@ -42,6 +42,12 @@ struct NodeData {
transform::Rigid2d point_cloud_pose; transform::Rigid2d point_cloud_pose;
}; };
struct SubmapData {
// TODO(whess): Keep nodes per trajectory instead.
const mapping::Submaps* trajectory;
transform::Rigid2d pose;
};
// Implements the SPA loop closure method. // Implements the SPA loop closure method.
class OptimizationProblem { class OptimizationProblem {
public: public:
@ -61,19 +67,22 @@ class OptimizationProblem {
void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time, void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time,
const transform::Rigid2d& initial_point_cloud_pose, const transform::Rigid2d& initial_point_cloud_pose,
const transform::Rigid2d& point_cloud_pose); const transform::Rigid2d& point_cloud_pose);
void AddSubmap(const mapping::Submaps* trajectory,
const transform::Rigid2d& submap_pose);
void SetMaxNumIterations(int32 max_num_iterations); void SetMaxNumIterations(int32 max_num_iterations);
// Computes the optimized poses. // Computes the optimized poses.
void Solve(const std::vector<Constraint>& constraints, void Solve(const std::vector<Constraint>& constraints);
std::vector<transform::Rigid2d>* submap_transforms);
const std::vector<NodeData>& node_data() const; const std::vector<NodeData>& node_data() const;
const 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_;
}; };
} // namespace sparse_pose_graph } // namespace sparse_pose_graph

62
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -53,30 +53,35 @@ SparsePoseGraph::~SparsePoseGraph() {
} }
void SparsePoseGraph::GrowSubmapTransformsAsNeeded( void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
const std::vector<const Submap*>& submaps) { const std::vector<const Submap*>& insertion_submaps) {
CHECK(!submaps.empty()); CHECK(!insertion_submaps.empty());
CHECK_LT(submap_transforms_.size(), std::numeric_limits<int>::max()); CHECK_LT(optimization_problem_.submap_data().size(),
const int next_transform_index = submap_transforms_.size(); std::numeric_limits<int>::max());
const int next_submap_index = optimization_problem_.submap_data().size();
// Verify that we have an index for the first submap. // Verify that we have an index for the first submap.
const int first_transform_index = GetSubmapIndex(submaps[0]); const int first_submap_index = GetSubmapIndex(insertion_submaps[0]);
if (submaps.size() == 1) { CHECK_LE(first_submap_index, next_submap_index);
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 this submap, add one.
if (first_transform_index == next_transform_index) { if (first_submap_index == next_submap_index) {
submap_transforms_.push_back(transform::Rigid3d::Identity()); optimization_problem_.AddSubmap(
submap_states_[first_submap_index].trajectory,
transform::Rigid3d::Identity());
} }
return; return;
} }
CHECK_EQ(2, submaps.size()); CHECK_EQ(2, insertion_submaps.size());
// CHECK that we have a index for the second submap. // CHECK that we have a index for the second submap.
const int second_transform_index = GetSubmapIndex(submaps[1]); const int second_submap_index = GetSubmapIndex(insertion_submaps[1]);
CHECK_LE(second_transform_index, next_transform_index); CHECK_LE(second_submap_index, next_submap_index);
// Extrapolate if necessary. // Extrapolate if necessary.
if (second_transform_index == next_transform_index) { if (second_submap_index == next_submap_index) {
const auto& first_submap_transform = const auto& first_submap_pose =
submap_transforms_[first_transform_index]; optimization_problem_.submap_data().at(first_submap_index).pose;
submap_transforms_.push_back(first_submap_transform * optimization_problem_.AddSubmap(
submaps[0]->local_pose().inverse() * submap_states_[second_submap_index].trajectory,
submaps[1]->local_pose()); first_submap_pose * insertion_submaps[0]->local_pose().inverse() *
insertion_submaps[1]->local_pose());
} }
} }
@ -152,7 +157,7 @@ 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 transform::Rigid3d relative_pose = const transform::Rigid3d relative_pose =
submap_transforms_[submap_index].inverse() * optimization_problem_.submap_data().at(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 =
@ -202,7 +207,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
GrowSubmapTransformsAsNeeded(insertion_submaps); GrowSubmapTransformsAsNeeded(insertion_submaps);
const int matching_index = GetSubmapIndex(matching_submap); const int matching_index = GetSubmapIndex(matching_submap);
const transform::Rigid3d optimized_pose = const transform::Rigid3d optimized_pose =
submap_transforms_[matching_index] * optimization_problem_.submap_data().at(matching_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 =
@ -325,8 +330,10 @@ void SparsePoseGraph::RunFinalOptimization() {
} }
void SparsePoseGraph::RunOptimization() { void SparsePoseGraph::RunOptimization() {
if (!submap_transforms_.empty()) { if (optimization_problem_.submap_data().empty()) {
optimization_problem_.Solve(constraints_, &submap_transforms_); return;
}
optimization_problem_.Solve(constraints_);
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
const auto& node_data = optimization_problem_.node_data(); const auto& node_data = optimization_problem_.node_data();
@ -337,14 +344,18 @@ 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(submap_transforms_, trajectory); ExtrapolateSubmapTransforms(submap_transforms, trajectory);
const auto old_submap_transforms = ExtrapolateSubmapTransforms( const auto old_submap_transforms =
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());
extrapolation_transforms[trajectory] = extrapolation_transforms[trajectory] =
transform::Rigid3d(new_submap_transforms.back() * transform::Rigid3d(new_submap_transforms.back() *
@ -353,7 +364,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_ = submap_transforms;
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) {
@ -362,7 +373,6 @@ void SparsePoseGraph::RunOptimization() {
} }
} }
} }
}
std::vector<mapping::TrajectoryNode> SparsePoseGraph::GetTrajectoryNodes() { std::vector<mapping::TrajectoryNode> SparsePoseGraph::GetTrajectoryNodes() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);

8
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -126,9 +126,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
return iterator->second; return iterator->second;
} }
// Grows 'submap_transforms_' to have an entry for every element of 'submaps'. // Grows the optimization problem to have an entry for every element of
void GrowSubmapTransformsAsNeeded(const std::vector<const Submap*>& submaps) // 'insertion_submaps'.
REQUIRES(mutex_); void GrowSubmapTransformsAsNeeded(
const std::vector<const Submap*>& insertion_submaps) REQUIRES(mutex_);
// Adds constraints for a scan, and starts scan matching in the background. // Adds constraints for a scan, and starts scan matching in the background.
void ComputeConstraintsForScan( void ComputeConstraintsForScan(
@ -187,7 +188,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
sparse_pose_graph::OptimizationProblem optimization_problem_; sparse_pose_graph::OptimizationProblem optimization_problem_;
sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_); sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
std::vector<Constraint> constraints_; std::vector<Constraint> constraints_;
std::vector<transform::Rigid3d> submap_transforms_; // (map <- submap)
// Submaps get assigned an index and state as soon as they are seen, even // Submaps get assigned an index and state as soon as they are seen, even
// before they take part in the background computations. // before they take part in the background computations.

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

@ -91,14 +91,17 @@ void OptimizationProblem::AddTrajectoryNode(
node_data_.push_back(NodeData{trajectory, time, point_cloud_pose}); node_data_.push_back(NodeData{trajectory, time, point_cloud_pose});
} }
void OptimizationProblem::AddSubmap(const mapping::Submaps* const trajectory,
const transform::Rigid3d& submap_pose) {
submap_data_.push_back(SubmapData{trajectory, submap_pose});
}
void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
options_.mutable_ceres_solver_options()->set_max_num_iterations( options_.mutable_ceres_solver_options()->set_max_num_iterations(
max_num_iterations); max_num_iterations);
} }
void OptimizationProblem::Solve( void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
const std::vector<Constraint>& constraints,
std::vector<transform::Rigid3d>* const submap_transforms) {
if (node_data_.empty()) { if (node_data_.empty()) {
// Nothing to optimize. // Nothing to optimize.
return; return;
@ -125,7 +128,7 @@ void OptimizationProblem::Solve(
std::deque<CeresPose> C_submaps; std::deque<CeresPose> C_submaps;
std::deque<CeresPose> C_point_clouds; std::deque<CeresPose> C_point_clouds;
// Tie the first submap to the origin. // Tie the first submap to the origin.
CHECK(!submap_transforms->empty()); CHECK(!submap_data_.empty());
C_submaps.emplace_back( C_submaps.emplace_back(
transform::Rigid3d::Identity(), translation_parameterization(), transform::Rigid3d::Identity(), translation_parameterization(),
common::make_unique<ceres::AutoDiffLocalParameterization< common::make_unique<ceres::AutoDiffLocalParameterization<
@ -133,9 +136,9 @@ void OptimizationProblem::Solve(
&problem); &problem);
problem.SetParameterBlockConstant(C_submaps.back().translation()); problem.SetParameterBlockConstant(C_submaps.back().translation());
for (size_t i = 1; i != submap_transforms->size(); ++i) { for (size_t i = 1; i != submap_data_.size(); ++i) {
C_submaps.emplace_back( C_submaps.emplace_back(
(*submap_transforms)[i], translation_parameterization(), submap_data_[i].pose, translation_parameterization(),
common::make_unique<ceres::QuaternionParameterization>(), &problem); 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) {
@ -147,7 +150,7 @@ void OptimizationProblem::Solve(
// 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.i, 0); CHECK_GE(constraint.i, 0);
CHECK_LT(constraint.i, submap_transforms->size()); CHECK_LT(constraint.i, submap_data_.size());
CHECK_GE(constraint.j, 0); CHECK_GE(constraint.j, 0);
CHECK_LT(constraint.j, node_data_.size()); CHECK_LT(constraint.j, node_data_.size());
problem.AddResidualBlock( problem.AddResidualBlock(
@ -239,8 +242,8 @@ void OptimizationProblem::Solve(
} }
// Store the result. // Store the result.
for (size_t i = 0; i != submap_transforms->size(); ++i) { for (size_t i = 0; i != submap_data_.size(); ++i) {
(*submap_transforms)[i] = C_submaps[i].ToRigid(); submap_data_[i].pose = C_submaps[i].ToRigid();
} }
for (size_t j = 0; j != node_data_.size(); ++j) { for (size_t j = 0; j != node_data_.size(); ++j) {
node_data_[j].point_cloud_pose = C_point_clouds[j].ToRigid(); node_data_[j].point_cloud_pose = C_point_clouds[j].ToRigid();
@ -251,6 +254,10 @@ const std::vector<NodeData>& OptimizationProblem::node_data() const {
return node_data_; return node_data_;
} }
const std::vector<SubmapData>& OptimizationProblem::submap_data() const {
return submap_data_;
}
} // namespace sparse_pose_graph } // namespace sparse_pose_graph
} // namespace mapping_3d } // namespace mapping_3d
} // namespace cartographer } // namespace cartographer

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

@ -42,6 +42,12 @@ struct NodeData {
transform::Rigid3d point_cloud_pose; transform::Rigid3d point_cloud_pose;
}; };
struct SubmapData {
// TODO(whess): Keep nodes per trajectory instead.
const mapping::Submaps* trajectory;
transform::Rigid3d pose;
};
// Implements the SPA loop closure method. // Implements the SPA loop closure method.
class OptimizationProblem { class OptimizationProblem {
public: public:
@ -63,20 +69,23 @@ class OptimizationProblem {
const Eigen::Vector3d& angular_velocity); const Eigen::Vector3d& angular_velocity);
void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time, void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time,
const transform::Rigid3d& point_cloud_pose); const transform::Rigid3d& point_cloud_pose);
void AddSubmap(const mapping::Submaps* trajectory,
const transform::Rigid3d& submap_pose);
void SetMaxNumIterations(int32 max_num_iterations); void SetMaxNumIterations(int32 max_num_iterations);
// Computes the optimized poses. // Computes the optimized poses.
void Solve(const std::vector<Constraint>& constraints, void Solve(const std::vector<Constraint>& constraints);
std::vector<transform::Rigid3d>* submap_transforms);
const std::vector<NodeData>& node_data() const; const std::vector<NodeData>& node_data() const;
const 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_;
double gravity_constant_ = 9.8; double gravity_constant_ = 9.8;
}; };

8
cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc
View File

@ -153,9 +153,6 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
1e-9 * Eigen::Matrix<double, 6, 6>::Identity()}}); 1e-9 * Eigen::Matrix<double, 6, 6>::Identity()}});
} }
std::vector<transform::Rigid3d> submap_transforms = {
kSubmap0Transform, kSubmap0Transform, kSubmap2Transform};
double translation_error_before = 0.; double translation_error_before = 0.;
double rotation_error_before = 0.; double rotation_error_before = 0.;
const auto& node_data = optimization_problem_.node_data(); const auto& node_data = optimization_problem_.node_data();
@ -168,7 +165,10 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
node_data[j].point_cloud_pose); node_data[j].point_cloud_pose);
} }
optimization_problem_.Solve(constraints, &submap_transforms); optimization_problem_.AddSubmap(kTrajectory, kSubmap0Transform);
optimization_problem_.AddSubmap(kTrajectory, kSubmap0Transform);
optimization_problem_.AddSubmap(kTrajectory, kSubmap2Transform);
optimization_problem_.Solve(constraints);
double translation_error_after = 0.; double translation_error_after = 0.;
double rotation_error_after = 0.; double rotation_error_after = 0.;