约束计算
闭环检测的策略：搜索闭环，通过匹配检测是否是闭环，采用了分支定界法。
前已经述及PoseGraph的内容，此处继续。位姿图类定义了pose_graph::ConstraintBuilder constraint_builder_对象。
1.ConstraintBuilder类图
定义了SubmapScanMatcher的键值对。
// Map of already constructed scan matchers by 'submap_id'.
std::map<mapping::SubmapId, SubmapScanMatcher> submap_scan_matchers_ GUARDED_BY(mutex_);
SubmapScanMatcher结构体定义如下：
struct SubmapScanMatcher
{
const ProbabilityGrid* probability_grid;
std::unique_ptr<scan_matching::FastCorrelativeScanMatcher>
fast_correlative_scan_matcher;
};
注意ConstraintBuilder::ComputeConstraint方法，MaybeAddConstraint和MaybeAddGlobalConstraint都调用了该方法。
void ConstraintBuilder::ComputeConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap,
const mapping::NodeId& node_id, bool match_full_submap,
const mapping::TrajectoryNode::Data* const constant_data,
const transform::Rigid2d& initial_relative_pose,
std::unique_ptr<ConstraintBuilder::Constraint>* constraint) {
const transform::Rigid2d initial_pose =
ComputeSubmapPose(*submap) * initial_relative_pose;
const SubmapScanMatcher* const submap_scan_matcher =
GetSubmapScanMatcher(submap_id); // The 'constraint_transform' (submap i <- node j) is computed from:
// - a 'filtered_gravity_aligned_point_cloud' in node j,
// - the initial guess 'initial_pose' for (map <- node j),
// - the result 'pose_estimate' of Match() (map <- node j).
// - the ComputeSubmapPose() (map <- submap i)
float score = .;
transform::Rigid2d pose_estimate = transform::Rigid2d::Identity(); // Compute 'pose_estimate' in three stages:
// 1. Fast estimate using the fast correlative scan matcher.
// 2. Prune if the score is too low.
// 3. Refine.
if (match_full_submap) {
if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
constant_data->filtered_gravity_aligned_point_cloud,
options_.global_localization_min_score(), &score, &pose_estimate)) {
CHECK_GT(score, options_.global_localization_min_score());
CHECK_GE(node_id.trajectory_id, );
CHECK_GE(submap_id.trajectory_id, );
} else {
return;
}
} else {
if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
initial_pose, constant_data->filtered_gravity_aligned_point_cloud,
options_.min_score(), &score, &pose_estimate)) {
// We've reported a successful local match.
CHECK_GT(score, options_.min_score());
} else {
return;
}
}
{
common::MutexLocker locker(&mutex_);
score_histogram_.Add(score);
} // Use the CSM estimate as both the initial and previous pose. This has the
// effect that, in the absence of better information, we prefer the original
// CSM estimate.
ceres::Solver::Summary unused_summary;
ceres_scan_matcher_.Match(pose_estimate.translation(), pose_estimate,
constant_data->filtered_gravity_aligned_point_cloud,
*submap_scan_matcher->probability_grid,
&pose_estimate, &unused_summary); const transform::Rigid2d constraint_transform =
ComputeSubmapPose(*submap).inverse() * pose_estimate;
constraint->reset(new Constraint{submap_id,
node_id,
{transform::Embed3D(constraint_transform),
options_.loop_closure_translation_weight(),
options_.loop_closure_rotation_weight()},
Constraint::INTER_SUBMAP}); if (options_.log_matches()) {
std::ostringstream info;
info << "Node " << node_id << " with "
<< constant_data->filtered_gravity_aligned_point_cloud.size()
<< " 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()
<< difference.translation().norm() << " rotation "
<< std::setprecision() << std::abs(difference.normalized_angle());
}
info << " with score " << std::setprecision() << . * score << "%.";
LOG(INFO) << info.str();
}
}
这里出现了scan_matching::FastCorrelativeScanMatcher，另一种扫描匹配的方法。论文中介绍的分支定界法就在这个类中实现。
以上FastCorrelativeScanMatcher::Match和FastCorrelativeScanMatcher::MatchFullSubmap方法都调用了FastCorrelativeScanMatcher::MatchWithSearchParameters方法。
FastCorrelativeScanMatcher::MatchWithSearchParameters调用了FastCorrelativeScanMatcher::BranchAndBound方法。
Tips：总结一下出现的几种扫描匹配的方法？ RealTimeCorrelativeScanMatcher FastCorrelativeScanMatcher |
bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
SearchParameters search_parameters,
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, float min_score, float* score,
transform::Rigid2d* pose_estimate) const
{
CHECK_NOTNULL(score);
CHECK_NOTNULL(pose_estimate); const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
point_cloud,
transform::Rigid3f::Rotation(Eigen::AngleAxisf(
initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
const std::vector<sensor::PointCloud> rotated_scans =
GenerateRotatedScans(rotated_point_cloud, search_parameters);
const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
limits_, rotated_scans,
Eigen::Translation2f(initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y()));
search_parameters.ShrinkToFit(discrete_scans, limits_.cell_limits()); const std::vector<Candidate> lowest_resolution_candidates =
ComputeLowestResolutionCandidates(discrete_scans, search_parameters);
const Candidate best_candidate = BranchAndBound(
discrete_scans, search_parameters, lowest_resolution_candidates,
precomputation_grid_stack_->max_depth(), min_score);//分支定界法
if (best_candidate.score > min_score) {
*score = best_candidate.score;
*pose_estimate = transform::Rigid2d(
{initial_pose_estimate.translation().x() + best_candidate.x,
initial_pose_estimate.translation().y() + best_candidate.y},
initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation));
return true;
}
return false;
}
2.分支定界法
FastCorrelativeScanMatcher::BranchAndBound，......
Candidate FastCorrelativeScanMatcher::BranchAndBound(
const std::vector<DiscreteScan>& discrete_scans,
const SearchParameters& search_parameters,
const std::vector<Candidate>& candidates, const int candidate_depth,
float min_score) const
{
if (candidate_depth == )
{
// Return the best candidate.
return *candidates.begin();
} Candidate best_high_resolution_candidate(, , , search_parameters);
best_high_resolution_candidate.score = min_score;
for (const Candidate& candidate : candidates)
{
if (candidate.score <= min_score) { break; }
std::vector<Candidate> higher_resolution_candidates;
const int half_width = << (candidate_depth - );
for (int x_offset : {, half_width})
{
if (candidate.x_index_offset + x_offset >
search_parameters.linear_bounds[candidate.scan_index].max_x) {
break;
}
for (int y_offset : {, half_width}) {
if (candidate.y_index_offset + y_offset >
search_parameters.linear_bounds[candidate.scan_index].max_y) {
break;
}
higher_resolution_candidates.emplace_back(
candidate.scan_index, candidate.x_index_offset + x_offset,
candidate.y_index_offset + y_offset, search_parameters);
}
}
ScoreCandidates(precomputation_grid_stack_->Get(candidate_depth - ),
discrete_scans, search_parameters,
&higher_resolution_candidates);
best_high_resolution_candidate = std::max(
best_high_resolution_candidate,
BranchAndBound(discrete_scans, search_parameters,
higher_resolution_candidates, candidate_depth - ,
best_high_resolution_candidate.score));
}
return best_high_resolution_candidate;
}