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

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

@ -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)
<< " 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())
<< " with score " << std::setprecision(1) << 100. * score
<< 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();

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());
}
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,

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

@ -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)
<< " 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)
<< " with score " << std::setprecision(1) << 100. * score << "%.";
<< std::setprecision(3) << transform::GetAngle(difference);
}
info << " with score " << std::setprecision(1) << 100. * score << "%.";
LOG(INFO) << info.str();
}
}