From 1b31c017c4d262934609d6aeb1616c4f709a07a3 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 7 Oct 2020 10:49:08 +0200 Subject: [PATCH] Use adaptive IMU weights in the PGO. (#1755) This weights IMU based on the time between nodes in the pose graph optimization. When moving slowly or stopping, IMU weights are reduced. This improves quality in these cases. The parameters are changed to approximately get the same behavior while moving as before for the examples. Signed-off-by: Wolfgang Hess --- .../optimization/optimization_problem_3d.cc | 14 ++++++++------ .../optimization/optimization_problem_3d_test.cc | 6 +++--- configuration_files/pose_graph.lua | 4 ++-- 3 files changed, 13 insertions(+), 11 deletions(-) diff --git a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc index 83d1777..f5ca1a1 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc @@ -395,14 +395,14 @@ void OptimizationProblem3D::Solve( const IntegrateImuResult result = IntegrateImu( imu_data, first_node_data.time, second_node_data.time, &imu_it); const auto next_node_it = std::next(node_it); + const common::Time first_time = first_node_data.time; + const common::Time second_time = second_node_data.time; + const common::Duration first_duration = second_time - first_time; if (next_node_it != trajectory_end && next_node_it->id.node_index == second_node_id.node_index + 1) { const NodeId third_node_id = next_node_it->id; const NodeSpec3D& third_node_data = next_node_it->data; - const common::Time first_time = first_node_data.time; - const common::Time second_time = second_node_data.time; const common::Time third_time = third_node_data.time; - const common::Duration first_duration = second_time - first_time; const common::Duration second_duration = third_time - second_time; const common::Time first_center = first_time + first_duration / 2; const common::Time second_center = second_time + second_duration / 2; @@ -421,8 +421,9 @@ void OptimizationProblem3D::Solve( result_center_to_center.delta_velocity; problem.AddResidualBlock( AccelerationCostFunction3D::CreateAutoDiffCostFunction( - options_.acceleration_weight(), delta_velocity, - common::ToSeconds(first_duration), + options_.acceleration_weight() / + common::ToSeconds(first_duration + second_duration), + delta_velocity, common::ToSeconds(first_duration), common::ToSeconds(second_duration)), nullptr /* loss function */, C_nodes.at(second_node_id).rotation(), @@ -434,7 +435,8 @@ void OptimizationProblem3D::Solve( } problem.AddResidualBlock( RotationCostFunction3D::CreateAutoDiffCostFunction( - options_.rotation_weight(), result.delta_rotation), + options_.rotation_weight() / common::ToSeconds(first_duration), + result.delta_rotation), nullptr /* loss function */, C_nodes.at(first_node_id).rotation(), C_nodes.at(second_node_id).rotation(), trajectory_data.imu_calibration.data()); diff --git a/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc index 05bfac5..92abbc6 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc @@ -39,8 +39,8 @@ class OptimizationProblem3DTest : public ::testing::Test { optimization::proto::OptimizationProblemOptions CreateOptions() { auto parameter_dictionary = common::MakeDictionary(R"text( return { - acceleration_weight = 1e-4, - rotation_weight = 1e-2, + acceleration_weight = 2e-5, + rotation_weight = 1e-3, huber_scale = 1., local_slam_pose_translation_weight = 1e-2, local_slam_pose_rotation_weight = 1e-2, @@ -134,7 +134,7 @@ TEST_F(OptimizationProblem3DTest, ReducesNoise) { Eigen::Vector3d::Zero()}); optimization_problem_.AddTrajectoryNode(kTrajectoryId, NodeSpec3D{now, pose, pose}); - now += common::FromSeconds(0.01); + now += common::FromSeconds(0.1); } std::vector constraints; diff --git a/configuration_files/pose_graph.lua b/configuration_files/pose_graph.lua index 572cd1a..a962641 100644 --- a/configuration_files/pose_graph.lua +++ b/configuration_files/pose_graph.lua @@ -63,8 +63,8 @@ POSE_GRAPH = { matcher_rotation_weight = 1.6e3, optimization_problem = { huber_scale = 1e1, - acceleration_weight = 1e3, - rotation_weight = 3e5, + acceleration_weight = 1.1e2, + rotation_weight = 1.6e4, local_slam_pose_translation_weight = 1e5, local_slam_pose_rotation_weight = 1e5, odometry_translation_weight = 1e5,