diff --git a/cartographer/common/math.h b/cartographer/common/math.h index 286b2d7..ea96c0b 100644 --- a/cartographer/common/math.h +++ b/cartographer/common/math.h @@ -82,29 +82,6 @@ T atan2(const Eigen::Matrix& vector) { return ceres::atan2(vector.y(), vector.x()); } -// Computes 'A'^{-1/2} for A being symmetric, positive-semidefinite. -// Eigenvalues of 'A' are clamped to be at least 'lower_eigenvalue_bound'. -template -Eigen::Matrix ComputeSpdMatrixSqrtInverse( - const Eigen::Matrix& A, const double lower_eigenvalue_bound) { - Eigen::SelfAdjointEigenSolver> - covariance_eigen_solver(A); - if (covariance_eigen_solver.info() != Eigen::Success) { - LOG(WARNING) << "SelfAdjointEigenSolver failed; A =\n" << A; - return Eigen::Matrix::Identity(); - } - // Since we compute the inverse, we do not allow smaller values to avoid - // infinity and NaN. - const double relative_lower_bound = lower_eigenvalue_bound; - return covariance_eigen_solver.eigenvectors() * - covariance_eigen_solver.eigenvalues() - .cwiseMax(relative_lower_bound) - .cwiseInverse() - .cwiseSqrt() - .asDiagonal() * - covariance_eigen_solver.eigenvectors().inverse(); -} - } // namespace common } // namespace cartographer diff --git a/cartographer/mapping/proto/sparse_pose_graph_options.proto b/cartographer/mapping/proto/sparse_pose_graph_options.proto index dc7e8a3..67c77a7 100644 --- a/cartographer/mapping/proto/sparse_pose_graph_options.proto +++ b/cartographer/mapping/proto/sparse_pose_graph_options.proto @@ -28,9 +28,12 @@ message SparsePoseGraphOptions { optional mapping.sparse_pose_graph.proto.ConstraintBuilderOptions constraint_builder_options = 3; - // Weights used in the optimization problem for non-loop-closure scan matcher - // constraints. + // Weight used in the optimization problem for the translational component of + // non-loop-closure scan matcher constraints. optional double matcher_translation_weight = 7; + + // Weight used in the optimization problem for the rotational component of + // non-loop-closure scan matcher constraints. optional double matcher_rotation_weight = 8; // Options for the optimization problem. diff --git a/cartographer/mapping/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping/sparse_pose_graph/constraint_builder.cc index 273a3c3..8a04cef 100644 --- a/cartographer/mapping/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping/sparse_pose_graph/constraint_builder.cc @@ -38,8 +38,6 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions( options.set_min_score(parameter_dictionary->GetDouble("min_score")); options.set_global_localization_min_score( parameter_dictionary->GetDouble("global_localization_min_score")); - options.set_lower_covariance_eigenvalue_bound( - parameter_dictionary->GetDouble("lower_covariance_eigenvalue_bound")); options.set_loop_closure_translation_weight( parameter_dictionary->GetDouble("loop_closure_translation_weight")); options.set_loop_closure_rotation_weight( diff --git a/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto b/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto index 8d32849..b10beb9 100644 --- a/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto +++ b/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto @@ -41,12 +41,12 @@ message ConstraintBuilderOptions { // Threshold below which global localizations are not trusted. optional double global_localization_min_score = 5; - // Lower bound for covariance eigenvalues to limit the weight of matches. - // TODO(whess): Deprecated and only used in 3D. Replace usage and remove. - optional double lower_covariance_eigenvalue_bound = 7; - - // Weights used in the optimization problem for loop closure constraints. + // Weight used in the optimization problem for the translational component of + // loop closure constraints. optional double loop_closure_translation_weight = 13; + + // Weight used in the optimization problem for the rotational component of + // loop closure constraints. optional double loop_closure_rotation_weight = 14; // If enabled, logs information of loop-closing constraints for debugging. diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index f3efff5..387dc4a 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -78,7 +78,6 @@ class SparsePoseGraphTest : public ::testing::Test { }, min_score = 0.5, global_localization_min_score = 0.6, - lower_covariance_eigenvalue_bound = 1e-6, loop_closure_translation_weight = 1., loop_closure_rotation_weight = 1., log_matches = true, diff --git a/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/mapping_3d/global_trajectory_builder.cc index 5353511..003271a 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.cc +++ b/cartographer/mapping_3d/global_trajectory_builder.cc @@ -55,9 +55,8 @@ void GlobalTrajectoryBuilder::AddRangefinderData( sparse_pose_graph_->AddScan( insertion_result->time, insertion_result->range_data_in_tracking, - insertion_result->pose_observation, insertion_result->covariance_estimate, - trajectory_id_, insertion_result->matching_submap, - insertion_result->insertion_submaps); + insertion_result->pose_observation, trajectory_id_, + insertion_result->matching_submap, insertion_result->insertion_submaps); } void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time, diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc index eea1691..15526d6 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc @@ -178,17 +178,16 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData( .scan_matcher_variance() * kalman_filter::PoseCovariance::Identity()); - kalman_filter::PoseCovariance covariance_estimate; + kalman_filter::PoseCovariance unused_covariance_estimate; pose_tracker_->GetPoseEstimateMeanAndCovariance( - time, &scan_matcher_pose_estimate_, &covariance_estimate); + time, &scan_matcher_pose_estimate_, &unused_covariance_estimate); last_pose_estimate_ = { time, scan_matcher_pose_estimate_, sensor::TransformPointCloud(filtered_range_data.returns, pose_observation.cast())}; - return InsertIntoSubmap(time, filtered_range_data, pose_observation, - covariance_estimate); + return InsertIntoSubmap(time, filtered_range_data, pose_observation); } void KalmanLocalTrajectoryBuilder::AddOdometerData( @@ -216,8 +215,7 @@ KalmanLocalTrajectoryBuilder::pose_estimate() const { std::unique_ptr KalmanLocalTrajectoryBuilder::InsertIntoSubmap( const common::Time time, const sensor::RangeData& range_data_in_tracking, - const transform::Rigid3d& pose_observation, - const kalman_filter::PoseCovariance& covariance_estimate) { + const transform::Rigid3d& pose_observation) { if (motion_filter_.IsSimilar(time, pose_observation)) { return nullptr; } @@ -231,9 +229,9 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap( sensor::TransformRangeData(range_data_in_tracking, pose_observation.cast()), pose_tracker_->gravity_orientation()); - return std::unique_ptr(new InsertionResult{ - time, range_data_in_tracking, pose_observation, covariance_estimate, - matching_submap, insertion_submaps}); + return std::unique_ptr( + new InsertionResult{time, range_data_in_tracking, pose_observation, + matching_submap, insertion_submaps}); } } // namespace mapping_3d diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.h b/cartographer/mapping_3d/kalman_local_trajectory_builder.h index ab451d3..7ec7439 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.h +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.h @@ -62,8 +62,7 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface { std::unique_ptr InsertIntoSubmap( common::Time time, const sensor::RangeData& range_data_in_tracking, - const transform::Rigid3d& pose_observation, - const kalman_filter::PoseCovariance& covariance_estimate); + const transform::Rigid3d& pose_observation); const proto::LocalTrajectoryBuilderOptions options_; std::unique_ptr submaps_; diff --git a/cartographer/mapping_3d/local_trajectory_builder_interface.h b/cartographer/mapping_3d/local_trajectory_builder_interface.h index a8b1dff..26b9a53 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_interface.h +++ b/cartographer/mapping_3d/local_trajectory_builder_interface.h @@ -37,7 +37,6 @@ class LocalTrajectoryBuilderInterface { common::Time time; sensor::RangeData range_data_in_tracking; transform::Rigid3d pose_observation; - kalman_filter::PoseCovariance covariance_estimate; const Submap* matching_submap; std::vector insertion_submaps; }; diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc index 61c215c..f347dcf 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc @@ -19,7 +19,6 @@ #include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/make_unique.h" #include "cartographer/common/time.h" -#include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.pb.h" #include "cartographer/mapping_3d/rotation_cost_function.h" #include "cartographer/mapping_3d/scan_matching/occupied_space_cost_functor.h" @@ -424,12 +423,9 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap( pose_observation.cast()), kFakeGravityOrientation); - const kalman_filter::PoseCovariance kCovariance = - 1e-7 * kalman_filter::PoseCovariance::Identity(); - return std::unique_ptr( new InsertionResult{time, range_data_in_tracking, pose_observation, - kCovariance, matching_submap, insertion_submaps}); + matching_submap, insertion_submaps}); } OptimizingLocalTrajectoryBuilder::State diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 268a437..5ae5d36 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -88,8 +88,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded( void SparsePoseGraph::AddScan( common::Time time, const sensor::RangeData& range_data_in_tracking, - const transform::Rigid3d& pose, - const kalman_filter::PoseCovariance& covariance, const int trajectory_id, + const transform::Rigid3d& pose, const int trajectory_id, const Submap* const matching_submap, const std::vector& insertion_submaps) { const transform::Rigid3d optimized_pose( @@ -124,7 +123,7 @@ void SparsePoseGraph::AddScan( AddWorkItem([=]() REQUIRES(mutex_) { ComputeConstraintsForScan(matching_submap, insertion_submaps, - finished_submap, pose, covariance); + finished_submap, pose); }); } @@ -224,8 +223,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) { void SparsePoseGraph::ComputeConstraintsForScan( const Submap* matching_submap, std::vector insertion_submaps, - const Submap* finished_submap, const transform::Rigid3d& pose, - const kalman_filter::PoseCovariance& covariance) { + const Submap* finished_submap, const transform::Rigid3d& pose) { GrowSubmapTransformsAsNeeded(insertion_submaps); const mapping::SubmapId matching_id = GetSubmapId(matching_submap); const transform::Rigid3d optimized_pose = @@ -251,17 +249,15 @@ void SparsePoseGraph::ComputeConstraintsForScan( const mapping::SubmapId submap_id = GetSubmapId(submap); CHECK(!submap_states_.at(submap_id).finished); submap_states_.at(submap_id).node_ids.emplace(node_id); - // Unchanged covariance as (submap <- map) is a translation. const transform::Rigid3d constraint_transform = submap->local_pose.inverse() * pose; - constraints_.push_back( - Constraint{submap_id, - node_id, - {constraint_transform, - common::ComputeSpdMatrixSqrtInverse( - covariance, options_.constraint_builder_options() - .lower_covariance_eigenvalue_bound())}, - Constraint::INTRA_SUBMAP}); + constraints_.push_back(Constraint{ + submap_id, + node_id, + {constraint_transform, mapping::FromTranslationRotationWeights( + options_.matcher_translation_weight(), + options_.matcher_rotation_weight())}, + Constraint::INTRA_SUBMAP}); } for (int trajectory_id = 0; trajectory_id < submap_states_.num_trajectories(); diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index e67e264..1075ac6 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -30,7 +30,6 @@ #include "cartographer/common/mutex.h" #include "cartographer/common/thread_pool.h" #include "cartographer/common/time.h" -#include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/trajectory_connectivity.h" #include "cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h" @@ -67,9 +66,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // 'insertion_submaps'. void AddScan(common::Time time, const sensor::RangeData& range_data_in_tracking, - const transform::Rigid3d& pose, - const kalman_filter::PoseCovariance& pose_covariance, - int trajectory_id, const Submap* matching_submap, + const transform::Rigid3d& pose, int trajectory_id, + const Submap* matching_submap, const std::vector& insertion_submaps) EXCLUDES(mutex_); @@ -120,11 +118,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const std::vector& insertion_submaps) REQUIRES(mutex_); // Adds constraints for a scan, and starts scan matching in the background. - void ComputeConstraintsForScan( - const Submap* matching_submap, - std::vector insertion_submaps, - const Submap* finished_submap, const transform::Rigid3d& pose, - const kalman_filter::PoseCovariance& covariance) REQUIRES(mutex_); + void ComputeConstraintsForScan(const Submap* matching_submap, + std::vector insertion_submaps, + const Submap* finished_submap, + const transform::Rigid3d& pose) + REQUIRES(mutex_); // Computes constraints for a scan and submap pair. void ComputeConstraint(const mapping::NodeId& node_id, diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index 079b6f6..dfabaae 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -225,9 +225,9 @@ void ConstraintBuilder::ComputeConstraint( constraint->reset(new OptimizationProblem::Constraint{ submap_id, node_id, - {constraint_transform, - 1. / std::sqrt(options_.lower_covariance_eigenvalue_bound()) * - kalman_filter::PoseCovariance::Identity()}, + {constraint_transform, mapping::FromTranslationRotationWeights( + options_.loop_closure_translation_weight(), + options_.loop_closure_rotation_weight())}, OptimizationProblem::Constraint::INTER_SUBMAP}); if (options_.log_matches()) { diff --git a/configuration_files/sparse_pose_graph.lua b/configuration_files/sparse_pose_graph.lua index 8a6e2db..1b1131e 100644 --- a/configuration_files/sparse_pose_graph.lua +++ b/configuration_files/sparse_pose_graph.lua @@ -24,7 +24,6 @@ SPARSE_POSE_GRAPH = { }, min_score = 0.55, global_localization_min_score = 0.6, - lower_covariance_eigenvalue_bound = 1e-11, loop_closure_translation_weight = 1.1e4, loop_closure_rotation_weight = 1e5, log_matches = true, @@ -68,8 +67,8 @@ SPARSE_POSE_GRAPH = { matcher_rotation_weight = 1.6e3, optimization_problem = { huber_scale = 1e1, - acceleration_weight = 1e4, - rotation_weight = 3e6, + acceleration_weight = 1e3, + rotation_weight = 3e5, consecutive_scan_translation_penalty_factor = 1e5, consecutive_scan_rotation_penalty_factor = 1e5, log_solver_summary = false, diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index dff8e91..0158812 100644 --- a/docs/source/configuration.rst +++ b/docs/source/configuration.rst @@ -62,15 +62,9 @@ cartographer.mapping.proto.MapBuilderOptions bool use_trajectory_builder_2d Not yet documented. -cartographer.mapping_2d.proto.LocalTrajectoryBuilderOptions trajectory_builder_2d_options - Not yet documented. - bool use_trajectory_builder_3d Not yet documented. -cartographer.mapping_3d.proto.LocalTrajectoryBuilderOptions trajectory_builder_3d_options - Not yet documented. - int32 num_background_threads Number of threads to use for background computations. @@ -88,6 +82,14 @@ int32 optimize_every_n_scans cartographer.mapping.sparse_pose_graph.proto.ConstraintBuilderOptions constraint_builder_options Options for the constraint builder. +double matcher_translation_weight + Weight used in the optimization problem for the translational component of + non-loop-closure scan matcher constraints. + +double matcher_rotation_weight + Weight used in the optimization problem for the rotational component of + non-loop-closure scan matcher constraints. + cartographer.mapping.sparse_pose_graph.proto.OptimizationProblemOptions optimization_problem_options Options for the optimization problem. @@ -100,6 +102,16 @@ double global_sampling_ratio localization. +cartographer.mapping.proto.TrajectoryBuilderOptions +=================================================== + +cartographer.mapping_2d.proto.LocalTrajectoryBuilderOptions trajectory_builder_2d_options + Not yet documented. + +cartographer.mapping_3d.proto.LocalTrajectoryBuilderOptions trajectory_builder_3d_options + Not yet documented. + + cartographer.mapping.sparse_pose_graph.proto.ConstraintBuilderOptions ===================================================================== @@ -120,8 +132,13 @@ double min_score double global_localization_min_score Threshold below which global localizations are not trusted. -double lower_covariance_eigenvalue_bound - Lower bound for covariance eigenvalues to limit the weight of matches. +double loop_closure_translation_weight + Weight used in the optimization problem for the translational component of + loop closure constraints. + +double loop_closure_rotation_weight + Weight used in the optimization problem for the rotational component of + loop closure constraints. bool log_matches If enabled, logs information of loop-closing constraints for debugging.