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

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>
master
Wolfgang Hess 2020-10-07 10:49:08 +02:00 committed by GitHub
parent af00de3b3f
commit 1b31c017c4
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 13 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
cartographer/mapping/internal/optimization/optimization_problem_3d.cc
View File

@ -395,14 +395,14 @@ void OptimizationProblem3D::Solve(
const IntegrateImuResult<double> result = IntegrateImu( const IntegrateImuResult<double> result = IntegrateImu(
imu_data, first_node_data.time, second_node_data.time, &imu_it); imu_data, first_node_data.time, second_node_data.time, &imu_it);
const auto next_node_it = std::next(node_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 && if (next_node_it != trajectory_end &&
next_node_it->id.node_index == second_node_id.node_index + 1) { next_node_it->id.node_index == second_node_id.node_index + 1) {
const NodeId third_node_id = next_node_it->id; const NodeId third_node_id = next_node_it->id;
const NodeSpec3D& third_node_data = next_node_it->data; 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::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::Duration second_duration = third_time - second_time;
const common::Time first_center = first_time + first_duration / 2; const common::Time first_center = first_time + first_duration / 2;
const common::Time second_center = second_time + second_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; result_center_to_center.delta_velocity;
problem.AddResidualBlock( problem.AddResidualBlock(
AccelerationCostFunction3D::CreateAutoDiffCostFunction( AccelerationCostFunction3D::CreateAutoDiffCostFunction(
options_.acceleration_weight(), delta_velocity, options_.acceleration_weight() /
common::ToSeconds(first_duration), common::ToSeconds(first_duration + second_duration),
delta_velocity, common::ToSeconds(first_duration),
common::ToSeconds(second_duration)), common::ToSeconds(second_duration)),
nullptr /* loss function */, nullptr /* loss function */,
C_nodes.at(second_node_id).rotation(), C_nodes.at(second_node_id).rotation(),
@ -434,7 +435,8 @@ void OptimizationProblem3D::Solve(
} }
problem.AddResidualBlock( problem.AddResidualBlock(
RotationCostFunction3D::CreateAutoDiffCostFunction( 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(), nullptr /* loss function */, C_nodes.at(first_node_id).rotation(),
C_nodes.at(second_node_id).rotation(), C_nodes.at(second_node_id).rotation(),
trajectory_data.imu_calibration.data()); trajectory_data.imu_calibration.data());

6
cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc
View File

@ -39,8 +39,8 @@ class OptimizationProblem3DTest : public ::testing::Test {
optimization::proto::OptimizationProblemOptions CreateOptions() { optimization::proto::OptimizationProblemOptions CreateOptions() {
auto parameter_dictionary = common::MakeDictionary(R"text( auto parameter_dictionary = common::MakeDictionary(R"text(
return { return {
acceleration_weight = 1e-4, acceleration_weight = 2e-5,
rotation_weight = 1e-2, rotation_weight = 1e-3,
huber_scale = 1., huber_scale = 1.,
local_slam_pose_translation_weight = 1e-2, local_slam_pose_translation_weight = 1e-2,
local_slam_pose_rotation_weight = 1e-2, local_slam_pose_rotation_weight = 1e-2,
@ -134,7 +134,7 @@ TEST_F(OptimizationProblem3DTest, ReducesNoise) {
Eigen::Vector3d::Zero()}); Eigen::Vector3d::Zero()});
optimization_problem_.AddTrajectoryNode(kTrajectoryId, optimization_problem_.AddTrajectoryNode(kTrajectoryId,
NodeSpec3D{now, pose, pose}); NodeSpec3D{now, pose, pose});
now += common::FromSeconds(0.01); now += common::FromSeconds(0.1);
} }
std::vector<OptimizationProblem3D::Constraint> constraints; std::vector<OptimizationProblem3D::Constraint> constraints;

4
configuration_files/pose_graph.lua
View File

@ -63,8 +63,8 @@ POSE_GRAPH = {
matcher_rotation_weight = 1.6e3, matcher_rotation_weight = 1.6e3,
optimization_problem = { optimization_problem = {
huber_scale = 1e1, huber_scale = 1e1,
acceleration_weight = 1e3, acceleration_weight = 1.1e2,
rotation_weight = 3e5, rotation_weight = 1.6e4,
local_slam_pose_translation_weight = 1e5, local_slam_pose_translation_weight = 1e5,
local_slam_pose_rotation_weight = 1e5, local_slam_pose_rotation_weight = 1e5,
odometry_translation_weight = 1e5, odometry_translation_weight = 1e5,