diff --git a/cartographer/mapping/proto/sparse_pose_graph_options.proto b/cartographer/mapping/proto/sparse_pose_graph_options.proto index 7436231..dc7e8a3 100644 --- a/cartographer/mapping/proto/sparse_pose_graph_options.proto +++ b/cartographer/mapping/proto/sparse_pose_graph_options.proto @@ -28,6 +28,11 @@ 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. + optional double matcher_translation_weight = 7; + optional double matcher_rotation_weight = 8; + // Options for the optimization problem. optional mapping.sparse_pose_graph.proto.OptimizationProblemOptions optimization_problem_options = 4; diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index a420ae0..0b46a3a 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -16,7 +16,6 @@ #include "cartographer/mapping/sparse_pose_graph.h" -#include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/mapping/sparse_pose_graph/constraint_builder.h" #include "cartographer/mapping/sparse_pose_graph/optimization_problem_options.h" #include "cartographer/transform/transform.h" @@ -44,6 +43,10 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( *options.mutable_constraint_builder_options() = sparse_pose_graph::CreateConstraintBuilderOptions( parameter_dictionary->GetDictionary("constraint_builder").get()); + options.set_matcher_translation_weight( + parameter_dictionary->GetDouble("matcher_translation_weight")); + options.set_matcher_rotation_weight( + parameter_dictionary->GetDouble("matcher_rotation_weight")); *options.mutable_optimization_problem_options() = sparse_pose_graph::CreateOptimizationProblemOptions( parameter_dictionary->GetDictionary("optimization_problem").get()); @@ -55,6 +58,20 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( return options; } +Eigen::Matrix FromTranslationRotationWeights( + const double translation_weight, const double rotation_weight) { + Eigen::Matrix result; + // clang-format off + result << translation_weight, 0., 0., 0., 0., 0., + 0., translation_weight, 0., 0., 0., 0., + 0., 0., translation_weight, 0., 0., 0., + 0., 0., 0., rotation_weight, 0., 0., + 0., 0., 0., 0., rotation_weight, 0., + 0., 0., 0., 0., 0., rotation_weight; + // clang-format on + return result; +} + proto::SparsePoseGraph SparsePoseGraph::ToProto() { proto::SparsePoseGraph proto; diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index 280172b..83915d6 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -35,6 +35,10 @@ namespace mapping { proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( common::LuaParameterDictionary* const parameter_dictionary); +// TODO(whess): Change to two doubles for performance. +Eigen::Matrix FromTranslationRotationWeights( + double translation_weight, double rotation_weight); + class SparsePoseGraph { public: // A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse diff --git a/cartographer/mapping/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping/sparse_pose_graph/constraint_builder.cc index fa11215..273a3c3 100644 --- a/cartographer/mapping/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping/sparse_pose_graph/constraint_builder.cc @@ -40,6 +40,10 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions( 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( + parameter_dictionary->GetDouble("loop_closure_rotation_weight")); options.set_log_matches(parameter_dictionary->GetBool("log_matches")); *options.mutable_fast_correlative_scan_matcher_options() = mapping_2d::scan_matching::CreateFastCorrelativeScanMatcherOptions( 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 4115dbe..8d32849 100644 --- a/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto +++ b/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto @@ -42,8 +42,13 @@ message ConstraintBuilderOptions { 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. + optional double loop_closure_translation_weight = 13; + optional double loop_closure_rotation_weight = 14; + // If enabled, logs information of loop-closing constraints for debugging. optional bool log_matches = 8; diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc index 64aa15a..88c6552 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ b/cartographer/mapping_2d/global_trajectory_builder.cc @@ -42,10 +42,8 @@ void GlobalTrajectoryBuilder::AddRangefinderData( sparse_pose_graph_->AddScan( insertion_result->time, insertion_result->tracking_to_tracking_2d, insertion_result->range_data_in_tracking_2d, - insertion_result->pose_estimate_2d, - kalman_filter::Project2D(insertion_result->covariance_estimate), - trajectory_id_, insertion_result->matching_submap, - insertion_result->insertion_submaps); + insertion_result->pose_estimate_2d, trajectory_id_, + insertion_result->matching_submap, insertion_result->insertion_submaps); } } diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index a128b95..5b1f9aa 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -69,8 +69,7 @@ void LocalTrajectoryBuilder::ScanMatch( common::Time time, const transform::Rigid3d& pose_prediction, const transform::Rigid3d& tracking_to_tracking_2d, const sensor::RangeData& range_data_in_tracking_2d, - transform::Rigid3d* pose_observation, - kalman_filter::PoseCovariance* covariance_observation) { + transform::Rigid3d* pose_observation) { const ProbabilityGrid& probability_grid = submaps_.Get(submaps_.matching_index())->probability_grid; transform::Rigid2d pose_prediction_2d = @@ -89,21 +88,13 @@ void LocalTrajectoryBuilder::ScanMatch( } transform::Rigid2d tracking_2d_to_map; - kalman_filter::Pose2DCovariance covariance_observation_2d; ceres::Solver::Summary summary; ceres_scan_matcher_.Match(pose_prediction_2d, initial_ceres_pose, filtered_point_cloud_in_tracking_2d, - probability_grid, &tracking_2d_to_map, - &covariance_observation_2d, &summary); + probability_grid, &tracking_2d_to_map, &summary); *pose_observation = transform::Embed3D(tracking_2d_to_map) * tracking_to_tracking_2d; - // This covariance is used for non-yaw rotation and the fake height of 0. - constexpr double kFakePositionCovariance = 1.; - constexpr double kFakeOrientationCovariance = 1.; - *covariance_observation = - kalman_filter::Embed3D(covariance_observation_2d, kFakePositionCovariance, - kFakeOrientationCovariance); } std::unique_ptr @@ -144,10 +135,8 @@ LocalTrajectoryBuilder::AddHorizontalRangeData( return nullptr; } - kalman_filter::PoseCovariance covariance_observation; ScanMatch(time, pose_prediction, tracking_to_tracking_2d, - range_data_in_tracking_2d, &pose_estimate_, - &covariance_observation); + range_data_in_tracking_2d, &pose_estimate_); odometry_correction_ = transform::Rigid3d::Identity(); if (!odometry_state_tracker_.empty()) { // We add an odometry state, so that the correction from the scan matching @@ -199,7 +188,7 @@ LocalTrajectoryBuilder::AddHorizontalRangeData( return common::make_unique(InsertionResult{ time, matching_submap, insertion_submaps, tracking_to_tracking_2d, - range_data_in_tracking_2d, pose_estimate_2d, covariance_observation}); + range_data_in_tracking_2d, pose_estimate_2d}); } const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index fa0cb96..56ba529 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -46,7 +46,6 @@ class LocalTrajectoryBuilder { transform::Rigid3d tracking_to_tracking_2d; sensor::RangeData range_data_in_tracking_2d; transform::Rigid2d pose_estimate_2d; - kalman_filter::PoseCovariance covariance_estimate; }; explicit LocalTrajectoryBuilder( @@ -71,13 +70,12 @@ class LocalTrajectoryBuilder { const transform::Rigid3f& tracking_to_tracking_2d, const sensor::RangeData& range_data) const; - // Scan match 'range_data_in_tracking_2d' and fill in the - // 'pose_observation' and 'covariance_observation' with the result. + // Scan matches 'range_data_in_tracking_2d' and fill in the 'pose_observation' + // with the result. void ScanMatch(common::Time time, const transform::Rigid3d& pose_prediction, const transform::Rigid3d& tracking_to_tracking_2d, const sensor::RangeData& range_data_in_tracking_2d, - transform::Rigid3d* pose_observation, - kalman_filter::PoseCovariance* covariance_observation); + transform::Rigid3d* pose_observation); // Lazily constructs an ImuTracker. void InitializeImuTracker(common::Time time); diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc index ae61c52..d55f0b7 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc +++ b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc @@ -22,7 +22,6 @@ #include "Eigen/Core" #include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h" #include "cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h" @@ -44,8 +43,6 @@ proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions( parameter_dictionary->GetDouble("translation_weight")); options.set_rotation_weight( parameter_dictionary->GetDouble("rotation_weight")); - options.set_covariance_scale( - parameter_dictionary->GetDouble("covariance_scale")); *options.mutable_ceres_solver_options() = common::CreateCeresSolverOptionsProto( parameter_dictionary->GetDictionary("ceres_solver_options").get()); @@ -67,7 +64,6 @@ void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose, const sensor::PointCloud& point_cloud, const ProbabilityGrid& probability_grid, transform::Rigid2d* const pose_estimate, - kalman_filter::Pose2DCovariance* const covariance, ceres::Solver::Summary* const summary) const { double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(), initial_pose_estimate.translation().y(), @@ -100,17 +96,6 @@ void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose, *pose_estimate = transform::Rigid2d( {ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]); - - ceres::Covariance::Options options; - ceres::Covariance covariance_computer(options); - std::vector> covariance_blocks; - covariance_blocks.emplace_back(ceres_pose_estimate, ceres_pose_estimate); - CHECK(covariance_computer.Compute(covariance_blocks, &problem)); - double ceres_covariance[3 * 3]; - covariance_computer.GetCovarianceBlock(ceres_pose_estimate, - ceres_pose_estimate, ceres_covariance); - *covariance = Eigen::Map(ceres_covariance); - *covariance *= options_.covariance_scale(); } } // namespace scan_matching diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h index 3abd49f..955121f 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h +++ b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h @@ -22,7 +22,6 @@ #include "Eigen/Core" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.pb.h" #include "cartographer/sensor/point_cloud.h" @@ -45,14 +44,13 @@ class CeresScanMatcher { CeresScanMatcher& operator=(const CeresScanMatcher&) = delete; // Aligns 'point_cloud' within the 'probability_grid' given an - // 'initial_pose_estimate' and returns 'pose_estimate', 'covariance', and - // the solver 'summary'. + // 'initial_pose_estimate' and returns a 'pose_estimate' and the solver + // 'summary'. void Match(const transform::Rigid2d& previous_pose, const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud& point_cloud, const ProbabilityGrid& probability_grid, transform::Rigid2d* pose_estimate, - kalman_filter::Pose2DCovariance* covariance, ceres::Solver::Summary* summary) const; private: diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc index 3cde0c4..495ea4f 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc @@ -47,7 +47,6 @@ class CeresScanMatcherTest : public ::testing::Test { occupied_space_weight = 1., translation_weight = 0.1, rotation_weight = 1.5, - covariance_scale = 10., ceres_solver_options = { use_nonmonotonic_steps = true, max_num_iterations = 50, @@ -61,12 +60,11 @@ class CeresScanMatcherTest : public ::testing::Test { void TestFromInitialPose(const transform::Rigid2d& initial_pose) { transform::Rigid2d pose; - kalman_filter::Pose2DCovariance covariance; const transform::Rigid2d expected_pose = transform::Rigid2d::Translation({-0.5, 0.5}); ceres::Solver::Summary summary; ceres_scan_matcher_->Match(initial_pose, initial_pose, point_cloud_, - probability_grid_, &pose, &covariance, &summary); + probability_grid_, &pose, &summary); EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport(); EXPECT_THAT(pose, transform::IsNearly(expected_pose, 1e-2)) << "Actual: " << transform::ToProto(pose).DebugString() diff --git a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc index a78d5a2..004a7f5 100644 --- a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc @@ -22,7 +22,6 @@ #include "Eigen/Geometry" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/make_unique.h" -#include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/range_data_inserter.h" #include "cartographer/sensor/point_cloud.h" diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 635ddf3..1da0f24 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -90,8 +90,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded( void SparsePoseGraph::AddScan( common::Time time, const transform::Rigid3d& tracking_to_pose, const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose, - const kalman_filter::Pose2DCovariance& covariance, const int trajectory_id, - const mapping::Submap* const matching_submap, + const int trajectory_id, const mapping::Submap* const matching_submap, const std::vector& insertion_submaps) { const transform::Rigid3d optimized_pose( GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose)); @@ -128,7 +127,7 @@ void SparsePoseGraph::AddScan( AddWorkItem([=]() REQUIRES(mutex_) { ComputeConstraintsForScan(matching_submap, insertion_submaps, - finished_submap, pose, covariance); + finished_submap, pose); }); } @@ -207,8 +206,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans( void SparsePoseGraph::ComputeConstraintsForScan( const mapping::Submap* matching_submap, std::vector insertion_submaps, - const mapping::Submap* finished_submap, const transform::Rigid2d& pose, - const kalman_filter::Pose2DCovariance& covariance) { + const mapping::Submap* finished_submap, const transform::Rigid2d& pose) { GrowSubmapTransformsAsNeeded(insertion_submaps); const mapping::SubmapId matching_id = GetSubmapId(matching_submap); const transform::Rigid2d optimized_pose = @@ -234,21 +232,16 @@ 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::Rigid2d constraint_transform = sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose; - constexpr double kFakePositionCovariance = 1e-6; - constexpr double kFakeOrientationCovariance = 1e-6; - constraints_.push_back(Constraint{ - submap_id, - node_id, - {transform::Embed3D(constraint_transform), - common::ComputeSpdMatrixSqrtInverse( - kalman_filter::Embed3D(covariance, kFakePositionCovariance, - kFakeOrientationCovariance), - options_.constraint_builder_options() - .lower_covariance_eigenvalue_bound())}, - Constraint::INTRA_SUBMAP}); + constraints_.push_back( + Constraint{submap_id, + node_id, + {transform::Embed3D(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_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index dbdddc0..a6e9b38 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/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_2d/sparse_pose_graph/constraint_builder.h" @@ -68,9 +67,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // into the 'insertion_submaps'. void AddScan(common::Time time, const transform::Rigid3d& tracking_to_pose, const sensor::RangeData& range_data_in_pose, - const transform::Rigid2d& pose, - const kalman_filter::Pose2DCovariance& pose_covariance, - int trajectory_id, const mapping::Submap* matching_submap, + const transform::Rigid2d& pose, int trajectory_id, + const mapping::Submap* matching_submap, const std::vector& insertion_submaps) EXCLUDES(mutex_); @@ -125,8 +123,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { void ComputeConstraintsForScan( 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_); + const mapping::Submap* finished_submap, const transform::Rigid2d& pose) + REQUIRES(mutex_); // Computes constraints for a scan and submap pair. void ComputeConstraint(const mapping::NodeId& node_id, diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index d602b91..aa5547f 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -29,7 +29,6 @@ #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" #include "cartographer/common/thread_pool.h" -#include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.pb.h" #include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" #include "cartographer/transform/transform.h" @@ -212,25 +211,20 @@ void ConstraintBuilder::ComputeConstraint( // effect that, in the absence of better information, we prefer the original // CSM estimate. ceres::Solver::Summary unused_summary; - kalman_filter::Pose2DCovariance covariance; ceres_scan_matcher_.Match(pose_estimate, pose_estimate, filtered_point_cloud, *submap_scan_matcher->probability_grid, - &pose_estimate, &covariance, &unused_summary); - // 'covariance' is unchanged as (submap <- map) is a translation. + &pose_estimate, &unused_summary); const transform::Rigid2d constraint_transform = ComputeSubmapPose(*submap).inverse() * pose_estimate; - constexpr double kFakePositionCovariance = 1e-6; - constexpr double kFakeOrientationCovariance = 1e-6; - constraint->reset(new Constraint{ - submap_id, - node_id, - {transform::Embed3D(constraint_transform), - common::ComputeSpdMatrixSqrtInverse( - kalman_filter::Embed3D(covariance, kFakePositionCovariance, - kFakeOrientationCovariance), - options_.lower_covariance_eigenvalue_bound())}, - Constraint::INTER_SUBMAP}); + constraint->reset( + new Constraint{submap_id, + node_id, + {transform::Embed3D(constraint_transform), + mapping::FromTranslationRotationWeights( + options_.loop_closure_translation_weight(), + options_.loop_closure_rotation_weight())}, + Constraint::INTER_SUBMAP}); if (options_.log_matches()) { std::ostringstream info; @@ -245,9 +239,7 @@ void ConstraintBuilder::ComputeConstraint( << difference.translation().norm() << " rotation " << std::setprecision(3) << std::abs(difference.normalized_angle()); } - info << " with score " << std::setprecision(1) << 100. * score - << "% covariance trace " << std::scientific << std::setprecision(4) - << covariance.trace() << "."; + info << " with score " << std::setprecision(1) << 100. * score << "%."; LOG(INFO) << info.str(); } } diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index 77bc740..f3efff5 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -79,6 +79,8 @@ 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, fast_correlative_scan_matcher = { linear_search_window = 3., @@ -89,7 +91,6 @@ class SparsePoseGraphTest : public ::testing::Test { occupied_space_weight = 20., translation_weight = 10., rotation_weight = 1., - covariance_scale = 1., ceres_solver_options = { use_nonmonotonic_steps = true, max_num_iterations = 50, @@ -117,6 +118,8 @@ class SparsePoseGraphTest : public ::testing::Test { }, }, }, + matcher_translation_weight = 1., + matcher_rotation_weight = 1., optimization_problem = { acceleration_weight = 1., rotation_weight = 1e2, @@ -147,8 +150,6 @@ class SparsePoseGraphTest : public ::testing::Test { const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud( point_cloud_, transform::Embed3D(current_pose_.inverse().cast())); - kalman_filter::Pose2DCovariance covariance = - kalman_filter::Pose2DCovariance::Identity(); const mapping::Submap* const matching_submap = submaps_->Get(submaps_->matching_index()); std::vector insertion_submaps; @@ -161,10 +162,9 @@ class SparsePoseGraphTest : public ::testing::Test { constexpr int kTrajectoryId = 0; submaps_->InsertRangeData(TransformRangeData( range_data, transform::Embed3D(pose_estimate.cast()))); - sparse_pose_graph_->AddScan(common::FromUniversal(0), - transform::Rigid3d::Identity(), range_data, - pose_estimate, covariance, kTrajectoryId, - matching_submap, insertion_submaps); + sparse_pose_graph_->AddScan( + common::FromUniversal(0), transform::Rigid3d::Identity(), range_data, + pose_estimate, kTrajectoryId, matching_submap, insertion_submaps); } void MoveRelative(const transform::Rigid2d& movement) { diff --git a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h index 15724c8..2ee349f 100644 --- a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h +++ b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h @@ -46,8 +46,8 @@ class CeresScanMatcher { CeresScanMatcher& operator=(const CeresScanMatcher&) = delete; // Aligns 'point_clouds' within the 'hybrid_grids' given an - // 'initial_pose_estimate' and returns 'pose_estimate', and - // the solver 'summary'. + // 'initial_pose_estimate' and returns a 'pose_estimate' and the solver + // 'summary'. void Match(const transform::Rigid3d& previous_pose, const transform::Rigid3d& initial_pose_estimate, const std::vector& diff --git a/configuration_files/sparse_pose_graph.lua b/configuration_files/sparse_pose_graph.lua index 7dcb784..8a6e2db 100644 --- a/configuration_files/sparse_pose_graph.lua +++ b/configuration_files/sparse_pose_graph.lua @@ -25,6 +25,8 @@ 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, fast_correlative_scan_matcher = { linear_search_window = 7., @@ -35,7 +37,6 @@ SPARSE_POSE_GRAPH = { occupied_space_weight = 20., translation_weight = 10., rotation_weight = 1., - covariance_scale = 1e-4, ceres_solver_options = { use_nonmonotonic_steps = true, max_num_iterations = 10, @@ -63,6 +64,8 @@ SPARSE_POSE_GRAPH = { }, }, }, + matcher_translation_weight = 5e2, + matcher_rotation_weight = 1.6e3, optimization_problem = { huber_scale = 1e1, acceleration_weight = 1e4, diff --git a/configuration_files/trajectory_builder_2d.lua b/configuration_files/trajectory_builder_2d.lua index a650c77..f491536 100644 --- a/configuration_files/trajectory_builder_2d.lua +++ b/configuration_files/trajectory_builder_2d.lua @@ -39,7 +39,6 @@ TRAJECTORY_BUILDER_2D = { occupied_space_weight = 1e1, translation_weight = 1e1, rotation_weight = 1e2, - covariance_scale = 1e-2, ceres_solver_options = { use_nonmonotonic_steps = false, max_num_iterations = 20,