12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Consistently name weight parameter. (#116) 

All parameters scaling optimization residuals to determine relative
importance are now consistently named.
master
Wolfgang Hess 2016-10-28 16:39:45 +02:00 committed by GitHub
parent 32b5beb225
commit 3e64a803a4
18 changed files with 122 additions and 130 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
cartographer/mapping/sparse_pose_graph/optimization_problem_options.cc
View File

@ -26,9 +26,10 @@ proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
common::LuaParameterDictionary* const parameter_dictionary) { common::LuaParameterDictionary* const parameter_dictionary) {
proto::OptimizationProblemOptions options; proto::OptimizationProblemOptions options;
options.set_huber_scale(parameter_dictionary->GetDouble("huber_scale")); options.set_huber_scale(parameter_dictionary->GetDouble("huber_scale"));
options.set_acceleration_scale( options.set_acceleration_weight(
parameter_dictionary->GetDouble("acceleration_scale")); parameter_dictionary->GetDouble("acceleration_weight"));
options.set_rotation_scale(parameter_dictionary->GetDouble("rotation_scale")); options.set_rotation_weight(
parameter_dictionary->GetDouble("rotation_weight"));
options.set_consecutive_scan_translation_penalty_factor( options.set_consecutive_scan_translation_penalty_factor(
parameter_dictionary->GetDouble( parameter_dictionary->GetDouble(
"consecutive_scan_translation_penalty_factor")); "consecutive_scan_translation_penalty_factor"));

4
cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.proto
View File

@ -24,10 +24,10 @@ message OptimizationProblemOptions {
optional double huber_scale = 1; optional double huber_scale = 1;
// Scaling parameter for the IMU acceleration term. // Scaling parameter for the IMU acceleration term.
optional double acceleration_scale = 8; optional double acceleration_weight = 8;
// Scaling parameter for the IMU rotation term. // 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. // Penalty factors for changes to the relative pose between consecutive scans.
optional double consecutive_scan_translation_penalty_factor = 2; optional double consecutive_scan_translation_penalty_factor = 2;

31
cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc
View File

@ -38,14 +38,12 @@ namespace scan_matching {
proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions( proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
common::LuaParameterDictionary* const parameter_dictionary) { common::LuaParameterDictionary* const parameter_dictionary) {
proto::CeresScanMatcherOptions options; proto::CeresScanMatcherOptions options;
options.set_occupied_space_cost_functor_weight( options.set_occupied_space_weight(
parameter_dictionary->GetDouble("occupied_space_cost_functor_weight")); parameter_dictionary->GetDouble("occupied_space_weight"));
options.set_previous_pose_translation_delta_cost_functor_weight( options.set_translation_weight(
parameter_dictionary->GetDouble( parameter_dictionary->GetDouble("translation_weight"));
"previous_pose_translation_delta_cost_functor_weight")); options.set_rotation_weight(
options.set_initial_pose_estimate_rotation_delta_cost_functor_weight( parameter_dictionary->GetDouble("rotation_weight"));
parameter_dictionary->GetDouble(
"initial_pose_estimate_rotation_delta_cost_functor_weight"));
options.set_covariance_scale( options.set_covariance_scale(
parameter_dictionary->GetDouble("covariance_scale")); parameter_dictionary->GetDouble("covariance_scale"));
*options.mutable_ceres_solver_options() = *options.mutable_ceres_solver_options() =
@ -75,29 +73,26 @@ void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose,
initial_pose_estimate.translation().y(), initial_pose_estimate.translation().y(),
initial_pose_estimate.rotation().angle()}; initial_pose_estimate.rotation().angle()};
ceres::Problem problem; ceres::Problem problem;
CHECK_GT(options_.occupied_space_cost_functor_weight(), 0.); CHECK_GT(options_.occupied_space_weight(), 0.);
problem.AddResidualBlock( problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunctor, ceres::DYNAMIC, new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunctor, ceres::DYNAMIC,
3>( 3>(
new OccupiedSpaceCostFunctor( new OccupiedSpaceCostFunctor(
options_.occupied_space_cost_functor_weight() / options_.occupied_space_weight() /
std::sqrt(static_cast<double>(point_cloud.size())), std::sqrt(static_cast<double>(point_cloud.size())),
point_cloud, probability_grid), point_cloud, probability_grid),
point_cloud.size()), point_cloud.size()),
nullptr, ceres_pose_estimate); nullptr, ceres_pose_estimate);
CHECK_GT(options_.previous_pose_translation_delta_cost_functor_weight(), 0.); CHECK_GT(options_.translation_weight(), 0.);
problem.AddResidualBlock( problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor, 2, 3>( new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor, 2, 3>(
new TranslationDeltaCostFunctor( new TranslationDeltaCostFunctor(options_.translation_weight(),
options_.previous_pose_translation_delta_cost_functor_weight(),
previous_pose)), previous_pose)),
nullptr, ceres_pose_estimate); nullptr, ceres_pose_estimate);
CHECK_GT(options_.initial_pose_estimate_rotation_delta_cost_functor_weight(), CHECK_GT(options_.rotation_weight(), 0.);
0.);
problem.AddResidualBlock( problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<RotationDeltaCostFunctor, 1, new ceres::AutoDiffCostFunction<RotationDeltaCostFunctor, 1, 3>(
3>(new RotationDeltaCostFunctor( new RotationDeltaCostFunctor(options_.rotation_weight(),
options_.initial_pose_estimate_rotation_delta_cost_functor_weight(),
ceres_pose_estimate[2])), ceres_pose_estimate[2])),
nullptr, ceres_pose_estimate); nullptr, ceres_pose_estimate);

