From 595ffba0cea39945a02fd071065402aee00f7a81 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 8 Jul 2020 14:50:02 +0200 Subject: [PATCH] Small improvements to 2D fixed frame pose support. (#1714) This is a follow up to #1580. Signed-off-by: Wolfgang Hess --- .../optimization/optimization_problem_2d.cc | 22 ++++++++++++------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/cartographer/mapping/internal/optimization/optimization_problem_2d.cc b/cartographer/mapping/internal/optimization/optimization_problem_2d.cc index 508cae7..b2ca862 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_2d.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_2d.cc @@ -209,6 +209,7 @@ void OptimizationProblem2D::InsertTrajectoryNode(const NodeId& node_id, void OptimizationProblem2D::TrimTrajectoryNode(const NodeId& node_id) { imu_data_.Trim(node_data_, node_id); odometry_data_.Trim(node_data_, node_id); + fixed_frame_pose_data_.Trim(node_data_, node_id); node_data_.Trim(node_id); if (node_data_.SizeOfTrajectoryOrZero(node_id.trajectory_id) == 0) { trajectory_data_.erase(node_id.trajectory_id); @@ -377,19 +378,24 @@ void OptimizationProblem2D::Solve( fixed_frame_pose_in_map = transform::Project2D( trajectory_data.fixed_frame_origin_in_map.value()); } else { - fixed_frame_pose_in_map = node_data.global_pose_2d; + fixed_frame_pose_in_map = + node_data.global_pose_2d * + transform::Project2D(constraint_pose.zbar_ij).inverse(); } - C_fixed_frames.emplace( - std::piecewise_construct, std::forward_as_tuple(trajectory_id), - std::forward_as_tuple(FromPose(fixed_frame_pose_in_map))); + C_fixed_frames.emplace(trajectory_id, + FromPose(fixed_frame_pose_in_map)); fixed_frame_pose_initialized = true; } - problem.AddResidualBlock(CreateAutoDiffSpaCostFunction(constraint_pose), - nullptr /* loss function */, - C_fixed_frames.at(trajectory_id).data(), - C_nodes.at(node_id).data()); + problem.AddResidualBlock( + CreateAutoDiffSpaCostFunction(constraint_pose), + options_.fixed_frame_pose_use_tolerant_loss() + ? new ceres::TolerantLoss( + options_.fixed_frame_pose_tolerant_loss_param_a(), + options_.fixed_frame_pose_tolerant_loss_param_b()) + : nullptr, + C_fixed_frames.at(trajectory_id).data(), C_nodes.at(node_id).data()); } }