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,