6
cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc
View File

@ -44,9 +44,9 @@ class CeresScanMatcherTest : public ::testing::Test {
auto parameter_dictionary = common::MakeDictionary(R"text( auto parameter_dictionary = common::MakeDictionary(R"text(
return { return {
occupied_space_cost_functor_weight = 1., occupied_space_weight = 1.,
previous_pose_translation_delta_cost_functor_weight = 0.1, translation_weight = 0.1,
initial_pose_estimate_rotation_delta_cost_functor_weight = 1.5, rotation_weight = 1.5,
covariance_scale = 10., covariance_scale = 10.,
ceres_solver_options = { ceres_solver_options = {
use_nonmonotonic_steps = true, use_nonmonotonic_steps = true,

6
cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto
View File

@ -21,9 +21,9 @@ import "cartographer/common/proto/ceres_solver_options.proto";
// NEXT ID: 10 // NEXT ID: 10
message CeresScanMatcherOptions { message CeresScanMatcherOptions {
// Scaling parameters for each cost functor. // Scaling parameters for each cost functor.
optional double occupied_space_cost_functor_weight = 1; optional double occupied_space_weight = 1;
optional double previous_pose_translation_delta_cost_functor_weight = 2; optional double translation_weight = 2;
optional double initial_pose_estimate_rotation_delta_cost_functor_weight = 3; optional double rotation_weight = 3;
// Scale applied to the covariance estimate from Ceres. // Scale applied to the covariance estimate from Ceres.
optional double covariance_scale = 4; optional double covariance_scale = 4;

16
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

@ -86,9 +86,9 @@ class SparsePoseGraphTest : public ::testing::Test {
branch_and_bound_depth = 3, branch_and_bound_depth = 3,
}, },
ceres_scan_matcher = { ceres_scan_matcher = {
occupied_space_cost_functor_weight = 20., occupied_space_weight = 20.,
previous_pose_translation_delta_cost_functor_weight = 10., translation_weight = 10.,
initial_pose_estimate_rotation_delta_cost_functor_weight = 1., rotation_weight = 1.,
covariance_scale = 1., covariance_scale = 1.,
ceres_solver_options = { ceres_solver_options = {
use_nonmonotonic_steps = true, use_nonmonotonic_steps = true,
@ -106,9 +106,9 @@ class SparsePoseGraphTest : public ::testing::Test {
angular_search_window = 0.1, angular_search_window = 0.1,
}, },
ceres_scan_matcher_3d = { ceres_scan_matcher_3d = {
occupied_space_cost_functor_weight_0 = 20., occupied_space_weight_0 = 20.,
previous_pose_translation_delta_cost_functor_weight = 10., translation_weight = 10.,
initial_pose_estimate_rotation_delta_cost_functor_weight = 1., rotation_weight = 1.,
covariance_scale = 1., covariance_scale = 1.,
only_optimize_yaw = true, only_optimize_yaw = true,
ceres_solver_options = { ceres_solver_options = {
@ -119,8 +119,8 @@ class SparsePoseGraphTest : public ::testing::Test {
}, },
}, },
optimization_problem = { optimization_problem = {
acceleration_scale = 1., acceleration_weight = 1.,
rotation_scale = 1e2, rotation_weight = 1e2,
huber_scale = 1., huber_scale = 1.,
consecutive_scan_translation_penalty_factor = 0., consecutive_scan_translation_penalty_factor = 0.,
consecutive_scan_rotation_penalty_factor = 0., consecutive_scan_rotation_penalty_factor = 0.,

22
cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc
View File

@ -65,10 +65,10 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
}, },
ceres_scan_matcher = { ceres_scan_matcher = {
occupied_space_cost_functor_weight_0 = 5., occupied_space_weight_0 = 5.,
occupied_space_cost_functor_weight_1 = 20., occupied_space_weight_1 = 20.,
previous_pose_translation_delta_cost_functor_weight = 0.1, translation_weight = 0.1,
initial_pose_estimate_rotation_delta_cost_functor_weight = 0.3, rotation_weight = 0.3,
covariance_scale = 1e-1, covariance_scale = 1e-1,
only_optimize_yaw = false, only_optimize_yaw = false,
ceres_solver_options = { ceres_solver_options = {
@ -113,13 +113,13 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
}, },
}, },
optimizing_local_trajectory_builder = { optimizing_local_trajectory_builder = {
high_resolution_grid_scale = 5., high_resolution_grid_weight = 5.,
low_resolution_grid_scale = 15., low_resolution_grid_weight = 15.,
velocity_scale = 4e1, velocity_weight = 4e1,
translation_scale = 1e2, translation_weight = 1e2,
rotation_scale = 1e3, rotation_weight = 1e3,
odometry_translation_scale = 1e4, odometry_translation_weight = 1e4,
odometry_rotation_scale = 1e4, odometry_rotation_weight = 1e4,
}, },
} }
)text"); )text");

14
cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
View File

@ -229,7 +229,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
ceres::DYNAMIC, 3, 4>( ceres::DYNAMIC, 3, 4>(
new scan_matching::OccupiedSpaceCostFunctor( new scan_matching::OccupiedSpaceCostFunctor(
options_.optimizing_local_trajectory_builder_options() options_.optimizing_local_trajectory_builder_options()
.high_resolution_grid_scale() / .high_resolution_grid_weight() /
std::sqrt(static_cast<double>( std::sqrt(static_cast<double>(
batch.high_resolution_filtered_points.size())), batch.high_resolution_filtered_points.size())),
batch.high_resolution_filtered_points, batch.high_resolution_filtered_points,
@ -241,7 +241,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
ceres::DYNAMIC, 3, 4>( ceres::DYNAMIC, 3, 4>(
new scan_matching::OccupiedSpaceCostFunctor( new scan_matching::OccupiedSpaceCostFunctor(
options_.optimizing_local_trajectory_builder_options() options_.optimizing_local_trajectory_builder_options()
.low_resolution_grid_scale() / .low_resolution_grid_weight() /
std::sqrt(static_cast<double>( std::sqrt(static_cast<double>(
batch.low_resolution_filtered_points.size())), batch.low_resolution_filtered_points.size())),
batch.low_resolution_filtered_points, batch.low_resolution_filtered_points,
@ -266,7 +266,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
new ceres::AutoDiffCostFunction<VelocityDeltaCostFunctor, 3, 3, 3>( new ceres::AutoDiffCostFunction<VelocityDeltaCostFunctor, 3, 3, 3>(
new VelocityDeltaCostFunctor( new VelocityDeltaCostFunctor(
options_.optimizing_local_trajectory_builder_options() options_.optimizing_local_trajectory_builder_options()
.velocity_scale())), .velocity_weight())),
nullptr, batches_[i - 1].state.velocity.data(), nullptr, batches_[i - 1].state.velocity.data(),
batches_[i].state.velocity.data()); batches_[i].state.velocity.data());
@ -274,7 +274,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
new ceres::AutoDiffCostFunction<TranslationCostFunction, 3, 3, 3, 3>( new ceres::AutoDiffCostFunction<TranslationCostFunction, 3, 3, 3, 3>(
new TranslationCostFunction( new TranslationCostFunction(
options_.optimizing_local_trajectory_builder_options() options_.optimizing_local_trajectory_builder_options()
.translation_scale(), .translation_weight(),
common::ToSeconds(batches_[i].time - batches_[i - 1].time))), common::ToSeconds(batches_[i].time - batches_[i - 1].time))),
nullptr, batches_[i - 1].state.translation.data(), nullptr, batches_[i - 1].state.translation.data(),
batches_[i].state.translation.data(), batches_[i].state.translation.data(),
@ -286,7 +286,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4>( new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4>(
new RotationCostFunction( new RotationCostFunction(
options_.optimizing_local_trajectory_builder_options() options_.optimizing_local_trajectory_builder_options()
.rotation_scale(), .rotation_weight(),
result.delta_rotation)), result.delta_rotation)),
nullptr, batches_[i - 1].state.rotation.data(), nullptr, batches_[i - 1].state.rotation.data(),
batches_[i].state.rotation.data()); batches_[i].state.rotation.data());
@ -315,9 +315,9 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
4, 3, 4, 3, 4>( 4, 3, 4, 3, 4>(
new RelativeTranslationAndYawCostFunction( new RelativeTranslationAndYawCostFunction(
options_.optimizing_local_trajectory_builder_options() options_.optimizing_local_trajectory_builder_options()
.odometry_translation_scale(), .odometry_translation_weight(),
options_.optimizing_local_trajectory_builder_options() options_.optimizing_local_trajectory_builder_options()
.odometry_rotation_scale(), .odometry_rotation_weight(),
delta_pose)), delta_pose)),
nullptr, batches_[i - 1].state.translation.data(), nullptr, batches_[i - 1].state.translation.data(),
batches_[i - 1].state.rotation.data(), batches_[i - 1].state.rotation.data(),

26
cartographer/mapping_3d/optimizing_local_trajectory_builder_options.cc
View File

@ -23,18 +23,20 @@ proto::OptimizingLocalTrajectoryBuilderOptions
CreateOptimizingLocalTrajectoryBuilderOptions( CreateOptimizingLocalTrajectoryBuilderOptions(
common::LuaParameterDictionary* const parameter_dictionary) { common::LuaParameterDictionary* const parameter_dictionary) {
proto::OptimizingLocalTrajectoryBuilderOptions options; proto::OptimizingLocalTrajectoryBuilderOptions options;
options.set_high_resolution_grid_scale( options.set_high_resolution_grid_weight(
parameter_dictionary->GetDouble("high_resolution_grid_scale")); parameter_dictionary->GetDouble("high_resolution_grid_weight"));
options.set_low_resolution_grid_scale( options.set_low_resolution_grid_weight(
parameter_dictionary->GetDouble("low_resolution_grid_scale")); parameter_dictionary->GetDouble("low_resolution_grid_weight"));
options.set_velocity_scale(parameter_dictionary->GetDouble("velocity_scale")); options.set_velocity_weight(
options.set_translation_scale( parameter_dictionary->GetDouble("velocity_weight"));
parameter_dictionary->GetDouble("translation_scale")); options.set_translation_weight(
options.set_rotation_scale(parameter_dictionary->GetDouble("rotation_scale")); parameter_dictionary->GetDouble("translation_weight"));
options.set_odometry_translation_scale( options.set_rotation_weight(
parameter_dictionary->GetDouble("odometry_translation_scale")); parameter_dictionary->GetDouble("rotation_weight"));
options.set_odometry_rotation_scale( options.set_odometry_translation_weight(
parameter_dictionary->GetDouble("odometry_rotation_scale")); parameter_dictionary->GetDouble("odometry_translation_weight"));
options.set_odometry_rotation_weight(
parameter_dictionary->GetDouble("odometry_rotation_weight"));
return options; return options;
} }

14
cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.proto
View File

@ -17,11 +17,11 @@ syntax = "proto2";
package cartographer.mapping_3d.proto; package cartographer.mapping_3d.proto;
message OptimizingLocalTrajectoryBuilderOptions { message OptimizingLocalTrajectoryBuilderOptions {
optional double high_resolution_grid_scale = 6; optional double high_resolution_grid_weight = 6;
optional double low_resolution_grid_scale = 7; optional double low_resolution_grid_weight = 7;
optional double velocity_scale = 1; optional double velocity_weight = 1;
optional double translation_scale = 2; optional double translation_weight = 2;
optional double rotation_scale = 3; optional double rotation_weight = 3;
optional double odometry_translation_scale = 4; optional double odometry_translation_weight = 4;
optional double odometry_rotation_scale = 5; optional double odometry_rotation_weight = 5;
} }

34
cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc
View File

@ -56,20 +56,17 @@ proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
common::LuaParameterDictionary* const parameter_dictionary) { common::LuaParameterDictionary* const parameter_dictionary) {
proto::CeresScanMatcherOptions options; proto::CeresScanMatcherOptions options;
for (int i = 0;; ++i) { for (int i = 0;; ++i) {
const string lua_identifier = const string lua_identifier = "occupied_space_weight_" + std::to_string(i);
"occupied_space_cost_functor_weight_" + std::to_string(i);
if (!parameter_dictionary->HasKey(lua_identifier)) { if (!parameter_dictionary->HasKey(lua_identifier)) {
break; break;
} }
options.add_occupied_space_cost_functor_weight( options.add_occupied_space_weight(
parameter_dictionary->GetDouble(lua_identifier)); parameter_dictionary->GetDouble(lua_identifier));
} }
options.set_previous_pose_translation_delta_cost_functor_weight( options.set_translation_weight(
parameter_dictionary->GetDouble( parameter_dictionary->GetDouble("translation_weight"));
"previous_pose_translation_delta_cost_functor_weight")); options.set_rotation_weight(
options.set_initial_pose_estimate_rotation_delta_cost_functor_weight( parameter_dictionary->GetDouble("rotation_weight"));
parameter_dictionary->GetDouble(
"initial_pose_estimate_rotation_delta_cost_functor_weight"));
options.set_covariance_scale( options.set_covariance_scale(
parameter_dictionary->GetDouble("covariance_scale")); parameter_dictionary->GetDouble("covariance_scale"));
options.set_only_optimize_yaw( options.set_only_optimize_yaw(
@ -106,10 +103,10 @@ void CeresScanMatcher::Match(const transform::Rigid3d& previous_pose,
common::make_unique<ceres::QuaternionParameterization>()), common::make_unique<ceres::QuaternionParameterization>()),
&problem); &problem);
CHECK_EQ(options_.occupied_space_cost_functor_weight_size(), CHECK_EQ(options_.occupied_space_weight_size(),
point_clouds_and_hybrid_grids.size()); point_clouds_and_hybrid_grids.size());
for (size_t i = 0; i != point_clouds_and_hybrid_grids.size(); ++i) { 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 = const sensor::PointCloud& point_cloud =
*point_clouds_and_hybrid_grids[i].first; *point_clouds_and_hybrid_grids[i].first;
const HybridGrid& hybrid_grid = *point_clouds_and_hybrid_grids[i].second; const HybridGrid& hybrid_grid = *point_clouds_and_hybrid_grids[i].second;
@ -117,25 +114,22 @@ void CeresScanMatcher::Match(const transform::Rigid3d& previous_pose,
new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunctor, new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunctor,
ceres::DYNAMIC, 3, 4>( ceres::DYNAMIC, 3, 4>(
new OccupiedSpaceCostFunctor( new OccupiedSpaceCostFunctor(
options_.occupied_space_cost_functor_weight(i) / options_.occupied_space_weight(i) /
std::sqrt(static_cast<double>(point_cloud.size())), std::sqrt(static_cast<double>(point_cloud.size())),
point_cloud, hybrid_grid), point_cloud, hybrid_grid),
point_cloud.size()), point_cloud.size()),
nullptr, ceres_pose.translation(), ceres_pose.rotation()); 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( problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor, 3, 3>( new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor, 3, 3>(
new TranslationDeltaCostFunctor( new TranslationDeltaCostFunctor(options_.translation_weight(),
options_.previous_pose_translation_delta_cost_functor_weight(),
previous_pose)), previous_pose)),
nullptr, ceres_pose.translation()); nullptr, ceres_pose.translation());
CHECK_GT(options_.initial_pose_estimate_rotation_delta_cost_functor_weight(), CHECK_GT(options_.rotation_weight(), 0.);
0.);
problem.AddResidualBlock( problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<RotationDeltaCostFunctor, 3, new ceres::AutoDiffCostFunction<RotationDeltaCostFunctor, 3, 4>(
4>(new RotationDeltaCostFunctor( new RotationDeltaCostFunctor(options_.rotation_weight(),
options_.initial_pose_estimate_rotation_delta_cost_functor_weight(),
initial_pose_estimate.rotation())), initial_pose_estimate.rotation())),
nullptr, ceres_pose.rotation()); nullptr, ceres_pose.rotation());

6
cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc
View File

@ -50,9 +50,9 @@ class CeresScanMatcherTest : public ::testing::Test {
auto parameter_dictionary = common::MakeDictionary(R"text( auto parameter_dictionary = common::MakeDictionary(R"text(
return { return {
occupied_space_cost_functor_weight_0 = 1., occupied_space_weight_0 = 1.,
previous_pose_translation_delta_cost_functor_weight = 0.01, translation_weight = 0.01,
initial_pose_estimate_rotation_delta_cost_functor_weight = 0.1, rotation_weight = 0.1,
covariance_scale = 10., covariance_scale = 10.,
only_optimize_yaw = false, only_optimize_yaw = false,
ceres_solver_options = { ceres_solver_options = {

6
cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto
View File

@ -21,9 +21,9 @@ import "cartographer/common/proto/ceres_solver_options.proto";
// NEXT ID: 7 // NEXT ID: 7
message CeresScanMatcherOptions { message CeresScanMatcherOptions {
// Scaling parameters for each cost functor. // Scaling parameters for each cost functor.
repeated double occupied_space_cost_functor_weight = 1; repeated double occupied_space_weight = 1;
optional double previous_pose_translation_delta_cost_functor_weight = 2; optional double translation_weight = 2;
optional double initial_pose_estimate_rotation_delta_cost_functor_weight = 3; optional double rotation_weight = 3;
// Scale applied to the covariance estimate from Ceres. // Scale applied to the covariance estimate from Ceres.
optional double covariance_scale = 4; optional double covariance_scale = 4;

4
cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
View File

@ -187,7 +187,7 @@ void OptimizationProblem::Solve(
problem.AddResidualBlock( problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<AccelerationCostFunction, 3, 4, 3, 3, new ceres::AutoDiffCostFunction<AccelerationCostFunction, 3, 4, 3, 3,
3, 1>(new AccelerationCostFunction( 3, 1>(new AccelerationCostFunction(
options_.acceleration_scale(), delta_velocity, options_.acceleration_weight(), delta_velocity,
common::ToSeconds(first_delta_time), common::ToSeconds(first_delta_time),
common::ToSeconds(second_delta_time))), common::ToSeconds(second_delta_time))),
nullptr, C_point_clouds[j].rotation(), nullptr, C_point_clouds[j].rotation(),
@ -196,7 +196,7 @@ void OptimizationProblem::Solve(
} }
problem.AddResidualBlock( problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4>( new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4>(
new RotationCostFunction(options_.rotation_scale(), new RotationCostFunction(options_.rotation_weight(),
result.delta_rotation)), result.delta_rotation)),
nullptr, C_point_clouds[j - 1].rotation(), nullptr, C_point_clouds[j - 1].rotation(),
C_point_clouds[j].rotation()); C_point_clouds[j].rotation());

4
cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc
View File

@ -40,8 +40,8 @@ class OptimizationProblemTest : public ::testing::Test {
CreateOptions() { CreateOptions() {
auto parameter_dictionary = common::MakeDictionary(R"text( auto parameter_dictionary = common::MakeDictionary(R"text(
return { return {
acceleration_scale = 1e-4, acceleration_weight = 1e-4,
rotation_scale = 1e-2, rotation_weight = 1e-2,
huber_scale = 1., huber_scale = 1.,
consecutive_scan_translation_penalty_factor = 1e-2, consecutive_scan_translation_penalty_factor = 1e-2,
consecutive_scan_rotation_penalty_factor = 1e-2, consecutive_scan_rotation_penalty_factor = 1e-2,

16
configuration_files/sparse_pose_graph.lua
View File

@ -32,9 +32,9 @@ SPARSE_POSE_GRAPH = {
branch_and_bound_depth = 7, branch_and_bound_depth = 7,
}, },
ceres_scan_matcher = { ceres_scan_matcher = {
occupied_space_cost_functor_weight = 20., occupied_space_weight = 20.,
previous_pose_translation_delta_cost_functor_weight = 10., translation_weight = 10.,
initial_pose_estimate_rotation_delta_cost_functor_weight = 1., rotation_weight = 1.,
covariance_scale = 1e-4, covariance_scale = 1e-4,
ceres_solver_options = { ceres_solver_options = {
use_nonmonotonic_steps = true, use_nonmonotonic_steps = true,
@ -52,9 +52,9 @@ SPARSE_POSE_GRAPH = {
angular_search_window = math.rad(15.), angular_search_window = math.rad(15.),
}, },
ceres_scan_matcher_3d = { ceres_scan_matcher_3d = {
occupied_space_cost_functor_weight_0 = 20., occupied_space_weight_0 = 20.,
previous_pose_translation_delta_cost_functor_weight = 10., translation_weight = 10.,
initial_pose_estimate_rotation_delta_cost_functor_weight = 1., rotation_weight = 1.,
covariance_scale = 1e-6, covariance_scale = 1e-6,
only_optimize_yaw = false, only_optimize_yaw = false,
ceres_solver_options = { ceres_solver_options = {
@ -66,8 +66,8 @@ SPARSE_POSE_GRAPH = {
}, },
optimization_problem = { optimization_problem = {
huber_scale = 1e1, huber_scale = 1e1,
acceleration_scale = 7e4, acceleration_weight = 7e4,
rotation_scale = 3e6, rotation_weight = 3e6,
consecutive_scan_translation_penalty_factor = 1e5, consecutive_scan_translation_penalty_factor = 1e5,
consecutive_scan_rotation_penalty_factor = 1e5, consecutive_scan_rotation_penalty_factor = 1e5,
log_solver_summary = false, log_solver_summary = false,

6
configuration_files/trajectory_builder_2d.lua
View File

@ -33,9 +33,9 @@ TRAJECTORY_BUILDER_2D = {
}, },
ceres_scan_matcher = { ceres_scan_matcher = {
occupied_space_cost_functor_weight = 20., occupied_space_weight = 20.,
previous_pose_translation_delta_cost_functor_weight = 1., translation_weight = 1.,
initial_pose_estimate_rotation_delta_cost_functor_weight = 1e2, rotation_weight = 1e2,
covariance_scale = 2.34e-4, covariance_scale = 2.34e-4,
ceres_solver_options = { ceres_solver_options = {
use_nonmonotonic_steps = true, use_nonmonotonic_steps = true,

22
configuration_files/trajectory_builder_3d.lua
View File

@ -33,10 +33,10 @@ TRAJECTORY_BUILDER_3D = {
}, },
ceres_scan_matcher = { ceres_scan_matcher = {
occupied_space_cost_functor_weight_0 = 5., occupied_space_weight_0 = 5.,
occupied_space_cost_functor_weight_1 = 30., occupied_space_weight_1 = 30.,
previous_pose_translation_delta_cost_functor_weight = 0.3, translation_weight = 0.3,
initial_pose_estimate_rotation_delta_cost_functor_weight = 2e3, rotation_weight = 2e3,
covariance_scale = 2.34e-4, covariance_scale = 2.34e-4,
only_optimize_yaw = false, only_optimize_yaw = false,
ceres_solver_options = { ceres_solver_options = {
@ -86,12 +86,12 @@ TRAJECTORY_BUILDER_3D = {
}, },
optimizing_local_trajectory_builder = { optimizing_local_trajectory_builder = {
high_resolution_grid_scale = 5., high_resolution_grid_weight = 5.,
low_resolution_grid_scale = 15., low_resolution_grid_weight = 15.,
velocity_scale = 4e1, velocity_weight = 4e1,
translation_scale = 1e2, translation_weight = 1e2,
rotation_scale = 1e3, rotation_weight = 1e3,
odometry_translation_scale = 1e4, odometry_translation_weight = 1e4,
odometry_rotation_scale = 1e4, odometry_rotation_weight = 1e4,
}, },
} }