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 <whess@lyft.com>
cartographer-project · Oct 7, 2020 · 1b31c01 · 1b31c01
1 parent af00de3
commit 1b31c01
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 11 deletions.
diff --git 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<double> 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
@@ -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<OptimizationProblem3D::Constraint> constraints;

diff --git 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,