From ffdbf1c161484cdd7fb163ff0dfacf2a2a733882 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 17 Jan 2018 13:09:48 +0100 Subject: [PATCH] Fix debug output for 3D loop closure error. (#826) PAIR=spielawa --- cartographer/mapping_3d/pose_graph/constraint_builder.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/cartographer/mapping_3d/pose_graph/constraint_builder.cc b/cartographer/mapping_3d/pose_graph/constraint_builder.cc index 1f5024f..c3ac1a4 100644 --- a/cartographer/mapping_3d/pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/pose_graph/constraint_builder.cc @@ -244,8 +244,10 @@ void ConstraintBuilder::ComputeConstraint( if (match_full_submap) { info << " matches"; } else { - const transform::Rigid3d difference = global_submap_pose.inverse() * - global_node_pose * + // Compute the difference between (submap i <- node j) according to loop + // closure ('constraint_transform') and according to global SLAM state. + const transform::Rigid3d difference = global_node_pose.inverse() * + global_submap_pose * constraint_transform; info << " differs by translation " << std::setprecision(2) << difference.translation().norm() << " rotation "