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

Fix debug output for 3D loop closure error. (#826) 

PAIR=spielawa
master
Wolfgang Hess 2018-01-17 13:09:48 +01:00 committed by Wally B. Feed
parent 35a9c3d63b
commit ffdbf1c161
1 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cartographer/mapping_3d/pose_graph/constraint_builder.cc
View File

@ -244,8 +244,10 @@ void ConstraintBuilder::ComputeConstraint(
if (match_full_submap) { if (match_full_submap) {
info << " matches"; info << " matches";
} else { } else {
const transform::Rigid3d difference = global_submap_pose.inverse() * // Compute the difference between (submap i <- node j) according to loop
global_node_pose * // closure ('constraint_transform') and according to global SLAM state.
const transform::Rigid3d difference = global_node_pose.inverse() *
global_submap_pose *
constraint_transform; constraint_transform;
info << " differs by translation " << std::setprecision(2) info << " differs by translation " << std::setprecision(2)
<< difference.translation().norm() << " rotation " << difference.translation().norm() << " rotation "