Cartographer源码阅读-2D后端回环计算
ConstraintBuilder2D
很重要的是:
(1)2D的回环形式和3D的回环形式不一样,2D回环:
submap的global pose(global pose * 重力方向)的逆 * node的global_pose_2d(global pose * 重力的逆)。
(2)需要明确的是,分支定界搜索的网格仍然是一个submap,不是所有的submap。是拿着一个Node的点云数据,在一个submap里做分支定界搜索。
class ConstraintBuilder2D {
public:
using Constraint = PoseGraphInterface::Constraint;
using Result = std::vector<Constraint>;
ConstraintBuilder2D(const proto::ConstraintBuilderOptions& options,
common::ThreadPoolInterface* thread_pool);
~ConstraintBuilder2D();
ConstraintBuilder2D(const ConstraintBuilder2D&) = delete;
ConstraintBuilder2D& operator=(const ConstraintBuilder2D&) = delete;
// 两个接口,在PoseGraph2D中调用,一个是有初始位姿,一个是没有初始位姿
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_id', and the 'compressed_point_cloud' for 'node_id'. The
// 'initial_relative_pose' is relative to the 'submap'.
//
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
// all computations are finished.
void MaybeAddConstraint(const SubmapId& submap_id, const Submap2D* submap,
const NodeId& node_id,
const TrajectoryNode::Data* const constant_data,
const transform::Rigid2d& initial_relative_pose);
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_id' and the 'compressed_point_cloud' for 'node_id'.
// This performs full-submap matching.
//
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
// all computations are finished.
void MaybeAddGlobalConstraint(
const SubmapId& submap_id, const Submap2D* submap, const NodeId& node_id,
const TrajectoryNode::Data* const constant_data);
// Must be called after all computations related to one node have been added.
void NotifyEndOfNode();
// Registers the 'callback' to be called with the results, after all
// computations triggered by 'MaybeAdd*Constraint' have finished.
// 'callback' is executed in the 'ThreadPool'.
void WhenDone(const std::function<void(const Result&)>& callback);
// Returns the number of consecutive finished nodes.
int GetNumFinishedNodes();
// Delete data related to 'submap_id'.
void DeleteScanMatcher(const SubmapId& submap_id);
static void RegisterMetrics(metrics::FamilyFactory* family_factory);
private:
struct SubmapScanMatcher {
// submap的网格数据
const Grid2D* grid;
// 分支定界搜索类
std::unique_ptr<scan_matching::FastCorrelativeScanMatcher2D>
fast_correlative_scan_matcher;
// 任务
std::weak_ptr<common::Task> creation_task_handle;
};
// 根据submap的网格数据和id,构造SubmapScanMatcher
// The returned 'grid' and 'fast_correlative_scan_matcher' must only be
// accessed after 'creation_task_handle' has completed.
const SubmapScanMatcher* DispatchScanMatcherConstruction(
const SubmapId& submap_id, const Grid2D* grid) REQUIRES(mutex_);
// Runs in a background thread and does computations for an additional
// constraint, assuming 'submap' and 'compressed_point_cloud' do not change
// anymore. As output, it may create a new Constraint in 'constraint'.
void ComputeConstraint(const SubmapId& submap_id, const Submap2D* submap,
const NodeId& node_id, bool match_full_submap,
const TrajectoryNode::Data* const constant_data,
const transform::Rigid2d& initial_relative_pose,
const SubmapScanMatcher& submap_scan_matcher,
std::unique_ptr<Constraint>* constraint)
EXCLUDES(mutex_);
void RunWhenDoneCallback() EXCLUDES(mutex_);
const constraints::proto::ConstraintBuilderOptions options_;
common::ThreadPoolInterface* thread_pool_;
common::Mutex mutex_;
// 'callback' set by WhenDone().
std::unique_ptr<std::function<void(const Result&)>> when_done_
GUARDED_BY(mutex_);
// TODO(gaschler): Use atomics instead of mutex to access these counters.
// Number of the node in reaction to which computations are currently
// added. This is always the number of nodes seen so far, even when older
// nodes are matched against a new submap.
int num_started_nodes_ GUARDED_BY(mutex_) = 0;
int num_finished_nodes_ GUARDED_BY(mutex_) = 0;
std::unique_ptr<common::Task> finish_node_task_ GUARDED_BY(mutex_);
std::unique_ptr<common::Task> when_done_task_ GUARDED_BY(mutex_);
// Constraints currently being computed in the background. A deque is used to
// keep pointers valid when adding more entries. Constraint search results
// with below-threshold scores are also 'nullptr'.
std::deque<std::unique_ptr<Constraint>> constraints_ GUARDED_BY(mutex_);
// Map of dispatched or constructed scan matchers by 'submap_id'.
std::map<SubmapId, SubmapScanMatcher> submap_scan_matchers_
GUARDED_BY(mutex_);
common::FixedRatioSampler sampler_;
scan_matching::CeresScanMatcher2D ceres_scan_matcher_;
// Histogram of scan matcher scores.
common::Histogram score_histogram_ GUARDED_BY(mutex_);
};
回环计算接口:
void ConstraintBuilder2D::MaybeAddConstraint(
const SubmapId& submap_id, const Submap2D* const submap,
const NodeId& node_id, const TrajectoryNode::Data* const constant_data,
const transform::Rigid2d& initial_relative_pose) {
// 相隔太远的大概率不会有回环,所以不用算
if (initial_relative_pose.translation().norm() >
options_.max_constraint_distance()) {
return;
}
if (!sampler_.Pulse()) return;
common::MutexLocker locker(&mutex_);
if (when_done_) {
LOG(WARNING)
<< "MaybeAddConstraint was called while WhenDone was scheduled.";
}
// 扩充constraints_
constraints_.emplace_back();
kQueueLengthMetric->Set(constraints_.size());
// 取出最后一个constraints_,此时为空,待计算
auto* const constraint = &constraints_.back();
// 构建submap的scan_matcher:其中其网格为submap的网格
const auto* scan_matcher =
DispatchScanMatcherConstruction(submap_id, submap->grid());
auto constraint_task = common::make_unique<common::Task>();
// 和局部网格匹配即可
constraint_task->SetWorkItem([=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */
constant_data, initial_relative_pose, *scan_matcher,
constraint);
});
constraint_task->AddDependency(scan_matcher->creation_task_handle);
auto constraint_task_handle =
thread_pool_->Schedule(std::move(constraint_task));
finish_node_task_->AddDependency(constraint_task_handle);
}
// 同上,只不过是需要match所有的网格
void ConstraintBuilder2D::MaybeAddGlobalConstraint(
const SubmapId& submap_id, const Submap2D* const submap,
const NodeId& node_id, const TrajectoryNode::Data* const constant_data) {
common::MutexLocker locker(&mutex_);
if (when_done_) {
LOG(WARNING)
<< "MaybeAddGlobalConstraint was called while WhenDone was scheduled.";
}
constraints_.emplace_back();
kQueueLengthMetric->Set(constraints_.size());
auto* const constraint = &constraints_.back();
const auto* scan_matcher =
DispatchScanMatcherConstruction(submap_id, submap->grid());
auto constraint_task = common::make_unique<common::Task>();
constraint_task->SetWorkItem([=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id, true, /* match_full_submap */
constant_data, transform::Rigid2d::Identity(),
*scan_matcher, constraint);
});
constraint_task->AddDependency(scan_matcher->creation_task_handle);
auto constraint_task_handle =
thread_pool_->Schedule(std::move(constraint_task));
finish_node_task_->AddDependency(constraint_task_handle);
}
// 计算回环
void ConstraintBuilder2D::ComputeConstraint(
const SubmapId& submap_id, const Submap2D* const submap,
const NodeId& node_id, bool match_full_submap,
const TrajectoryNode::Data* const constant_data,
const transform::Rigid2d& initial_relative_pose,
const SubmapScanMatcher& submap_scan_matcher,
std::unique_ptr<ConstraintBuilder2D::Constraint>* constraint) {
// constaint = submap.local_pose_3d.inverse() * node.local_pose_2d
// 得到node.local_pose_2d
const transform::Rigid2d initial_pose =
ComputeSubmapPose(*submap) * initial_relative_pose;
// 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 = 0.;
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) {
kGlobalConstraintsSearchedMetric->Increment();
// 该部分仍然是暴力匹配,不过加上了分支定界搜索,和前端的方法是有一定的相似性,传进来的都是重力对齐的点云数据,搜索框大小为:线:1e6*0.05,角度为pi
// 分数是global_localization_min_score
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, 0);
CHECK_GE(submap_id.trajectory_id, 0);
kGlobalConstraintsFoundMetric->Increment();
kGlobalConstraintScoresMetric->Observe(score);
} else {
return;
}
} else {
kConstraintsSearchedMetric->Increment();
// 搜索框大小是设置参数
// 分数是min_score,如果满足,则得到回环
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());
kConstraintsFoundMetric->Increment();
kConstraintScoresMetric->Observe(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.
// 仍然使用CSM来求解位姿
ceres::Solver::Summary unused_summary;
ceres_scan_matcher_.Match(pose_estimate.translation(), pose_estimate,
constant_data->filtered_gravity_aligned_point_cloud,
*submap_scan_matcher.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(2)
<< difference.translation().norm() << " rotation "
<< std::setprecision(3) << std::abs(difference.normalized_angle());
}
info << " with score " << std::setprecision(1) << 100. * score << "%.";
LOG(INFO) << info.str();
}
}
由此,计算回环的方法主要为:
(1)通过分支定界方法,计算node和submap之间的粗略的初始相对位姿;
(2)利用CSM算法,计算相对位姿。
和前端很相似,前端使用暴力匹配的方法得到初始相对位姿,而本处是通过分支定界计算的,至于为什么不用暴力匹配,相信大家都知道(还不是因为慢)。前端可以用暴力匹配来做,因为只需要和一个submap匹配就好啦,所以对算力的要求没那么高,此外,如果你的轮速计或IMU数据足够好,可以不做暴力匹配的。
构造分支定界匹配类:
// 根据submap构造SubmapScanMatcher
const ConstraintBuilder2D::SubmapScanMatcher*
ConstraintBuilder2D::DispatchScanMatcherConstruction(const SubmapId& submap_id,
const Grid2D* const grid) {
if (submap_scan_matchers_.count(submap_id) != 0) {
return &submap_scan_matchers_.at(submap_id);
}
auto& submap_scan_matcher = submap_scan_matchers_[submap_id];
submap_scan_matcher.grid = grid;
auto& scan_matcher_options = options_.fast_correlative_scan_matcher_options();
auto scan_matcher_task = common::make_unique<common::Task>();
scan_matcher_task->SetWorkItem(
[&submap_scan_matcher, &scan_matcher_options]() {
submap_scan_matcher.fast_correlative_scan_matcher =
common::make_unique<scan_matching::FastCorrelativeScanMatcher2D>(
*submap_scan_matcher.grid, scan_matcher_options);
});
submap_scan_matcher.creation_task_handle =
thread_pool_->Schedule(std::move(scan_matcher_task));
return &submap_scan_matchers_.at(submap_id);
}
分支定界FastCorrelativeScanMatcher2D
此处默认,你应该基本了解分支定界的原理,所以此处不再详细展开。只需要重点看一下是怎么实现的:
(1)多分辨率的网格是怎么实现的;
(2)分支定界怎么实现的;
(3)打分是怎么实现的。
// 单个分辨率的网格地图,
class PrecomputationGrid2D {
public:
// width是新的网格的
// 参数grid为submap的原始网格数据,limits是原始网格数据中xy方向上的网格数量
PrecomputationGrid2D(const Grid2D& grid, const CellLimits& limits, int width,
std::vector<float>* reusable_intermediate_grid);
// Returns a value between 0 and 255 to represent probabilities between
// min_score and max_score.
// 根据xy坐标,获取该(xy)坐标下width*width网格内的最大值
int GetValue(const Eigen::Array2i& xy_index) const {
const Eigen::Array2i local_xy_index = xy_index - offset_;
// The static_cast<unsigned> is for performance to check with 2 comparisons
// xy_index.x() < offset_.x() || xy_index.y() < offset_.y() ||
// local_xy_index.x() >= wide_limits_.num_x_cells ||
// local_xy_index.y() >= wide_limits_.num_y_cells
// instead of using 4 comparisons.
if (static_cast<unsigned>(local_xy_index.x()) >=
static_cast<unsigned>(wide_limits_.num_x_cells) ||
static_cast<unsigned>(local_xy_index.y()) >=
static_cast<unsigned>(wide_limits_.num_y_cells)) {
return 0;
}
const int stride = wide_limits_.num_x_cells;
return cells_[local_xy_index.x() + local_xy_index.y() * stride];
}
// Maps values from [0, 255] to [min_score, max_score].
float ToScore(float value) const {
return min_score_ + value * ((max_score_ - min_score_) / 255.f);
}
private:
uint8 ComputeCellValue(float probability) const;
// Offset of the precomputation grid in relation to the 'grid'
// including the additional 'width' - 1 cells.
const Eigen::Array2i offset_;
// Size of the precomputation grid.
const CellLimits wide_limits_;
const float min_score_;
const float max_score_;
// Probabilites mapped to 0 to 255.
std::vector<uint8> cells_;
};
// 管理分支定界所需的不同分辨率的网格
class PrecomputationGridStack2D {
public:
PrecomputationGridStack2D(
const Grid2D& grid,
const proto::FastCorrelativeScanMatcherOptions2D& options);
const PrecomputationGrid2D& Get(int index) {
return precomputation_grids_[index];
}
int max_depth() const { return precomputation_grids_.size() - 1; }
private:
std::vector<PrecomputationGrid2D> precomputation_grids_;
};
// 分支定界的接口类
// An implementation of "Real-Time Correlative Scan Matching" by Olson.
class FastCorrelativeScanMatcher2D {
public:
FastCorrelativeScanMatcher2D(
const Grid2D& grid,
const proto::FastCorrelativeScanMatcherOptions2D& options);
~FastCorrelativeScanMatcher2D();
FastCorrelativeScanMatcher2D(const FastCorrelativeScanMatcher2D&) = delete;
FastCorrelativeScanMatcher2D& operator=(const FastCorrelativeScanMatcher2D&) =
delete;
// Aligns 'point_cloud' within the 'grid' given an
// 'initial_pose_estimate'. If a score above 'min_score' (excluding equality)
// is possible, true is returned, and 'score' and 'pose_estimate' are updated
// with the result.
bool Match(const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, float min_score,
float* score, transform::Rigid2d* pose_estimate) const;
// Aligns 'point_cloud' within the full 'grid', i.e., not
// restricted to the configured search window. If a score above 'min_score'
// (excluding equality) is possible, true is returned, and 'score' and
// 'pose_estimate' are updated with the result.
bool MatchFullSubmap(const sensor::PointCloud& point_cloud, float min_score,
float* score, transform::Rigid2d* pose_estimate) const;
private:
// The actual implementation of the scan matcher, called by Match() and
// MatchFullSubmap() with appropriate 'initial_pose_estimate' and
// 'search_parameters'.
bool 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;
// 计算低分辨率的可能解
std::vector<Candidate2D> ComputeLowestResolutionCandidates(
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters) const;
// 生成低分辨率可能解
std::vector<Candidate2D> GenerateLowestResolutionCandidates(
const SearchParameters& search_parameters) const;
// 打分函数
void ScoreCandidates(const PrecomputationGrid2D& precomputation_grid,
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters,
std::vector<Candidate2D>* const candidates) const;
// 分支定界
Candidate2D BranchAndBound(const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters,
const std::vector<Candidate2D>& candidates,
int candidate_depth, float min_score) const;
const proto::FastCorrelativeScanMatcherOptions2D options_;
MapLimits limits_;
std::unique_ptr<PrecomputationGridStack2D> precomputation_grid_stack_;
};
接口
(1)有初始位姿的接口
// 有初始位姿的情况下的接口
bool FastCorrelativeScanMatcher2D::Match(
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, const float min_score, float* score,
transform::Rigid2d* pose_estimate) const {
// 利用设定参数构造搜索框的大小,搜索框的中心位置设定为初值位姿
const SearchParameters search_parameters(options_.linear_search_window(),
options_.angular_search_window(),
point_cloud, limits_.resolution());
return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
point_cloud, min_score, score,
pose_estimate);
}
(2)无初始位姿的接口
bool FastCorrelativeScanMatcher2D::MatchFullSubmap(
const sensor::PointCloud& point_cloud, float min_score, float* score,
transform::Rigid2d* pose_estimate) const {
// Compute a search window around the center of the submap that includes it
// fully.
// 和整个submap匹配,为了不放过所有的可能,所以,线搜索和角度搜索都需要设计的很大,且搜索的中心位置设定在整个submap的中心
const SearchParameters search_parameters(
1e6 * limits_.resolution(), // Linear search window, 1e6 cells/direction.
M_PI, // Angular search window, 180 degrees in both directions.
point_cloud, limits_.resolution());
const transform::Rigid2d center = transform::Rigid2d::Translation(
// 该处的计算公式在之前有介绍过,不再详细阐述
limits_.max() - 0.5 * limits_.resolution() *
Eigen::Vector2d(limits_.cell_limits().num_y_cells,
limits_.cell_limits().num_x_cells));
return MatchWithSearchParameters(search_parameters, center, point_cloud,
min_score, score, pose_estimate);
}
同一入口:
// 有无初始位姿的统一接口
bool FastCorrelativeScanMatcher2D::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);
// 将初始点云调整角度,调整到submap坐标系下
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())));
// 根据角度旋转生成一系列的PointCloud,虽然都在世界坐标系下,但是原点和世界坐标系不重合,该处的处理方式和前端的暴力匹配一样,不再阐述
const std::vector<sensor::PointCloud> rotated_scans =
GenerateRotatedScans(rotated_point_cloud, search_parameters);
// 旋转后的所有PointCloud加上平移,同一转换到世界坐标系下,并计算在submap坐标系下的xy坐标,存放在DiscreteScan2D里(一个可能位姿,对应着一个点云数据),该处的处理方式和前端的暴力匹配一样,不再阐述
const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
limits_, rotated_scans,
Eigen::Translation2f(initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y()));
// 将超过边界的点的坐标计算出来,并更新search_parameters里的边界
search_parameters.ShrinkToFit(discrete_scans, limits_.cell_limits());
// 生成低分辨率地图的候选解,并且和最低分辨率的地图进行匹配,并且得到所有激光帧和候选解的得分
const std::vector<Candidate2D> lowest_resolution_candidates =
ComputeLowestResolutionCandidates(discrete_scans, search_parameters);
// 分支定界搜索最好的可能解(多个分辨率搜索)
const Candidate2D 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;
}
生成低分辨率候选解
// 生成低分辨率的候选解,候选解中包括scan的id,offset,和初始位姿的相对位姿,得分情况
std::vector<Candidate2D>
FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates(
const SearchParameters& search_parameters) const {
const int linear_step_size = 1 << precomputation_grid_stack_->max_depth();
int num_candidates = 0;
// 遍历所有的帧,计算所有可行解的数量
for (int scan_index = 0; scan_index != search_parameters.num_scans;
++scan_index) {
const int num_lowest_resolution_linear_x_candidates =
(search_parameters.linear_bounds[scan_index].max_x -
search_parameters.linear_bounds[scan_index].min_x + linear_step_size) /
linear_step_size;
const int num_lowest_resolution_linear_y_candidates =
(search_parameters.linear_bounds[scan_index].max_y -
search_parameters.linear_bounds[scan_index].min_y + linear_step_size) /
linear_step_size;
num_candidates += num_lowest_resolution_linear_x_candidates *
num_lowest_resolution_linear_y_candidates;
}
// 把所有的可行解放入candidates中
std::vector<Candidate2D> candidates;
candidates.reserve(num_candidates);
for (int scan_index = 0; scan_index != search_parameters.num_scans;
++scan_index) {
for (int x_index_offset = search_parameters.linear_bounds[scan_index].min_x;
x_index_offset <= search_parameters.linear_bounds[scan_index].max_x;
x_index_offset += linear_step_size) {
for (int y_index_offset =
search_parameters.linear_bounds[scan_index].min_y;
y_index_offset <= search_parameters.linear_bounds[scan_index].max_y;
y_index_offset += linear_step_size) {
candidates.emplace_back(scan_index, x_index_offset, y_index_offset,
search_parameters);
}
}
}
CHECK_EQ(candidates.size(), num_candidates);
return candidates;
}
低分辨率候选解打分
std::vector<Candidate2D>
FastCorrelativeScanMatcher2D::ComputeLowestResolutionCandidates(
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters) const {
// 生成低分辨率的可能解
std::vector<Candidate2D> lowest_resolution_candidates =
GenerateLowestResolutionCandidates(search_parameters);
// 对低分辨率的候选解打分
ScoreCandidates(
precomputation_grid_stack_->Get(precomputation_grid_stack_->max_depth()),
discrete_scans, search_parameters, &lowest_resolution_candidates);
return lowest_resolution_candidates;
}
打分
void FastCorrelativeScanMatcher2D::ScoreCandidates(
const PrecomputationGrid2D& precomputation_grid,
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters,
std::vector<Candidate2D>* const candidates) const {
// 遍历所有可能解,并得到可能解的得分
for (Candidate2D& candidate : *candidates) {
int sum = 0;
// 针对当前解,遍历所有的点云,如果点云和submap匹配的越好,得分肯定越高
for (const Eigen::Array2i& xy_index :
discrete_scans[candidate.scan_index]) {
const Eigen::Array2i proposed_xy_index(
xy_index.x() + candidate.x_index_offset,
xy_index.y() + candidate.y_index_offset);
sum += precomputation_grid.GetValue(proposed_xy_index);
}
candidate.score = precomputation_grid.ToScore(
sum / static_cast<float>(discrete_scans[candidate.scan_index].size()));
}
// 分数从高到底排序
std::sort(candidates->begin(), candidates->end(),
std::greater<Candidate2D>());
}
分支定界
// 分支定界:递归的方法
Candidate2D FastCorrelativeScanMatcher2D::BranchAndBound(
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters,
const std::vector<Candidate2D>& candidates, const int candidate_depth,
float min_score) const {
// 结束条件:叶节点,不能再向下搜索,此时返回排序后candidates的第一个得分最高的解
if (candidate_depth == 0) {
// Return the best candidate.
return *candidates.begin();
}
// 构造Candidate2D, 参数分别为:scan_id(该id包含角度信息,参考前端的解释);x方向offset(相对于初始位姿的x方向平移);y方向Offset(相对于初始位姿的y方向平移);搜索参数(参考Candidate2D,主要是用里面的resolution以计算真实xy做得分、num_angular_perturbations和angular_perturbation_step_size以计算相对于初始位姿的角度偏移)
Candidate2D best_high_resolution_candidate(0, 0, 0, search_parameters);
// 得分赋初值,初值必须赋值为设定的阈值
best_high_resolution_candidate.score = min_score;
for (const Candidate2D& candidate : candidates) {
// 如果当前根节点的得分低,则没必要再往下搜索
if (candidate.score <= min_score) {
break;
}
// 存放该根节点的子节点:子节点共四个
std::vector<Candidate2D> higher_resolution_candidates;
// 步长减半,进行分支操作(因为网格采样的时候分辨率是除以2的),所有从粗糙层到下一层,其子节点应该有4个:当前的offset,(0,0),(offset/2,0),(0,offset/2),(offset/2,offset/2),若超出边界,则不需要添加该子节点
const int half_width = 1 << (candidate_depth - 1);
for (int x_offset : {0, half_width}) {
if (candidate.x_index_offset + x_offset >
search_parameters.linear_bounds[candidate.scan_index].max_x) {
break;
}
for (int y_offset : {0, half_width}) {
if (candidate.y_index_offset + y_offset >
search_parameters.linear_bounds[candidate.scan_index].max_y) {
break;
}
// 分支操作,将四个可能解放到新的vector<Candidate2D>中,作为下次递归的参数
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 - 1),
discrete_scans, search_parameters,
&higher_resolution_candidates);
// 递归,对得分最高的节点继续分支操作(因为该处的值赋值为得分最高的可能解的分),直到最底层,返回一个得分最高的叶节点,此时,得分最高的节点的得分肯定高于设定的阈值,如果没有可能解满足>min_score的要求,则返回值的分数为min_score,不满足分支定界的要求
best_high_resolution_candidate = std::max(
best_high_resolution_candidate,
BranchAndBound(discrete_scans, search_parameters,
higher_resolution_candidates, candidate_depth - 1,
best_high_resolution_candidate.score));
}
return best_high_resolution_candidate;
}
多分辨率网格的生成
在FastCorrelativeScanMatcher2D类的构造函数中,使用submap的网格数据构造了多分辨率网格的管理器:precomputation_grid_stack_,在该管理器中的构造函数中构造了由精到糙的不同分辨率网格地图。
FastCorrelativeScanMatcher2D::FastCorrelativeScanMatcher2D(
const Grid2D& grid,
const proto::FastCorrelativeScanMatcherOptions2D& options)
: options_(options),
limits_(grid.limits()),
precomputation_grid_stack_(
common::make_unique<PrecomputationGridStack2D>(grid, options)) {}
PrecomputationGridStack2D::PrecomputationGridStack2D(
const Grid2D& grid,
const proto::FastCorrelativeScanMatcherOptions2D& options) {
CHECK_GE(options.branch_and_bound_depth(), 1);
// 根据分支定界参数设置,设置生成多少个网格地图
const int max_width = 1 << (options.branch_and_bound_depth() - 1);
precomputation_grids_.reserve(options.branch_and_bound_depth());
// 每层低分辨率的网格可以反复利用上一次计算的结果,该vector里存放的是(x0, y)上的最大值,y是固定的大小:原始网格的y方向上的网格数量
std::vector<float> reusable_intermediate_grid;
const CellLimits limits = grid.limits().cell_limits();
reusable_intermediate_grid.reserve((limits.num_x_cells + max_width - 1) *
limits.num_y_cells);
for (int i = 0; i != options.branch_and_bound_depth(); ++i) {
// 由精到糙生成网格数据
const int width = 1 << i;
precomputation_grids_.emplace_back(grid, limits, width,
&reusable_intermediate_grid);
}
}
// 管理最大值
class SlidingWindowMaximum {
public:
// 将最大值放在最前面,并排序
void AddValue(const float value) {
while (!non_ascending_maxima_.empty() &&
value > non_ascending_maxima_.back()) {
non_ascending_maxima_.pop_back();
}
non_ascending_maxima_.push_back(value);
}
// 判断是否清空最大值,如果该区域内的数据遍历完了,此时最大值会被清掉
void RemoveValue(const float value) {
// DCHECK for performance, since this is done for every value in the
// precomputation grid.
DCHECK(!non_ascending_maxima_.empty());
DCHECK_LE(value, non_ascending_maxima_.front());
if (value == non_ascending_maxima_.front()) {
non_ascending_maxima_.pop_front();
}
}
// 获取最大值
float GetMaximum() const {
// DCHECK for performance, since this is done for every value in the
// precomputation grid.
DCHECK_GT(non_ascending_maxima_.size(), 0);
return non_ascending_maxima_.front();
}
void CheckIsEmpty() const { CHECK_EQ(non_ascending_maxima_.size(), 0); }
private:
// Maximum of the current sliding window at the front. Then the maximum of the
// remaining window that came after this values first occurrence, and so on.
std::deque<float> non_ascending_maxima_;
};
PrecomputationGrid2D::PrecomputationGrid2D(
const Grid2D& grid, const CellLimits& limits, const int width,
std::vector<float>* reusable_intermediate_grid)
// 从左下角xy方向分别扩展-width+1大小,所以offset的xy要分别减去(width - 1)
: offset_(-width + 1, -width + 1),
// 该分辨率地图的网格数量是比原始地图大,因为要采样一个width*width内的最大值,所以每个(x,y)处,都需要计算一个width*width内的最大值
wide_limits_(limits.num_x_cells + width - 1,
limits.num_y_cells + width - 1),
min_score_(1.f - grid.GetMaxCorrespondenceCost()),
max_score_(1.f - grid.GetMinCorrespondenceCost()),
// 同上
cells_(wide_limits_.num_x_cells * wide_limits_.num_y_cells) {
CHECK_GE(width, 1);
CHECK_GE(limits.num_x_cells, 1);
CHECK_GE(limits.num_y_cells, 1);
const int stride = wide_limits_.num_x_cells;
// First we compute the maximum probability for each (x0, y) achieved in the
// span defined by x0 <= x < x0 + width.
std::vector<float>& intermediate = *reusable_intermediate_grid;
intermediate.resize(wide_limits_.num_x_cells * limits.num_y_cells);
// 每一行x0处,都获取该列y的内的最大值
for (int y = 0; y != limits.num_y_cells; ++y) {
SlidingWindowMaximum current_values;
// 在[-width+1, 0]区间内,current_values的最大值的初值要取x = 0处的值
current_values.AddValue(
1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(0, y))));
// 获取[-width+1, 0]内width宽度内的最大值一直放入current_values里
for (int x = -width + 1; x != 0; ++x) {
// 每个(x + width - 1 + y * stride)处的值是从[-width + 1]开始的最大值
intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
if (x + width < limits.num_x_cells) {
current_values.AddValue(1.f - std::abs(grid.GetCorrespondenceCost(
Eigen::Array2i(x + width, y))));
}
}
// 求x=(x,x+width)区间的最大值
for (int x = 0; x < limits.num_x_cells - width; ++x) {
intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
current_values.RemoveValue(
1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(x, y))));
current_values.AddValue(1.f - std::abs(grid.GetCorrespondenceCost(
Eigen::Array2i(x + width, y))));
}
// 求x=(width,limits.num_x_cells)区间的最大值
for (int x = std::max(limits.num_x_cells - width, 0);
x != limits.num_x_cells; ++x) {
intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
current_values.RemoveValue(
1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(x, y))));
}
current_values.CheckIsEmpty();
}
// For each (x, y), we compute the maximum probability in the width x width
// region starting at each (x, y) and precompute the resulting bound on the
// score.
// 再遍历x行,获取y方向上,width范围内的最大值,因此,在cell_里在(x+y*width)处存放的都是width*width范围内的最大值
for (int x = 0; x != wide_limits_.num_x_cells; ++x) {
SlidingWindowMaximum current_values;
current_values.AddValue(intermediate[x]);
for (int y = -width + 1; y != 0; ++y) {
cells_[x + (y + width - 1) * stride] =
ComputeCellValue(current_values.GetMaximum());
if (y + width < limits.num_y_cells) {
current_values.AddValue(intermediate[x + (y + width) * stride]);
}
}
for (int y = 0; y < limits.num_y_cells - width; ++y) {
cells_[x + (y + width - 1) * stride] =
ComputeCellValue(current_values.GetMaximum());
current_values.RemoveValue(intermediate[x + y * stride]);
current_values.AddValue(intermediate[x + (y + width) * stride]);
}
for (int y = std::max(limits.num_y_cells - width, 0);
y != limits.num_y_cells; ++y) {
cells_[x + (y + width - 1) * stride] =
ComputeCellValue(current_values.GetMaximum());
current_values.RemoveValue(intermediate[x + y * stride]);
}
current_values.CheckIsEmpty();
}
}