From 3e64a803a436e8c24dc5f28c123328ffffcb7557 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 28 Oct 2016 16:39:45 +0200 Subject: [PATCH] Consistently name weight parameter. (#116) All parameters scaling optimization residuals to determine relative importance are now consistently named. --- .../optimization_problem_options.cc | 7 ++-- .../proto/optimization_problem_options.proto | 4 +- .../scan_matching/ceres_scan_matcher.cc | 35 ++++++++--------- .../scan_matching/ceres_scan_matcher_test.cc | 6 +-- .../proto/ceres_scan_matcher_options.proto | 6 +-- .../mapping_2d/sparse_pose_graph_test.cc | 16 ++++---- .../kalman_local_trajectory_builder_test.cc | 22 +++++------ .../optimizing_local_trajectory_builder.cc | 14 +++---- ...mizing_local_trajectory_builder_options.cc | 26 +++++++------ ...ing_local_trajectory_builder_options.proto | 14 +++---- .../scan_matching/ceres_scan_matcher.cc | 38 ++++++++----------- .../scan_matching/ceres_scan_matcher_test.cc | 6 +-- .../proto/ceres_scan_matcher_options.proto | 6 +-- .../sparse_pose_graph/optimization_problem.cc | 4 +- .../optimization_problem_test.cc | 4 +- configuration_files/sparse_pose_graph.lua | 16 ++++---- configuration_files/trajectory_builder_2d.lua | 6 +-- configuration_files/trajectory_builder_3d.lua | 22 +++++------ 18 files changed, 122 insertions(+), 130 deletions(-) diff --git a/cartographer/mapping/sparse_pose_graph/optimization_problem_options.cc b/cartographer/mapping/sparse_pose_graph/optimization_problem_options.cc index 4220b13..8d1b335 100644 --- a/cartographer/mapping/sparse_pose_graph/optimization_problem_options.cc +++ b/cartographer/mapping/sparse_pose_graph/optimization_problem_options.cc @@ -26,9 +26,10 @@ proto::OptimizationProblemOptions CreateOptimizationProblemOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::OptimizationProblemOptions options; options.set_huber_scale(parameter_dictionary->GetDouble("huber_scale")); - options.set_acceleration_scale( - parameter_dictionary->GetDouble("acceleration_scale")); - options.set_rotation_scale(parameter_dictionary->GetDouble("rotation_scale")); + options.set_acceleration_weight( + parameter_dictionary->GetDouble("acceleration_weight")); + options.set_rotation_weight( + parameter_dictionary->GetDouble("rotation_weight")); options.set_consecutive_scan_translation_penalty_factor( parameter_dictionary->GetDouble( "consecutive_scan_translation_penalty_factor")); 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 20d1d79..2b10f5e 100644 --- a/cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.proto +++ b/cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.proto @@ -24,10 +24,10 @@ message OptimizationProblemOptions { optional double huber_scale = 1; // Scaling parameter for the IMU acceleration term. - optional double acceleration_scale = 8; + optional double acceleration_weight = 8; // Scaling parameter for the IMU rotation term. - optional double rotation_scale = 9; + optional double rotation_weight = 9; // Penalty factors for changes to the relative pose between consecutive scans. optional double consecutive_scan_translation_penalty_factor = 2; diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc index 82f5c2c..ae61c52 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc +++ b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc @@ -38,14 +38,12 @@ namespace scan_matching { proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::CeresScanMatcherOptions options; - options.set_occupied_space_cost_functor_weight( - parameter_dictionary->GetDouble("occupied_space_cost_functor_weight")); - options.set_previous_pose_translation_delta_cost_functor_weight( - parameter_dictionary->GetDouble( - "previous_pose_translation_delta_cost_functor_weight")); - options.set_initial_pose_estimate_rotation_delta_cost_functor_weight( - parameter_dictionary->GetDouble( - "initial_pose_estimate_rotation_delta_cost_functor_weight")); + options.set_occupied_space_weight( + parameter_dictionary->GetDouble("occupied_space_weight")); + options.set_translation_weight( + 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() = @@ -75,30 +73,27 @@ void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose, initial_pose_estimate.translation().y(), initial_pose_estimate.rotation().angle()}; ceres::Problem problem; - CHECK_GT(options_.occupied_space_cost_functor_weight(), 0.); + CHECK_GT(options_.occupied_space_weight(), 0.); problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new OccupiedSpaceCostFunctor( - options_.occupied_space_cost_functor_weight() / + options_.occupied_space_weight() / std::sqrt(static_cast(point_cloud.size())), point_cloud, probability_grid), point_cloud.size()), nullptr, ceres_pose_estimate); - CHECK_GT(options_.previous_pose_translation_delta_cost_functor_weight(), 0.); + CHECK_GT(options_.translation_weight(), 0.); problem.AddResidualBlock( new ceres::AutoDiffCostFunction( - new TranslationDeltaCostFunctor( - options_.previous_pose_translation_delta_cost_functor_weight(), - previous_pose)), + new TranslationDeltaCostFunctor(options_.translation_weight(), + previous_pose)), nullptr, ceres_pose_estimate); - CHECK_GT(options_.initial_pose_estimate_rotation_delta_cost_functor_weight(), - 0.); + CHECK_GT(options_.rotation_weight(), 0.); problem.AddResidualBlock( - new ceres::AutoDiffCostFunction(new RotationDeltaCostFunctor( - options_.initial_pose_estimate_rotation_delta_cost_functor_weight(), - ceres_pose_estimate[2])), + new ceres::AutoDiffCostFunction( + new RotationDeltaCostFunctor(options_.rotation_weight(), + ceres_pose_estimate[2])), nullptr, ceres_pose_estimate); ceres::Solve(ceres_solver_options_, &problem, summary); 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 a2098e7..3cde0c4 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc @@ -44,9 +44,9 @@ class CeresScanMatcherTest : public ::testing::Test { auto parameter_dictionary = common::MakeDictionary(R"text( return { - occupied_space_cost_functor_weight = 1., - previous_pose_translation_delta_cost_functor_weight = 0.1, - initial_pose_estimate_rotation_delta_cost_functor_weight = 1.5, + occupied_space_weight = 1., + translation_weight = 0.1, + rotation_weight = 1.5, covariance_scale = 10., ceres_solver_options = { use_nonmonotonic_steps = true, diff --git a/cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto b/cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto index cdda1d4..f10e7b2 100644 --- a/cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto +++ b/cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto @@ -21,9 +21,9 @@ import "cartographer/common/proto/ceres_solver_options.proto"; // NEXT ID: 10 message CeresScanMatcherOptions { // Scaling parameters for each cost functor. - optional double occupied_space_cost_functor_weight = 1; - optional double previous_pose_translation_delta_cost_functor_weight = 2; - optional double initial_pose_estimate_rotation_delta_cost_functor_weight = 3; + optional double occupied_space_weight = 1; + optional double translation_weight = 2; + optional double rotation_weight = 3; // Scale applied to the covariance estimate from Ceres. optional double covariance_scale = 4; diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index c5fdda2..3cb77c3 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -86,9 +86,9 @@ class SparsePoseGraphTest : public ::testing::Test { branch_and_bound_depth = 3, }, ceres_scan_matcher = { - occupied_space_cost_functor_weight = 20., - previous_pose_translation_delta_cost_functor_weight = 10., - initial_pose_estimate_rotation_delta_cost_functor_weight = 1., + occupied_space_weight = 20., + translation_weight = 10., + rotation_weight = 1., covariance_scale = 1., ceres_solver_options = { use_nonmonotonic_steps = true, @@ -106,9 +106,9 @@ class SparsePoseGraphTest : public ::testing::Test { angular_search_window = 0.1, }, ceres_scan_matcher_3d = { - occupied_space_cost_functor_weight_0 = 20., - previous_pose_translation_delta_cost_functor_weight = 10., - initial_pose_estimate_rotation_delta_cost_functor_weight = 1., + occupied_space_weight_0 = 20., + translation_weight = 10., + rotation_weight = 1., covariance_scale = 1., only_optimize_yaw = true, ceres_solver_options = { @@ -119,8 +119,8 @@ class SparsePoseGraphTest : public ::testing::Test { }, }, optimization_problem = { - acceleration_scale = 1., - rotation_scale = 1e2, + acceleration_weight = 1., + rotation_weight = 1e2, huber_scale = 1., consecutive_scan_translation_penalty_factor = 0., consecutive_scan_rotation_penalty_factor = 0., diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc index 2070501..41da9d7 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc @@ -65,10 +65,10 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test { }, ceres_scan_matcher = { - occupied_space_cost_functor_weight_0 = 5., - occupied_space_cost_functor_weight_1 = 20., - previous_pose_translation_delta_cost_functor_weight = 0.1, - initial_pose_estimate_rotation_delta_cost_functor_weight = 0.3, + occupied_space_weight_0 = 5., + occupied_space_weight_1 = 20., + translation_weight = 0.1, + rotation_weight = 0.3, covariance_scale = 1e-1, only_optimize_yaw = false, ceres_solver_options = { @@ -113,13 +113,13 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test { }, }, optimizing_local_trajectory_builder = { - high_resolution_grid_scale = 5., - low_resolution_grid_scale = 15., - velocity_scale = 4e1, - translation_scale = 1e2, - rotation_scale = 1e3, - odometry_translation_scale = 1e4, - odometry_rotation_scale = 1e4, + high_resolution_grid_weight = 5., + low_resolution_grid_weight = 15., + velocity_weight = 4e1, + translation_weight = 1e2, + rotation_weight = 1e3, + odometry_translation_weight = 1e4, + odometry_rotation_weight = 1e4, }, } )text"); diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc index 6e94ce1..d0506ad 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc @@ -229,7 +229,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) { ceres::DYNAMIC, 3, 4>( new scan_matching::OccupiedSpaceCostFunctor( options_.optimizing_local_trajectory_builder_options() - .high_resolution_grid_scale() / + .high_resolution_grid_weight() / std::sqrt(static_cast( batch.high_resolution_filtered_points.size())), batch.high_resolution_filtered_points, @@ -241,7 +241,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) { ceres::DYNAMIC, 3, 4>( new scan_matching::OccupiedSpaceCostFunctor( options_.optimizing_local_trajectory_builder_options() - .low_resolution_grid_scale() / + .low_resolution_grid_weight() / std::sqrt(static_cast( batch.low_resolution_filtered_points.size())), batch.low_resolution_filtered_points, @@ -266,7 +266,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) { new ceres::AutoDiffCostFunction( new VelocityDeltaCostFunctor( options_.optimizing_local_trajectory_builder_options() - .velocity_scale())), + .velocity_weight())), nullptr, batches_[i - 1].state.velocity.data(), batches_[i].state.velocity.data()); @@ -274,7 +274,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) { new ceres::AutoDiffCostFunction( new TranslationCostFunction( options_.optimizing_local_trajectory_builder_options() - .translation_scale(), + .translation_weight(), common::ToSeconds(batches_[i].time - batches_[i - 1].time))), nullptr, batches_[i - 1].state.translation.data(), batches_[i].state.translation.data(), @@ -286,7 +286,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) { new ceres::AutoDiffCostFunction( new RotationCostFunction( options_.optimizing_local_trajectory_builder_options() - .rotation_scale(), + .rotation_weight(), result.delta_rotation)), nullptr, batches_[i - 1].state.rotation.data(), batches_[i].state.rotation.data()); @@ -315,9 +315,9 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) { 4, 3, 4, 3, 4>( new RelativeTranslationAndYawCostFunction( options_.optimizing_local_trajectory_builder_options() - .odometry_translation_scale(), + .odometry_translation_weight(), options_.optimizing_local_trajectory_builder_options() - .odometry_rotation_scale(), + .odometry_rotation_weight(), delta_pose)), nullptr, batches_[i - 1].state.translation.data(), batches_[i - 1].state.rotation.data(), diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder_options.cc b/cartographer/mapping_3d/optimizing_local_trajectory_builder_options.cc index 0390d8e..7ba7507 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder_options.cc +++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder_options.cc @@ -23,18 +23,20 @@ proto::OptimizingLocalTrajectoryBuilderOptions CreateOptimizingLocalTrajectoryBuilderOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::OptimizingLocalTrajectoryBuilderOptions options; - options.set_high_resolution_grid_scale( - parameter_dictionary->GetDouble("high_resolution_grid_scale")); - options.set_low_resolution_grid_scale( - parameter_dictionary->GetDouble("low_resolution_grid_scale")); - options.set_velocity_scale(parameter_dictionary->GetDouble("velocity_scale")); - options.set_translation_scale( - parameter_dictionary->GetDouble("translation_scale")); - options.set_rotation_scale(parameter_dictionary->GetDouble("rotation_scale")); - options.set_odometry_translation_scale( - parameter_dictionary->GetDouble("odometry_translation_scale")); - options.set_odometry_rotation_scale( - parameter_dictionary->GetDouble("odometry_rotation_scale")); + options.set_high_resolution_grid_weight( + parameter_dictionary->GetDouble("high_resolution_grid_weight")); + options.set_low_resolution_grid_weight( + parameter_dictionary->GetDouble("low_resolution_grid_weight")); + options.set_velocity_weight( + parameter_dictionary->GetDouble("velocity_weight")); + options.set_translation_weight( + parameter_dictionary->GetDouble("translation_weight")); + options.set_rotation_weight( + parameter_dictionary->GetDouble("rotation_weight")); + options.set_odometry_translation_weight( + parameter_dictionary->GetDouble("odometry_translation_weight")); + options.set_odometry_rotation_weight( + parameter_dictionary->GetDouble("odometry_rotation_weight")); return options; } diff --git a/cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.proto b/cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.proto index ba5cded..209f6a2 100644 --- a/cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.proto +++ b/cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.proto @@ -17,11 +17,11 @@ syntax = "proto2"; package cartographer.mapping_3d.proto; message OptimizingLocalTrajectoryBuilderOptions { - optional double high_resolution_grid_scale = 6; - optional double low_resolution_grid_scale = 7; - optional double velocity_scale = 1; - optional double translation_scale = 2; - optional double rotation_scale = 3; - optional double odometry_translation_scale = 4; - optional double odometry_rotation_scale = 5; + optional double high_resolution_grid_weight = 6; + optional double low_resolution_grid_weight = 7; + optional double velocity_weight = 1; + optional double translation_weight = 2; + optional double rotation_weight = 3; + optional double odometry_translation_weight = 4; + optional double odometry_rotation_weight = 5; } diff --git a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc index 78e246f..10148d4 100644 --- a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc @@ -56,20 +56,17 @@ proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::CeresScanMatcherOptions options; for (int i = 0;; ++i) { - const string lua_identifier = - "occupied_space_cost_functor_weight_" + std::to_string(i); + const string lua_identifier = "occupied_space_weight_" + std::to_string(i); if (!parameter_dictionary->HasKey(lua_identifier)) { break; } - options.add_occupied_space_cost_functor_weight( + options.add_occupied_space_weight( parameter_dictionary->GetDouble(lua_identifier)); } - options.set_previous_pose_translation_delta_cost_functor_weight( - parameter_dictionary->GetDouble( - "previous_pose_translation_delta_cost_functor_weight")); - options.set_initial_pose_estimate_rotation_delta_cost_functor_weight( - parameter_dictionary->GetDouble( - "initial_pose_estimate_rotation_delta_cost_functor_weight")); + options.set_translation_weight( + parameter_dictionary->GetDouble("translation_weight")); + options.set_rotation_weight( + parameter_dictionary->GetDouble("rotation_weight")); options.set_covariance_scale( parameter_dictionary->GetDouble("covariance_scale")); options.set_only_optimize_yaw( @@ -106,10 +103,10 @@ void CeresScanMatcher::Match(const transform::Rigid3d& previous_pose, common::make_unique()), &problem); - CHECK_EQ(options_.occupied_space_cost_functor_weight_size(), + CHECK_EQ(options_.occupied_space_weight_size(), point_clouds_and_hybrid_grids.size()); for (size_t i = 0; i != point_clouds_and_hybrid_grids.size(); ++i) { - CHECK_GT(options_.occupied_space_cost_functor_weight(i), 0.); + CHECK_GT(options_.occupied_space_weight(i), 0.); const sensor::PointCloud& point_cloud = *point_clouds_and_hybrid_grids[i].first; const HybridGrid& hybrid_grid = *point_clouds_and_hybrid_grids[i].second; @@ -117,26 +114,23 @@ void CeresScanMatcher::Match(const transform::Rigid3d& previous_pose, new ceres::AutoDiffCostFunction( new OccupiedSpaceCostFunctor( - options_.occupied_space_cost_functor_weight(i) / + options_.occupied_space_weight(i) / std::sqrt(static_cast(point_cloud.size())), point_cloud, hybrid_grid), point_cloud.size()), nullptr, ceres_pose.translation(), ceres_pose.rotation()); } - CHECK_GT(options_.previous_pose_translation_delta_cost_functor_weight(), 0.); + CHECK_GT(options_.translation_weight(), 0.); problem.AddResidualBlock( new ceres::AutoDiffCostFunction( - new TranslationDeltaCostFunctor( - options_.previous_pose_translation_delta_cost_functor_weight(), - previous_pose)), + new TranslationDeltaCostFunctor(options_.translation_weight(), + previous_pose)), nullptr, ceres_pose.translation()); - CHECK_GT(options_.initial_pose_estimate_rotation_delta_cost_functor_weight(), - 0.); + CHECK_GT(options_.rotation_weight(), 0.); problem.AddResidualBlock( - new ceres::AutoDiffCostFunction(new RotationDeltaCostFunctor( - options_.initial_pose_estimate_rotation_delta_cost_functor_weight(), - initial_pose_estimate.rotation())), + new ceres::AutoDiffCostFunction( + new RotationDeltaCostFunctor(options_.rotation_weight(), + initial_pose_estimate.rotation())), nullptr, ceres_pose.rotation()); ceres::Solve(ceres_solver_options_, &problem, summary); diff --git a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc index d2809d3..11f61d2 100644 --- a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc +++ b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc @@ -50,9 +50,9 @@ class CeresScanMatcherTest : public ::testing::Test { auto parameter_dictionary = common::MakeDictionary(R"text( return { - occupied_space_cost_functor_weight_0 = 1., - previous_pose_translation_delta_cost_functor_weight = 0.01, - initial_pose_estimate_rotation_delta_cost_functor_weight = 0.1, + occupied_space_weight_0 = 1., + translation_weight = 0.01, + rotation_weight = 0.1, covariance_scale = 10., only_optimize_yaw = false, ceres_solver_options = { diff --git a/cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto b/cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto index 3bbbd2a..1e71a6e 100644 --- a/cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto +++ b/cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto @@ -21,9 +21,9 @@ import "cartographer/common/proto/ceres_solver_options.proto"; // NEXT ID: 7 message CeresScanMatcherOptions { // Scaling parameters for each cost functor. - repeated double occupied_space_cost_functor_weight = 1; - optional double previous_pose_translation_delta_cost_functor_weight = 2; - optional double initial_pose_estimate_rotation_delta_cost_functor_weight = 3; + repeated double occupied_space_weight = 1; + optional double translation_weight = 2; + optional double rotation_weight = 3; // Scale applied to the covariance estimate from Ceres. optional double covariance_scale = 4; diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index 48a7eaf..99b34b9 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -187,7 +187,7 @@ void OptimizationProblem::Solve( problem.AddResidualBlock( new ceres::AutoDiffCostFunction(new AccelerationCostFunction( - options_.acceleration_scale(), delta_velocity, + options_.acceleration_weight(), delta_velocity, common::ToSeconds(first_delta_time), common::ToSeconds(second_delta_time))), nullptr, C_point_clouds[j].rotation(), @@ -196,7 +196,7 @@ void OptimizationProblem::Solve( } problem.AddResidualBlock( new ceres::AutoDiffCostFunction( - new RotationCostFunction(options_.rotation_scale(), + new RotationCostFunction(options_.rotation_weight(), result.delta_rotation)), nullptr, C_point_clouds[j - 1].rotation(), C_point_clouds[j].rotation()); 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 022e416..0c3e843 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc @@ -40,8 +40,8 @@ class OptimizationProblemTest : public ::testing::Test { CreateOptions() { auto parameter_dictionary = common::MakeDictionary(R"text( return { - acceleration_scale = 1e-4, - rotation_scale = 1e-2, + acceleration_weight = 1e-4, + rotation_weight = 1e-2, huber_scale = 1., consecutive_scan_translation_penalty_factor = 1e-2, consecutive_scan_rotation_penalty_factor = 1e-2, diff --git a/configuration_files/sparse_pose_graph.lua b/configuration_files/sparse_pose_graph.lua index 4b2b805..16d4269 100644 --- a/configuration_files/sparse_pose_graph.lua +++ b/configuration_files/sparse_pose_graph.lua @@ -32,9 +32,9 @@ SPARSE_POSE_GRAPH = { branch_and_bound_depth = 7, }, ceres_scan_matcher = { - occupied_space_cost_functor_weight = 20., - previous_pose_translation_delta_cost_functor_weight = 10., - initial_pose_estimate_rotation_delta_cost_functor_weight = 1., + occupied_space_weight = 20., + translation_weight = 10., + rotation_weight = 1., covariance_scale = 1e-4, ceres_solver_options = { use_nonmonotonic_steps = true, @@ -52,9 +52,9 @@ SPARSE_POSE_GRAPH = { angular_search_window = math.rad(15.), }, ceres_scan_matcher_3d = { - occupied_space_cost_functor_weight_0 = 20., - previous_pose_translation_delta_cost_functor_weight = 10., - initial_pose_estimate_rotation_delta_cost_functor_weight = 1., + occupied_space_weight_0 = 20., + translation_weight = 10., + rotation_weight = 1., covariance_scale = 1e-6, only_optimize_yaw = false, ceres_solver_options = { @@ -66,8 +66,8 @@ SPARSE_POSE_GRAPH = { }, optimization_problem = { huber_scale = 1e1, - acceleration_scale = 7e4, - rotation_scale = 3e6, + acceleration_weight = 7e4, + rotation_weight = 3e6, consecutive_scan_translation_penalty_factor = 1e5, consecutive_scan_rotation_penalty_factor = 1e5, log_solver_summary = false, diff --git a/configuration_files/trajectory_builder_2d.lua b/configuration_files/trajectory_builder_2d.lua index 43d6a74..bc5bcf4 100644 --- a/configuration_files/trajectory_builder_2d.lua +++ b/configuration_files/trajectory_builder_2d.lua @@ -33,9 +33,9 @@ TRAJECTORY_BUILDER_2D = { }, ceres_scan_matcher = { - occupied_space_cost_functor_weight = 20., - previous_pose_translation_delta_cost_functor_weight = 1., - initial_pose_estimate_rotation_delta_cost_functor_weight = 1e2, + occupied_space_weight = 20., + translation_weight = 1., + rotation_weight = 1e2, covariance_scale = 2.34e-4, ceres_solver_options = { use_nonmonotonic_steps = true, diff --git a/configuration_files/trajectory_builder_3d.lua b/configuration_files/trajectory_builder_3d.lua index 8402d86..d5df44d 100644 --- a/configuration_files/trajectory_builder_3d.lua +++ b/configuration_files/trajectory_builder_3d.lua @@ -33,10 +33,10 @@ TRAJECTORY_BUILDER_3D = { }, ceres_scan_matcher = { - occupied_space_cost_functor_weight_0 = 5., - occupied_space_cost_functor_weight_1 = 30., - previous_pose_translation_delta_cost_functor_weight = 0.3, - initial_pose_estimate_rotation_delta_cost_functor_weight = 2e3, + occupied_space_weight_0 = 5., + occupied_space_weight_1 = 30., + translation_weight = 0.3, + rotation_weight = 2e3, covariance_scale = 2.34e-4, only_optimize_yaw = false, ceres_solver_options = { @@ -86,12 +86,12 @@ TRAJECTORY_BUILDER_3D = { }, optimizing_local_trajectory_builder = { - high_resolution_grid_scale = 5., - low_resolution_grid_scale = 15., - velocity_scale = 4e1, - translation_scale = 1e2, - rotation_scale = 1e3, - odometry_translation_scale = 1e4, - odometry_rotation_scale = 1e4, + high_resolution_grid_weight = 5., + low_resolution_grid_weight = 15., + velocity_weight = 4e1, + translation_weight = 1e2, + rotation_weight = 1e3, + odometry_translation_weight = 1e4, + odometry_rotation_weight = 1e4, }, }