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

Fix crash caused by setting gravity lower bound (#1893) 

Signed-off-by: Linh Nguyen <linh.nguyen@enway.ai>
master
Linh Nguyen 2022-07-22 01:47:18 +08:00 committed by GitHub
parent dc6263414b
commit ca526de1f8
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 8 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -374,6 +374,8 @@ void OptimizationProblem3D::Solve(
auto imu_it = imu_data.begin(); auto imu_it = imu_data.begin();
auto prev_node_it = node_it; auto prev_node_it = node_it;
bool gravity_block_added = false;
for (++node_it; node_it != trajectory_end; ++node_it) { for (++node_it; node_it != trajectory_end; ++node_it) {
const NodeId first_node_id = prev_node_it->id; const NodeId first_node_id = prev_node_it->id;
const NodeSpec3D& first_node_data = prev_node_it->data; const NodeSpec3D& first_node_data = prev_node_it->data;
@ -432,6 +434,7 @@ void OptimizationProblem3D::Solve(
C_nodes.at(third_node_id).translation(), C_nodes.at(third_node_id).translation(),
&trajectory_data.gravity_constant, &trajectory_data.gravity_constant,
trajectory_data.imu_calibration.data()); trajectory_data.imu_calibration.data());
gravity_block_added = true;
} }
problem.AddResidualBlock( problem.AddResidualBlock(
RotationCostFunction3D::CreateAutoDiffCostFunction( RotationCostFunction3D::CreateAutoDiffCostFunction(
@ -442,8 +445,11 @@ void OptimizationProblem3D::Solve(
trajectory_data.imu_calibration.data()); trajectory_data.imu_calibration.data());
} }
if (gravity_block_added) {
// Force gravity constant to be positive. // Force gravity constant to be positive.
problem.SetParameterLowerBound(&trajectory_data.gravity_constant, 0, 0.0); problem.SetParameterLowerBound(&trajectory_data.gravity_constant, 0,
0.0);
}
} }
} }