diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index 9b5ad76..eb3a820 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -233,15 +233,19 @@ void ConstraintBuilder::ComputeConstraint( Constraint::INTER_SUBMAP}); if (options_.log_matches()) { - const transform::Rigid2d difference = - initial_pose.inverse() * pose_estimate; std::ostringstream info; info << "Node " << node_id << " with " << filtered_point_cloud.size() - << " points on submap " << submap_id << " differs by translation " - << std::fixed << std::setprecision(2) - << difference.translation().norm() << " rotation " - << std::setprecision(3) << std::abs(difference.normalized_angle()) - << " with score " << std::setprecision(1) << 100. * score + << " points on submap " << submap_id << std::fixed; + if (match_full_submap) { + info << " matches"; + } else { + const transform::Rigid2d difference = + initial_pose.inverse() * pose_estimate; + info << " differs by translation " << std::setprecision(2) + << difference.translation().norm() << " rotation " + << std::setprecision(3) << std::abs(difference.normalized_angle()); + } + info << " with score " << std::setprecision(1) << 100. * score << "% covariance trace " << std::scientific << std::setprecision(4) << covariance.trace() << "."; LOG(INFO) << info.str(); diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc index 598f4ea..153ac7e 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc @@ -129,7 +129,8 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap( max_point_distance = std::max(max_point_distance, point.norm()); } const int linear_window_size = - (width_in_voxels_ + 1) / 2 + std::ceil(max_point_distance); + (width_in_voxels_ + 1) / 2 + + common::RoundToInt(max_point_distance / resolution_ + 0.5f); const SearchParameters search_parameters{linear_window_size, linear_window_size, M_PI}; return MatchWithSearchParameters(search_parameters, initial_pose_estimate, diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index c94c730..2491082 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -236,15 +236,19 @@ void ConstraintBuilder::ComputeConstraint( OptimizationProblem::Constraint::INTER_SUBMAP}); if (options_.log_matches()) { - const transform::Rigid3d difference = - initial_pose.inverse() * pose_estimate; std::ostringstream info; info << "Node " << node_id << " with " << filtered_point_cloud.size() - << " points on submap " << submap_id << " differs by translation " - << std::fixed << std::setprecision(2) - << difference.translation().norm() << " rotation " - << std::setprecision(3) << transform::GetAngle(difference) - << " with score " << std::setprecision(1) << 100. * score << "%."; + << " points on submap " << submap_id << std::fixed; + if (match_full_submap) { + info << " matches"; + } else { + const transform::Rigid3d difference = + initial_pose.inverse() * pose_estimate; + info << " differs by translation " << std::setprecision(2) + << difference.translation().norm() << " rotation " + << std::setprecision(3) << transform::GetAngle(difference); + } + info << " with score " << std::setprecision(1) << 100. * score << "%."; LOG(INFO) << info.str(); } }