diff --git a/cartographer/mapping/sparse_pose_graph/optimization_problem_options.cc b/cartographer/mapping/sparse_pose_graph/optimization_problem_options.cc index 8d1b335..656dadc 100644 --- a/cartographer/mapping/sparse_pose_graph/optimization_problem_options.cc +++ b/cartographer/mapping/sparse_pose_graph/optimization_problem_options.cc @@ -38,8 +38,6 @@ proto::OptimizationProblemOptions CreateOptimizationProblemOptions( "consecutive_scan_rotation_penalty_factor")); options.set_log_solver_summary( parameter_dictionary->GetBool("log_solver_summary")); - options.set_log_residual_histograms( - parameter_dictionary->GetBool("log_residual_histograms")); *options.mutable_ceres_solver_options() = common::CreateCeresSolverOptionsProto( parameter_dictionary->GetDictionary("ceres_solver_options").get()); diff --git a/cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.proto b/cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.proto index 2b10f5e..7dbcca2 100644 --- a/cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.proto +++ b/cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.proto @@ -36,9 +36,5 @@ message OptimizationProblemOptions { // If true, the Ceres solver summary will be logged for every optimization. optional bool log_solver_summary = 5; - // If true, histograms of the final residual values after optimization will be - // logged for every optimization. - optional bool log_residual_histograms = 10; - optional common.proto.CeresSolverOptions ceres_solver_options = 7; } diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc index 22c4882..fc18b0a 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ b/cartographer/mapping_2d/global_trajectory_builder.cc @@ -54,6 +54,8 @@ void GlobalTrajectoryBuilder::AddImuData( const Eigen::Vector3d& angular_velocity) { local_trajectory_builder_.AddImuData(time, linear_acceleration, angular_velocity); + sparse_pose_graph_->AddImuData(local_trajectory_builder_.submaps(), time, + linear_acceleration, angular_velocity); } void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time, diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index c544388..32e5a23 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -127,7 +127,7 @@ void SparsePoseGraph::AddScan( } AddWorkItem([=]() REQUIRES(mutex_) { - ComputeConstraintsForScan(j, matching_submap, insertion_submaps, + ComputeConstraintsForScan(time, j, matching_submap, insertion_submaps, finished_submap, pose, covariance); }); } @@ -140,11 +140,22 @@ void SparsePoseGraph::AddWorkItem(std::function work_item) { } } +void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory, + common::Time time, + const Eigen::Vector3d& linear_acceleration, + const Eigen::Vector3d& angular_velocity) { + common::MutexLocker locker(&mutex_); + AddWorkItem([=]() REQUIRES(mutex_) { + optimization_problem_.AddImuData(trajectory, time, linear_acceleration, + angular_velocity); + }); +} + void SparsePoseGraph::ComputeConstraint(const int scan_index, const int submap_index) { const transform::Rigid2d relative_pose = submap_transforms_[submap_index].inverse() * - point_cloud_poses_[scan_index]; + optimization_problem_.node_data().at(scan_index).point_cloud_pose; const mapping::Submaps* const scan_trajectory = trajectory_nodes_[scan_index].constant_data->trajectory; @@ -176,9 +187,10 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index, void SparsePoseGraph::ComputeConstraintsForOldScans( const mapping::Submap* submap) { const int submap_index = GetSubmapIndex(submap); - CHECK_GT(point_cloud_poses_.size(), 0); - CHECK_LT(point_cloud_poses_.size(), std::numeric_limits::max()); - const int num_nodes = point_cloud_poses_.size(); + const auto& node_data = optimization_problem_.node_data(); + CHECK_GT(node_data.size(), 0); + CHECK_LT(node_data.size(), std::numeric_limits::max()); + const int num_nodes = node_data.size(); for (int scan_index = 0; scan_index < num_nodes; ++scan_index) { if (submap_states_[submap_index].scan_indices.count(scan_index) == 0) { ComputeConstraint(scan_index, submap_index); @@ -187,7 +199,8 @@ void SparsePoseGraph::ComputeConstraintsForOldScans( } void SparsePoseGraph::ComputeConstraintsForScan( - int scan_index, const mapping::Submap* matching_submap, + const common::Time time, const int scan_index, + const mapping::Submap* matching_submap, std::vector insertion_submaps, const mapping::Submap* finished_submap, const transform::Rigid2d& pose, const kalman_filter::Pose2DCovariance& covariance) { @@ -196,9 +209,11 @@ void SparsePoseGraph::ComputeConstraintsForScan( const transform::Rigid2d optimized_pose = submap_transforms_[matching_index] * sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose; - CHECK_EQ(scan_index, point_cloud_poses_.size()); - initial_point_cloud_poses_.push_back(pose); - point_cloud_poses_.push_back(optimized_pose); + CHECK_EQ(scan_index, optimization_problem_.node_data().size()); + const mapping::Submaps* const scan_trajectory = + trajectory_nodes_[scan_index].constant_data->trajectory; + optimization_problem_.AddTrajectoryNode(scan_trajectory, time, pose, + optimized_pose); for (const mapping::Submap* submap : insertion_submaps) { const int submap_index = GetSubmapIndex(submap); CHECK(!submap_states_[submap_index].finished); @@ -206,8 +221,8 @@ void SparsePoseGraph::ComputeConstraintsForScan( // Unchanged covariance as (submap <- map) is a translation. const transform::Rigid2d constraint_transform = sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose; - constexpr double kFakePositionCovariance = 1.; - constexpr double kFakeOrientationCovariance = 1.; + constexpr double kFakePositionCovariance = 1e-6; + constexpr double kFakeOrientationCovariance = 1e-6; constraints_.push_back(Constraint{ submap_index, scan_index, @@ -318,24 +333,15 @@ void SparsePoseGraph::RunFinalOptimization() { void SparsePoseGraph::RunOptimization() { if (!submap_transforms_.empty()) { - std::vector trajectories; - { - common::MutexLocker locker(&mutex_); - CHECK(!submap_states_.empty()); - for (const auto& trajectory_node : trajectory_nodes_) { - trajectories.push_back(trajectory_node.constant_data->trajectory); - } - } - - optimization_problem_.Solve(constraints_, trajectories, - initial_point_cloud_poses_, &point_cloud_poses_, - &submap_transforms_); + optimization_problem_.Solve(constraints_, &submap_transforms_); common::MutexLocker locker(&mutex_); has_new_optimized_poses_ = true; - const size_t num_optimized_poses = point_cloud_poses_.size(); + + 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(point_cloud_poses_[i])); + transform::Rigid3d(transform::Embed3D(node_data[i].point_cloud_pose)); } // Extrapolate all point cloud poses that were added later. std::unordered_map diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index e01ee31..4e747eb 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -80,6 +80,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const std::vector& insertion_submaps) EXCLUDES(mutex_); + // Adds new IMU data to be used in the optimization. + void AddImuData(const mapping::Submaps* trajectory, common::Time time, + const Eigen::Vector3d& linear_acceleration, + const Eigen::Vector3d& angular_velocity); + void RunFinalOptimization() override; bool HasNewOptimizedPoses() override; mapping::proto::ScanMatchingProgress GetScanMatchingProgress() override; @@ -127,7 +132,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Adds constraints for a scan, and starts scan matching in the background. void ComputeConstraintsForScan( - int scan_index, const mapping::Submap* matching_submap, + common::Time time, int scan_index, const mapping::Submap* matching_submap, std::vector insertion_submaps, const mapping::Submap* finished_submap, const transform::Rigid2d& pose, const kalman_filter::Pose2DCovariance& covariance) REQUIRES(mutex_); @@ -183,8 +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 initial_point_cloud_poses_; - std::vector point_cloud_poses_; // (map <- point cloud) std::vector submap_transforms_; // (map <- submap) // Submaps get assigned an index and state as soon as they are seen, even diff --git a/cartographer/mapping_2d/sparse_pose_graph/CMakeLists.txt b/cartographer/mapping_2d/sparse_pose_graph/CMakeLists.txt index 0d7cc0a..d7a56ce 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/CMakeLists.txt +++ b/cartographer/mapping_2d/sparse_pose_graph/CMakeLists.txt @@ -55,8 +55,10 @@ google_library(mapping_2d_sparse_pose_graph_optimization_problem common_histogram common_math common_port + common_time mapping_2d_sparse_pose_graph_spa_cost_function mapping_2d_submaps + mapping_3d_imu_integration mapping_sparse_pose_graph mapping_sparse_pose_graph_proto_optimization_problem_options transform_transform diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index 7bffa6c..4261dff 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -228,8 +228,8 @@ void ConstraintBuilder::ComputeConstraint( const transform::Rigid2d constraint_transform = ComputeSubmapPose(*submap).inverse() * pose_estimate; - constexpr double kFakePositionCovariance = 1.; - constexpr double kFakeOrientationCovariance = 1.; + constexpr double kFakePositionCovariance = 1e-6; + constexpr double kFakeOrientationCovariance = 1e-6; constraint->reset(new Constraint{ submap_index, scan_index, diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 56d7259..091a29a 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -59,6 +59,22 @@ OptimizationProblem::OptimizationProblem( OptimizationProblem::~OptimizationProblem() {} +void OptimizationProblem::AddImuData(const mapping::Submaps* const trajectory, + const common::Time time, + const Eigen::Vector3d& linear_acceleration, + const Eigen::Vector3d& angular_velocity) { + imu_data_[trajectory].push_back( + mapping_3d::ImuData{time, linear_acceleration, angular_velocity}); +} + +void OptimizationProblem::AddTrajectoryNode( + const mapping::Submaps* const trajectory, const common::Time time, + const transform::Rigid2d& initial_point_cloud_pose, + const transform::Rigid2d& point_cloud_pose) { + node_data_.push_back( + NodeData{trajectory, time, initial_point_cloud_pose, point_cloud_pose}); +} + void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { options_.mutable_ceres_solver_options()->set_max_num_iterations( max_num_iterations); @@ -66,11 +82,8 @@ void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { void OptimizationProblem::Solve( const std::vector& constraints, - const std::vector& trajectories, - const std::vector& initial_point_cloud_poses, - std::vector* point_cloud_poses, - std::vector* submap_transforms) { - if (point_cloud_poses->empty()) { + std::vector* const submap_transforms) { + if (node_data_.empty()) { // Nothing to optimize. return; } @@ -80,13 +93,13 @@ void OptimizationProblem::Solve( // Set the starting point. std::vector> C_submaps(submap_transforms->size()); - std::vector> C_point_clouds(point_cloud_poses->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]); problem.AddParameterBlock(C_submaps[i].data(), 3); } - for (size_t j = 0; j != point_cloud_poses->size(); ++j) { - C_point_clouds[j] = FromPose((*point_cloud_poses)[j]); + for (size_t j = 0; j != node_data_.size(); ++j) { + C_point_clouds[j] = FromPose(node_data_[j].point_cloud_pose); problem.AddParameterBlock(C_point_clouds[j].data(), 3); } @@ -100,7 +113,7 @@ void OptimizationProblem::Solve( CHECK_GE(constraint.i, 0); CHECK_LT(constraint.i, submap_transforms->size()); CHECK_GE(constraint.j, 0); - CHECK_LT(constraint.j, point_cloud_poses->size()); + CHECK_LT(constraint.j, node_data_.size()); constraints_residual_blocks.emplace_back( constraint.tag, problem.AddResidualBlock( @@ -115,22 +128,19 @@ void OptimizationProblem::Solve( } // Add penalties for changes between consecutive scans. - CHECK(!point_cloud_poses->empty()); const Eigen::DiagonalMatrix consecutive_pose_change_penalty_matrix( options_.consecutive_scan_translation_penalty_factor(), options_.consecutive_scan_translation_penalty_factor(), options_.consecutive_scan_rotation_penalty_factor()); - CHECK_GE(initial_point_cloud_poses.size(), point_cloud_poses->size()); - CHECK_GE(trajectories.size(), point_cloud_poses->size()); - // The poses in initial_point_cloud_poses and point_cloud_poses are - // interleaved from multiple trajectories (although the points from a given - // trajectory are in time order). 'last_pose_indices[trajectory]' is the index - // into 'initial_point_cloud_poses' of the most-recent pose on 'trajectory'. + // The poses in 'node_data_' are interleaved from multiple trajectories + // (although the points from a given trajectory are in time order). + // 'last_pose_indices[trajectory]' is the index of the most-recent pose on + // 'trajectory'. std::map last_pose_indices; - for (size_t j = 0; j != point_cloud_poses->size(); ++j) { - const mapping::Submaps* trajectory = trajectories[j]; + for (size_t j = 0; j != node_data_.size(); ++j) { + const mapping::Submaps* trajectory = node_data_[j].trajectory; // This pose has a predecessor. if (last_pose_indices.count(trajectory) != 0) { const int last_pose_index = last_pose_indices[trajectory]; @@ -139,9 +149,9 @@ void OptimizationProblem::Solve( problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new SpaCostFunction(Constraint::Pose{ - transform::Embed3D( - initial_point_cloud_poses[last_pose_index].inverse() * - initial_point_cloud_poses[j]), + transform::Embed3D(node_data_[last_pose_index] + .initial_point_cloud_pose.inverse() * + node_data_[j].initial_point_cloud_pose), kalman_filter::Embed3D(consecutive_pose_change_penalty_matrix, kUnusedPositionPenalty, kUnusedOrientationPenalty)})), @@ -157,41 +167,6 @@ void OptimizationProblem::Solve( common::CreateCeresSolverOptions(options_.ceres_solver_options()), &problem, &summary); - if (options_.log_residual_histograms()) { - common::Histogram intra_submap_xy_residuals; - common::Histogram intra_submap_theta_residuals; - common::Histogram inter_submap_xy_residuals; - common::Histogram inter_submap_theta_residuals; - for (auto constraint_residual_block : constraints_residual_blocks) { - ceres::Problem::EvaluateOptions options; - options.apply_loss_function = false; - options.residual_blocks = {constraint_residual_block.second}; - std::vector residuals; - problem.Evaluate(options, nullptr, &residuals, nullptr, nullptr); - CHECK_EQ(3, residuals.size()); - switch (constraint_residual_block.first) { - case Constraint::INTRA_SUBMAP: - intra_submap_xy_residuals.Add(common::Pow2(residuals[0]) + - common::Pow2(residuals[1])); - intra_submap_theta_residuals.Add(common::Pow2(residuals[2])); - break; - case Constraint::INTER_SUBMAP: - inter_submap_xy_residuals.Add(common::Pow2(residuals[0]) + - common::Pow2(residuals[1])); - inter_submap_theta_residuals.Add(common::Pow2(residuals[2])); - break; - } - } - LOG(INFO) << "Intra-submap x^2 + y^2 residual histogram:\n" - << intra_submap_xy_residuals.ToString(10); - LOG(INFO) << "Intra-submap theta^2 residual histogram:\n" - << intra_submap_theta_residuals.ToString(10); - LOG(INFO) << "Inter-submap x^2 + y^2 residual histogram:\n" - << inter_submap_xy_residuals.ToString(10); - LOG(INFO) << "Inter-submap theta^2 residual histogram:\n" - << inter_submap_theta_residuals.ToString(10); - } - if (options_.log_solver_summary()) { LOG(INFO) << summary.FullReport(); } @@ -200,11 +175,15 @@ void OptimizationProblem::Solve( for (size_t i = 0; i != submap_transforms->size(); ++i) { (*submap_transforms)[i] = ToPose(C_submaps[i]); } - for (size_t j = 0; j != point_cloud_poses->size(); ++j) { - (*point_cloud_poses)[j] = ToPose(C_point_clouds[j]); + for (size_t j = 0; j != node_data_.size(); ++j) { + node_data_[j].point_cloud_pose = ToPose(C_point_clouds[j]); } } +const std::vector& OptimizationProblem::node_data() const { + return node_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 0a117ab..1599e33 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -24,14 +24,24 @@ #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/port.h" +#include "cartographer/common/time.h" #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h" #include "cartographer/mapping_2d/submaps.h" +#include "cartographer/mapping_3d/imu_integration.h" namespace cartographer { namespace mapping_2d { namespace sparse_pose_graph { +struct NodeData { + // TODO(whess): Keep nodes per trajectory instead. + const mapping::Submaps* trajectory; + common::Time time; + transform::Rigid2d initial_point_cloud_pose; + transform::Rigid2d point_cloud_pose; +}; + // Implements the SPA loop closure method. class OptimizationProblem { public: @@ -45,19 +55,25 @@ class OptimizationProblem { OptimizationProblem(const OptimizationProblem&) = delete; OptimizationProblem& operator=(const OptimizationProblem&) = delete; + void AddImuData(const mapping::Submaps* trajectory, common::Time time, + const Eigen::Vector3d& linear_acceleration, + const Eigen::Vector3d& angular_velocity); + void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time, + const transform::Rigid2d& initial_point_cloud_pose, + const transform::Rigid2d& point_cloud_pose); + void SetMaxNumIterations(int32 max_num_iterations); - // Computes the optimized poses. The point cloud at 'point_cloud_poses[i]' - // belongs to 'trajectories[i]'. Within a given trajectory, scans are expected - // to be contiguous. + // Computes the optimized poses. void Solve(const std::vector& constraints, - const std::vector& trajectories, - const std::vector& initial_point_cloud_poses, - std::vector* point_cloud_poses, std::vector* submap_transforms); + const std::vector& node_data() const; + private: mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; + std::map> imu_data_; + std::vector node_data_; }; } // namespace sparse_pose_graph diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index 3cb77c3..d589924 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -125,7 +125,6 @@ class SparsePoseGraphTest : public ::testing::Test { consecutive_scan_translation_penalty_factor = 0., consecutive_scan_rotation_penalty_factor = 0., log_solver_summary = true, - log_residual_histograms = true, ceres_solver_options = { use_nonmonotonic_steps = false, max_num_iterations = 200, diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index a363042..f1db4ce 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -30,7 +30,6 @@ #include "cartographer/common/time.h" #include "cartographer/mapping_3d/acceleration_cost_function.h" #include "cartographer/mapping_3d/ceres_pose.h" -#include "cartographer/mapping_3d/imu_integration.h" #include "cartographer/mapping_3d/rotation_cost_function.h" #include "cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h" #include "cartographer/transform/transform.h" 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 6646c5a..2ed6c48 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc @@ -47,7 +47,6 @@ class OptimizationProblemTest : public ::testing::Test { consecutive_scan_translation_penalty_factor = 1e-2, consecutive_scan_rotation_penalty_factor = 1e-2, log_solver_summary = true, - log_residual_histograms = true, ceres_solver_options = { use_nonmonotonic_steps = false, max_num_iterations = 200, diff --git a/configuration_files/sparse_pose_graph.lua b/configuration_files/sparse_pose_graph.lua index d9731e0..9a35254 100644 --- a/configuration_files/sparse_pose_graph.lua +++ b/configuration_files/sparse_pose_graph.lua @@ -71,7 +71,6 @@ SPARSE_POSE_GRAPH = { consecutive_scan_translation_penalty_factor = 1e5, consecutive_scan_rotation_penalty_factor = 1e5, log_solver_summary = false, - log_residual_histograms = false, ceres_solver_options = { use_nonmonotonic_steps = false, max_num_iterations = 50,