From ca526de1f84cf85d5ea6345deea0bb4104f8c715 Mon Sep 17 00:00:00 2001 From: Linh Nguyen Date: Fri, 22 Jul 2022 01:47:18 +0800 Subject: [PATCH] Fix crash caused by setting gravity lower bound (#1893) Signed-off-by: Linh Nguyen --- .../internal/optimization/optimization_problem_3d.cc | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc index fca36d1..246707b 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc @@ -374,6 +374,8 @@ void OptimizationProblem3D::Solve( auto imu_it = imu_data.begin(); auto prev_node_it = node_it; + + bool gravity_block_added = false; for (++node_it; node_it != trajectory_end; ++node_it) { const NodeId first_node_id = prev_node_it->id; const NodeSpec3D& first_node_data = prev_node_it->data; @@ -432,6 +434,7 @@ void OptimizationProblem3D::Solve( C_nodes.at(third_node_id).translation(), &trajectory_data.gravity_constant, trajectory_data.imu_calibration.data()); + gravity_block_added = true; } problem.AddResidualBlock( RotationCostFunction3D::CreateAutoDiffCostFunction( @@ -442,8 +445,11 @@ void OptimizationProblem3D::Solve( trajectory_data.imu_calibration.data()); } - // Force gravity constant to be positive. - problem.SetParameterLowerBound(&trajectory_data.gravity_constant, 0, 0.0); + if (gravity_block_added) { + // Force gravity constant to be positive. + problem.SetParameterLowerBound(&trajectory_data.gravity_constant, 0, + 0.0); + } } }