diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 755c27f..e1abf70 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -52,31 +52,37 @@ SparsePoseGraph::~SparsePoseGraph() { } void SparsePoseGraph::GrowSubmapTransformsAsNeeded( - const std::vector& submaps) { - CHECK(!submaps.empty()); - CHECK_LT(submap_transforms_.size(), std::numeric_limits::max()); - const int next_transform_index = submap_transforms_.size(); + const std::vector& insertion_submaps) { + CHECK(!insertion_submaps.empty()); + CHECK_LT(optimization_problem_.submap_data().size(), + std::numeric_limits::max()); + const int next_submap_index = optimization_problem_.submap_data().size(); // Verify that we have an index for the first submap. - const int first_transform_index = GetSubmapIndex(submaps[0]); - if (submaps.size() == 1) { + const int first_submap_index = GetSubmapIndex(insertion_submaps[0]); + 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 (first_transform_index == next_transform_index) { - submap_transforms_.push_back(transform::Rigid2d::Identity()); + if (first_submap_index == next_submap_index) { + optimization_problem_.AddSubmap( + submap_states_[first_submap_index].trajectory, + transform::Rigid2d::Identity()); } return; } - CHECK_EQ(2, submaps.size()); + CHECK_EQ(2, insertion_submaps.size()); // CHECK that we have a index for the second submap. - const int second_transform_index = GetSubmapIndex(submaps[1]); - CHECK_LE(second_transform_index, next_transform_index); + const int second_submap_index = GetSubmapIndex(insertion_submaps[1]); + CHECK_LE(second_submap_index, next_submap_index); // Extrapolate if necessary. - if (second_transform_index == next_transform_index) { - const auto& first_submap_transform = - submap_transforms_[first_transform_index]; - submap_transforms_.push_back( - first_submap_transform * - sparse_pose_graph::ComputeSubmapPose(*submaps[0]).inverse() * - sparse_pose_graph::ComputeSubmapPose(*submaps[1])); + if (second_submap_index == next_submap_index) { + const auto& first_submap_pose = + optimization_problem_.submap_data().at(first_submap_index).pose; + optimization_problem_.AddSubmap( + submap_states_[second_submap_index].trajectory, + first_submap_pose * + 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, const int submap_index) { 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; const mapping::Submaps* const scan_trajectory = @@ -203,7 +209,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( GrowSubmapTransformsAsNeeded(insertion_submaps); const int matching_index = GetSubmapIndex(matching_submap); 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; CHECK_EQ(scan_index, optimization_problem_.node_data().size()); const mapping::TrajectoryNode::ConstantData* const scan_data = @@ -330,42 +336,47 @@ void SparsePoseGraph::RunFinalOptimization() { } void SparsePoseGraph::RunOptimization() { - if (!submap_transforms_.empty()) { - optimization_problem_.Solve(constraints_, &submap_transforms_); - common::MutexLocker locker(&mutex_); + if (optimization_problem_.submap_data().empty()) { + return; + } + optimization_problem_.Solve(constraints_); + common::MutexLocker locker(&mutex_); - const auto& node_data = optimization_problem_.node_data(); - const size_t num_optimized_poses = node_data.size(); - for (size_t i = 0; i != num_optimized_poses; ++i) { - trajectory_nodes_[i].pose = - transform::Rigid3d(transform::Embed3D(node_data[i].point_cloud_pose)); + const auto& node_data = optimization_problem_.node_data(); + const size_t num_optimized_poses = node_data.size(); + for (size_t i = 0; i != num_optimized_poses; ++i) { + trajectory_nodes_[i].pose = + transform::Rigid3d(transform::Embed3D(node_data[i].point_cloud_pose)); + } + // Extrapolate all point cloud poses that were added later. + std::unordered_map + extrapolation_transforms; + std::vector 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) { + const mapping::Submaps* trajectory = + trajectory_nodes_[i].constant_data->trajectory; + if (extrapolation_transforms.count(trajectory) == 0) { + const auto new_submap_transforms = + ExtrapolateSubmapTransforms(submap_transforms, trajectory); + const auto old_submap_transforms = + ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory); + CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size()); + extrapolation_transforms[trajectory] = + transform::Rigid3d(new_submap_transforms.back() * + old_submap_transforms.back().inverse()); } - // Extrapolate all point cloud poses that were added later. - std::unordered_map - extrapolation_transforms; - for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) { - const mapping::Submaps* trajectory = - trajectory_nodes_[i].constant_data->trajectory; - if (extrapolation_transforms.count(trajectory) == 0) { - const auto new_submap_transforms = - ExtrapolateSubmapTransforms(submap_transforms_, trajectory); - const auto old_submap_transforms = ExtrapolateSubmapTransforms( - optimized_submap_transforms_, trajectory); - CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size()); - extrapolation_transforms[trajectory] = - transform::Rigid3d(new_submap_transforms.back() * - old_submap_transforms.back().inverse()); - } - trajectory_nodes_[i].pose = - extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose; - } - optimized_submap_transforms_ = submap_transforms_; - connected_components_ = trajectory_connectivity_.ConnectedComponents(); - reverse_connected_components_.clear(); - for (size_t i = 0; i != connected_components_.size(); ++i) { - for (const auto* trajectory : connected_components_[i]) { - reverse_connected_components_.emplace(trajectory, i); - } + trajectory_nodes_[i].pose = + extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose; + } + optimized_submap_transforms_ = submap_transforms; + connected_components_ = trajectory_connectivity_.ConnectedComponents(); + reverse_connected_components_.clear(); + for (size_t i = 0; i != connected_components_.size(); ++i) { + for (const auto* trajectory : connected_components_[i]) { + reverse_connected_components_.emplace(trajectory, i); } } } diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 1d3eeb8..b852db7 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -105,9 +105,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { 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( - const std::vector& submaps) REQUIRES(mutex_); + const std::vector& insertion_submaps) + REQUIRES(mutex_); // Adds constraints for a scan, and starts scan matching in the background. void ComputeConstraintsForScan( @@ -167,7 +169,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { sparse_pose_graph::OptimizationProblem optimization_problem_; sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_); std::vector constraints_; - std::vector submap_transforms_; // (map <- submap) // Submaps get assigned an index and state as soon as they are seen, even // before they take part in the background computations. diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 3cfa996..f30eadf 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -75,14 +75,17 @@ void OptimizationProblem::AddTrajectoryNode( 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) { options_.mutable_ceres_solver_options()->set_max_num_iterations( max_num_iterations); } -void OptimizationProblem::Solve( - const std::vector& constraints, - std::vector* const submap_transforms) { +void OptimizationProblem::Solve(const std::vector& constraints) { if (node_data_.empty()) { // Nothing to optimize. return; @@ -92,10 +95,10 @@ void OptimizationProblem::Solve( ceres::Problem problem(problem_options); // Set the starting point. - std::vector> C_submaps(submap_transforms->size()); + std::vector> C_submaps(submap_data_.size()); std::vector> C_point_clouds(node_data_.size()); - for (size_t i = 0; i != submap_transforms->size(); ++i) { - C_submaps[i] = FromPose((*submap_transforms)[i]); + for (size_t i = 0; i != submap_data_.size(); ++i) { + C_submaps[i] = FromPose(submap_data_[i].pose); problem.AddParameterBlock(C_submaps[i].data(), 3); } 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. for (const Constraint& constraint : constraints) { 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_LT(constraint.j, node_data_.size()); problem.AddResidualBlock( @@ -167,8 +170,8 @@ void OptimizationProblem::Solve( } // Store the result. - for (size_t i = 0; i != submap_transforms->size(); ++i) { - (*submap_transforms)[i] = ToPose(C_submaps[i]); + for (size_t i = 0; i != submap_data_.size(); ++i) { + submap_data_[i].pose = ToPose(C_submaps[i]); } for (size_t j = 0; j != node_data_.size(); ++j) { node_data_[j].point_cloud_pose = ToPose(C_point_clouds[j]); @@ -179,6 +182,10 @@ const std::vector& OptimizationProblem::node_data() const { return node_data_; } +const std::vector& OptimizationProblem::submap_data() const { + return submap_data_; +} + } // namespace sparse_pose_graph } // namespace mapping_2d } // namespace cartographer diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h index 1599e33..451cf0f 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -42,6 +42,12 @@ struct NodeData { 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. class OptimizationProblem { public: @@ -61,19 +67,22 @@ class OptimizationProblem { void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time, const transform::Rigid2d& initial_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); // Computes the optimized poses. - void Solve(const std::vector& constraints, - std::vector* submap_transforms); + void Solve(const std::vector& constraints); const std::vector& node_data() const; + const std::vector& submap_data() const; private: mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; std::map> imu_data_; std::vector node_data_; + std::vector submap_data_; }; } // namespace sparse_pose_graph diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index f6e53b3..2e0917c 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -53,30 +53,35 @@ SparsePoseGraph::~SparsePoseGraph() { } void SparsePoseGraph::GrowSubmapTransformsAsNeeded( - const std::vector& submaps) { - CHECK(!submaps.empty()); - CHECK_LT(submap_transforms_.size(), std::numeric_limits::max()); - const int next_transform_index = submap_transforms_.size(); + const std::vector& insertion_submaps) { + CHECK(!insertion_submaps.empty()); + CHECK_LT(optimization_problem_.submap_data().size(), + std::numeric_limits::max()); + const int next_submap_index = optimization_problem_.submap_data().size(); // Verify that we have an index for the first submap. - const int first_transform_index = GetSubmapIndex(submaps[0]); - if (submaps.size() == 1) { + const int first_submap_index = GetSubmapIndex(insertion_submaps[0]); + 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 (first_transform_index == next_transform_index) { - submap_transforms_.push_back(transform::Rigid3d::Identity()); + if (first_submap_index == next_submap_index) { + optimization_problem_.AddSubmap( + submap_states_[first_submap_index].trajectory, + transform::Rigid3d::Identity()); } return; } - CHECK_EQ(2, submaps.size()); + CHECK_EQ(2, insertion_submaps.size()); // CHECK that we have a index for the second submap. - const int second_transform_index = GetSubmapIndex(submaps[1]); - CHECK_LE(second_transform_index, next_transform_index); + const int second_submap_index = GetSubmapIndex(insertion_submaps[1]); + CHECK_LE(second_submap_index, next_submap_index); // Extrapolate if necessary. - if (second_transform_index == next_transform_index) { - const auto& first_submap_transform = - submap_transforms_[first_transform_index]; - submap_transforms_.push_back(first_submap_transform * - submaps[0]->local_pose().inverse() * - submaps[1]->local_pose()); + if (second_submap_index == next_submap_index) { + const auto& first_submap_pose = + optimization_problem_.submap_data().at(first_submap_index).pose; + optimization_problem_.AddSubmap( + submap_states_[second_submap_index].trajectory, + 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, const int submap_index) { 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; const mapping::Submaps* const scan_trajectory = @@ -202,7 +207,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( GrowSubmapTransformsAsNeeded(insertion_submaps); const int matching_index = GetSubmapIndex(matching_submap); const transform::Rigid3d optimized_pose = - submap_transforms_[matching_index] * + optimization_problem_.submap_data().at(matching_index).pose * matching_submap->local_pose().inverse() * pose; CHECK_EQ(scan_index, optimization_problem_.node_data().size()); const mapping::TrajectoryNode::ConstantData* const scan_data = @@ -325,41 +330,46 @@ void SparsePoseGraph::RunFinalOptimization() { } void SparsePoseGraph::RunOptimization() { - if (!submap_transforms_.empty()) { - optimization_problem_.Solve(constraints_, &submap_transforms_); - common::MutexLocker locker(&mutex_); + if (optimization_problem_.submap_data().empty()) { + return; + } + optimization_problem_.Solve(constraints_); + common::MutexLocker locker(&mutex_); - const auto& node_data = optimization_problem_.node_data(); - const size_t num_optimized_poses = node_data.size(); - for (size_t i = 0; i != num_optimized_poses; ++i) { - trajectory_nodes_[i].pose = node_data[i].point_cloud_pose; + const auto& node_data = optimization_problem_.node_data(); + const size_t num_optimized_poses = node_data.size(); + for (size_t i = 0; i != num_optimized_poses; ++i) { + trajectory_nodes_[i].pose = node_data[i].point_cloud_pose; + } + // Extrapolate all point cloud poses that were added later. + std::unordered_map + extrapolation_transforms; + std::vector 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) { + const mapping::Submaps* trajectory = + trajectory_nodes_[i].constant_data->trajectory; + if (extrapolation_transforms.count(trajectory) == 0) { + const auto new_submap_transforms = + ExtrapolateSubmapTransforms(submap_transforms, trajectory); + const auto old_submap_transforms = + ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory); + CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size()); + extrapolation_transforms[trajectory] = + transform::Rigid3d(new_submap_transforms.back() * + old_submap_transforms.back().inverse()); } - // Extrapolate all point cloud poses that were added later. - std::unordered_map - extrapolation_transforms; - for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) { - const mapping::Submaps* trajectory = - trajectory_nodes_[i].constant_data->trajectory; - if (extrapolation_transforms.count(trajectory) == 0) { - const auto new_submap_transforms = - ExtrapolateSubmapTransforms(submap_transforms_, trajectory); - const auto old_submap_transforms = ExtrapolateSubmapTransforms( - optimized_submap_transforms_, trajectory); - CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size()); - extrapolation_transforms[trajectory] = - transform::Rigid3d(new_submap_transforms.back() * - old_submap_transforms.back().inverse()); - } - trajectory_nodes_[i].pose = - extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose; - } - optimized_submap_transforms_ = submap_transforms_; - connected_components_ = trajectory_connectivity_.ConnectedComponents(); - reverse_connected_components_.clear(); - for (size_t i = 0; i != connected_components_.size(); ++i) { - for (const auto* trajectory : connected_components_[i]) { - reverse_connected_components_.emplace(trajectory, i); - } + trajectory_nodes_[i].pose = + extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose; + } + optimized_submap_transforms_ = submap_transforms; + connected_components_ = trajectory_connectivity_.ConnectedComponents(); + reverse_connected_components_.clear(); + for (size_t i = 0; i != connected_components_.size(); ++i) { + for (const auto* trajectory : connected_components_[i]) { + reverse_connected_components_.emplace(trajectory, i); } } } diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 98be737..ea61f79 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -126,9 +126,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { return iterator->second; } - // Grows 'submap_transforms_' to have an entry for every element of 'submaps'. - void GrowSubmapTransformsAsNeeded(const std::vector& submaps) - REQUIRES(mutex_); + // Grows the optimization problem to have an entry for every element of + // 'insertion_submaps'. + void GrowSubmapTransformsAsNeeded( + const std::vector& insertion_submaps) REQUIRES(mutex_); // Adds constraints for a scan, and starts scan matching in the background. void ComputeConstraintsForScan( @@ -187,7 +188,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { sparse_pose_graph::OptimizationProblem optimization_problem_; sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_); std::vector constraints_; - std::vector submap_transforms_; // (map <- submap) // Submaps get assigned an index and state as soon as they are seen, even // before they take part in the background computations. diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index f1db4ce..328214f 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -91,14 +91,17 @@ void OptimizationProblem::AddTrajectoryNode( 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) { options_.mutable_ceres_solver_options()->set_max_num_iterations( max_num_iterations); } -void OptimizationProblem::Solve( - const std::vector& constraints, - std::vector* const submap_transforms) { +void OptimizationProblem::Solve(const std::vector& constraints) { if (node_data_.empty()) { // Nothing to optimize. return; @@ -125,7 +128,7 @@ void OptimizationProblem::Solve( std::deque C_submaps; std::deque C_point_clouds; // Tie the first submap to the origin. - CHECK(!submap_transforms->empty()); + CHECK(!submap_data_.empty()); C_submaps.emplace_back( transform::Rigid3d::Identity(), translation_parameterization(), common::make_uniquesize(); ++i) { + for (size_t i = 1; i != submap_data_.size(); ++i) { C_submaps.emplace_back( - (*submap_transforms)[i], translation_parameterization(), + submap_data_[i].pose, translation_parameterization(), common::make_unique(), &problem); } 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. for (const Constraint& constraint : constraints) { 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_LT(constraint.j, node_data_.size()); problem.AddResidualBlock( @@ -239,8 +242,8 @@ void OptimizationProblem::Solve( } // Store the result. - for (size_t i = 0; i != submap_transforms->size(); ++i) { - (*submap_transforms)[i] = C_submaps[i].ToRigid(); + for (size_t i = 0; i != submap_data_.size(); ++i) { + submap_data_[i].pose = C_submaps[i].ToRigid(); } for (size_t j = 0; j != node_data_.size(); ++j) { node_data_[j].point_cloud_pose = C_point_clouds[j].ToRigid(); @@ -251,6 +254,10 @@ const std::vector& OptimizationProblem::node_data() const { return node_data_; } +const std::vector& OptimizationProblem::submap_data() const { + return submap_data_; +} + } // namespace sparse_pose_graph } // namespace mapping_3d } // namespace cartographer diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h index c966873..1de87c9 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -42,6 +42,12 @@ struct NodeData { 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. class OptimizationProblem { public: @@ -63,20 +69,23 @@ class OptimizationProblem { const Eigen::Vector3d& angular_velocity); void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time, const transform::Rigid3d& point_cloud_pose); + void AddSubmap(const mapping::Submaps* trajectory, + const transform::Rigid3d& submap_pose); void SetMaxNumIterations(int32 max_num_iterations); // Computes the optimized poses. - void Solve(const std::vector& constraints, - std::vector* submap_transforms); + void Solve(const std::vector& constraints); const std::vector& node_data() const; + const std::vector& submap_data() const; private: mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; FixZ fix_z_; std::map> imu_data_; std::vector node_data_; + std::vector submap_data_; double gravity_constant_ = 9.8; }; diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc index 1db9d13..d234206 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc @@ -153,9 +153,6 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { 1e-9 * Eigen::Matrix::Identity()}}); } - std::vector submap_transforms = { - kSubmap0Transform, kSubmap0Transform, kSubmap2Transform}; - double translation_error_before = 0.; double rotation_error_before = 0.; const auto& node_data = optimization_problem_.node_data(); @@ -168,7 +165,10 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { 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 rotation_error_after = 0.;