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

Improve debug output and tiny bug fix. (#293) 

The full submap search window in 3D SLAM was
not calculated correctly.
master
Wolfgang Hess 2017-05-17 15:21:39 +02:00 committed by GitHub
parent 6c9e24d7c5
commit 33ce5dee37
3 changed files with 24 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc
View File

@ -233,15 +233,19 @@ void ConstraintBuilder::ComputeConstraint(
Constraint::INTER_SUBMAP}); Constraint::INTER_SUBMAP});
if (options_.log_matches()) { if (options_.log_matches()) {
const transform::Rigid2d difference =
initial_pose.inverse() * pose_estimate;
std::ostringstream info; std::ostringstream info;
info << "Node " << node_id << " with " << filtered_point_cloud.size() info << "Node " << node_id << " with " << filtered_point_cloud.size()
<< " points on submap " << submap_id << " differs by translation " << " points on submap " << submap_id << std::fixed;
<< std::fixed << std::setprecision(2) if (match_full_submap) {
<< difference.translation().norm() << " rotation " info << " matches";
<< std::setprecision(3) << std::abs(difference.normalized_angle()) } else {
<< " with score " << std::setprecision(1) << 100. * score 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 " << std::scientific << std::setprecision(4)
<< covariance.trace() << "."; << covariance.trace() << ".";
LOG(INFO) << info.str(); LOG(INFO) << info.str();

3
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc
View File

@ -129,7 +129,8 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
max_point_distance = std::max(max_point_distance, point.norm()); max_point_distance = std::max(max_point_distance, point.norm());
} }
const int linear_window_size = 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, const SearchParameters search_parameters{linear_window_size,
linear_window_size, M_PI}; linear_window_size, M_PI};
return MatchWithSearchParameters(search_parameters, initial_pose_estimate, return MatchWithSearchParameters(search_parameters, initial_pose_estimate,

18
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc
View File

@ -236,15 +236,19 @@ void ConstraintBuilder::ComputeConstraint(
OptimizationProblem::Constraint::INTER_SUBMAP}); OptimizationProblem::Constraint::INTER_SUBMAP});
if (options_.log_matches()) { if (options_.log_matches()) {
const transform::Rigid3d difference =
initial_pose.inverse() * pose_estimate;
std::ostringstream info; std::ostringstream info;
info << "Node " << node_id << " with " << filtered_point_cloud.size() info << "Node " << node_id << " with " << filtered_point_cloud.size()
<< " points on submap " << submap_id << " differs by translation " << " points on submap " << submap_id << std::fixed;
<< std::fixed << std::setprecision(2) if (match_full_submap) {
<< difference.translation().norm() << " rotation " info << " matches";
<< std::setprecision(3) << transform::GetAngle(difference) } else {
<< " with score " << std::setprecision(1) << 100. * score << "%."; 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(); LOG(INFO) << info.str();
} }
} }