计算回环-ConstraintBuilder3D
和ConstraintBuilder2D基本流程一致，通过2个接口：MaybeAddConstraint和MaybeAddGlobalConstraint，然后调用统一接口：ComputeConstraint函数，计算分支定界，然后利用最高得分的解作为初始解，进行CSM位姿解算。
和2D不同的地方主要包括：
（1）分支定界的方法不2D不同：
多分辨率网格的形式不一致；
离散方式不同：先通过直方图匹配的方法计算进行角度离散，再进行距离离散；
分支操作：分支时，考虑深度，每次分支需要计算的子节点变为8个。
（2）CSM需要考虑高低分辨率的匹配。
CSM不同的地方主要体现在：
void ConstraintBuilder3D::ComputeConstraint(
const SubmapId& submap_id, const NodeId& node_id, bool match_full_submap,
const TrajectoryNode::Data* const constant_data,
const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose,
const SubmapScanMatcher& submap_scan_matcher,
std::unique_ptr<Constraint>* constraint) {
// The 'constraint_transform' (submap i <- node j) is computed from:
// - a 'high_resolution_point_cloud' in node j and
// - the initial guess 'initial_pose' (submap i <- node j).
std::unique_ptr<scan_matching::FastCorrelativeScanMatcher3D::Result>
match_result;
// 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();
// 分支定界搜索整个submap,传进去的是submap和node的global rotation及global_localization_min_score
match_result =
submap_scan_matcher.fast_correlative_scan_matcher->MatchFullSubmap(
global_node_pose.rotation(), global_submap_pose.rotation(),
*constant_data, options_.global_localization_min_score());
if (match_result != nullptr) {
CHECK_GT(match_result->score, options_.global_localization_min_score());
CHECK_GE(node_id.trajectory_id, 0);
CHECK_GE(submap_id.trajectory_id, 0);
kGlobalConstraintsFoundMetric->Increment();
kGlobalConstraintScoresMetric->Observe(match_result->score);
kGlobalConstraintRotationalScoresMetric->Observe(
match_result->rotational_score);
kGlobalConstraintLowResolutionScoresMetric->Observe(
match_result->low_resolution_score);
} else {
return;
}
} else {
kConstraintsSearchedMetric->Increment();
// 分支定界搜索部分submap，传进去的是submap和node的global pose，分数为min_score
match_result = submap_scan_matcher.fast_correlative_scan_matcher->Match(
global_node_pose, global_submap_pose, *constant_data,
options_.min_score());
if (match_result != nullptr) {
// We've reported a successful local match.
CHECK_GT(match_result->score, options_.min_score());
kConstraintsFoundMetric->Increment();
kConstraintScoresMetric->Observe(match_result->score);
kConstraintRotationalScoresMetric->Observe(
match_result->rotational_score);
kConstraintLowResolutionScoresMetric->Observe(
match_result->low_resolution_score);
} else {
return;
}
}
{
common::MutexLocker locker(&mutex_);
score_histogram_.Add(match_result->score);
rotational_score_histogram_.Add(match_result->rotational_score);
low_resolution_score_histogram_.Add(match_result->low_resolution_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计算约束，分为高低分辨率的网格和前端计算CSM基本一致
ceres::Solver::Summary unused_summary;
transform::Rigid3d constraint_transform;
ceres_scan_matcher_.Match(match_result->pose_estimate.translation(),
match_result->pose_estimate,
{{&constant_data->high_resolution_point_cloud,
submap_scan_matcher.high_resolution_hybrid_grid},
{&constant_data->low_resolution_point_cloud,
submap_scan_matcher.low_resolution_hybrid_grid}},
&constraint_transform, &unused_summary);
constraint->reset(new Constraint{
submap_id,
node_id,
{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->high_resolution_point_cloud.size()
<< " points on submap " << submap_id << std::fixed;
if (match_full_submap) {
info << " matches";
} else {
// Compute the difference between (submap i <- node j) according to loop
// closure ('constraint_transform') and according to global SLAM state.
const transform::Rigid3d difference = global_node_pose.inverse() *
global_submap_pose *
constraint_transform;
info << " differs by translation " << std::setprecision(2)
<< difference.translation().norm() << " rotation "
<< std::setprecision(3) << transform::GetAngle(difference);
}
info << " with score " << std::setprecision(1) << 100. * match_result->score
<< "%.";
LOG(INFO) << info.str();
}
}
分支定界-FastCorrelativeScanMatcher3D：
和2D一样，有2个入口：MatchFullSubmap和Match，MatchFullSubmap利用默认参数，设置一个较大的窗口，而Match则是构造一个局部的小窗口，且都在最粗糙的网格进行暴力匹配，计算得分。统一调用MatchWithSearchParameters函数，先进行点云离散，生成候选解（通过直方图匹配进行角度离散，再进行xyz方向距离离散），然后进行分支定界。
class PrecomputationGridStack3D {
public:
PrecomputationGridStack3D(
const HybridGrid& hybrid_grid,
const proto::FastCorrelativeScanMatcherOptions3D& options);
const PrecomputationGrid3D& Get(int depth) const {
return precomputation_grids_.at(depth);
}
int max_depth() const { return precomputation_grids_.size() - 1; }
private:
// 存放由精到糙的网格
std::vector<PrecomputationGrid3D> precomputation_grids_;
};
struct DiscreteScan3D;
struct Candidate3D;
struct DiscreteScan3D {
transform::Rigid3f pose;
// Contains a vector of discretized scans for each 'depth'.
std::vector<std::vector<Eigen::Array3i>> cell_indices_per_depth;
float rotational_score;
};
struct Candidate3D {
Candidate3D(const int scan_index, const Eigen::Array3i& offset)
: scan_index(scan_index), offset(offset) {}
static Candidate3D Unsuccessful() {
return Candidate3D(0, Eigen::Array3i::Zero());
}
// Index into the discrete scans vectors.
int scan_index;
// Linear offset from the initial pose in cell indices. For lower resolution
// candidates this is the lowest offset of the 2^depth x 2^depth x 2^depth
// block of possibilities.
Eigen::Array3i offset;
// Score, higher is better.
float score = -std::numeric_limits<float>::infinity();
// Score of the low resolution matcher.
float low_resolution_score = 0.f;
bool operator<(const Candidate3D& other) const { return score < other.score; }
bool operator>(const Candidate3D& other) const { return score > other.score; }
};
// Used to compute scores between 0 and 1 how well the given pose matches.
using MatchingFunction = std::function<float(const transform::Rigid3f&)>;
class FastCorrelativeScanMatcher3D {
public:
struct Result {
float score;
transform::Rigid3d pose_estimate;
float rotational_score;
float low_resolution_score;
};
FastCorrelativeScanMatcher3D(
const HybridGrid& hybrid_grid,
const HybridGrid* low_resolution_hybrid_grid,
const std::vector<TrajectoryNode>& nodes,
const proto::FastCorrelativeScanMatcherOptions3D& options);
~FastCorrelativeScanMatcher3D();
FastCorrelativeScanMatcher3D(const FastCorrelativeScanMatcher3D&) = delete;
FastCorrelativeScanMatcher3D& operator=(const FastCorrelativeScanMatcher3D&) =
delete;
// Aligns the node with the given 'constant_data' within the 'hybrid_grid'
// given 'global_node_pose' and 'global_submap_pose'. 'Result' is only
// returned if a score above 'min_score' (excluding equality) is possible.
std::unique_ptr<Result> Match(const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose,
const TrajectoryNode::Data& constant_data,
float min_score) const;
// Aligns the node with the given 'constant_data' within the 'hybrid_grid'
// given rotations which are expected to be approximately gravity aligned.
// 'Result' is only returned if a score above 'min_score' (excluding equality)
// is possible.
std::unique_ptr<Result> MatchFullSubmap(
const Eigen::Quaterniond& global_node_rotation,
const Eigen::Quaterniond& global_submap_rotation,
const TrajectoryNode::Data& constant_data, float min_score) const;
private:
struct SearchParameters {
const int linear_xy_window_size; // voxels
const int linear_z_window_size; // voxels
const double angular_search_window; // radians
const MatchingFunction* const low_resolution_matcher;
};
std::unique_ptr<Result> MatchWithSearchParameters(
const SearchParameters& search_parameters,
const transform::Rigid3f& global_node_pose,
const transform::Rigid3f& global_submap_pose,
const sensor::PointCloud& point_cloud,
const Eigen::VectorXf& rotational_scan_matcher_histogram,
const Eigen::Quaterniond& gravity_alignment, float min_score) const;
DiscreteScan3D DiscretizeScan(const SearchParameters& search_parameters,
const sensor::PointCloud& point_cloud,
const transform::Rigid3f& pose,
float rotational_score) const;
std::vector<DiscreteScan3D> GenerateDiscreteScans(
const SearchParameters& search_parameters,
const sensor::PointCloud& point_cloud,
const Eigen::VectorXf& rotational_scan_matcher_histogram,
const Eigen::Quaterniond& gravity_alignment,
const transform::Rigid3f& global_node_pose,
const transform::Rigid3f& global_submap_pose) const;
std::vector<Candidate3D> GenerateLowestResolutionCandidates(
const SearchParameters& search_parameters, int num_discrete_scans) const;
void ScoreCandidates(int depth,
const std::vector<DiscreteScan3D>& discrete_scans,
std::vector<Candidate3D>* const candidates) const;
std::vector<Candidate3D> ComputeLowestResolutionCandidates(
const SearchParameters& search_parameters,
const std::vector<DiscreteScan3D>& discrete_scans) const;
Candidate3D BranchAndBound(const SearchParameters& search_parameters,
const std::vector<DiscreteScan3D>& discrete_scans,
const std::vector<Candidate3D>& candidates,
int candidate_depth, float min_score) const;
transform::Rigid3f GetPoseFromCandidate(
const std::vector<DiscreteScan3D>& discrete_scans,
const Candidate3D& candidate) const;
const proto::FastCorrelativeScanMatcherOptions3D options_;
const float resolution_;
const int width_in_voxels_;
std::unique_ptr<PrecomputationGridStack3D> precomputation_grid_stack_;
const HybridGrid* const low_resolution_hybrid_grid_;
RotationalScanMatcher rotational_scan_matcher_;
};
// 构造函数，构造多分辨率网格管理器：precomputation_grid_stack_，构造直方图匹配器rotational_scan_matcher
FastCorrelativeScanMatcher3D::FastCorrelativeScanMatcher3D(
const HybridGrid& hybrid_grid,
const HybridGrid* const low_resolution_hybrid_grid,
const std::vector<TrajectoryNode>& nodes,
const proto::FastCorrelativeScanMatcherOptions3D& options)
: options_(options),
resolution_(hybrid_grid.resolution()),
width_in_voxels_(hybrid_grid.grid_size()),
precomputation_grid_stack_(
common::make_unique<PrecomputationGridStack3D>(hybrid_grid, options)),
low_resolution_hybrid_grid_(low_resolution_hybrid_grid),
rotational_scan_matcher_(HistogramsAtAnglesFromNodes(nodes)) {}
// 入口函数
std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
FastCorrelativeScanMatcher3D::Match(
const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose,
const TrajectoryNode::Data& constant_data, const float min_score) const {
// 构造函数，使用submap的低分辨率网格和node的低分辨率点云，传入pose,暴力计算该pose下，点云和网格的匹配得分
const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher(
low_resolution_hybrid_grid_, &constant_data.low_resolution_point_cloud);
// 构造搜索框
const SearchParameters search_parameters{
common::RoundToInt(options_.linear_xy_search_window() / resolution_),
common::RoundToInt(options_.linear_z_search_window() / resolution_),
options_.angular_search_window(), &low_resolution_matcher};
// 分支定界搜索
return MatchWithSearchParameters(
search_parameters, global_node_pose.cast<float>(),
global_submap_pose.cast<float>(),
constant_data.high_resolution_point_cloud,
constant_data.rotational_scan_matcher_histogram,
constant_data.gravity_alignment, min_score);
}
// 入口函数
std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
FastCorrelativeScanMatcher3D::MatchFullSubmap(
const Eigen::Quaterniond& global_node_rotation,
const Eigen::Quaterniond& global_submap_rotation,
const TrajectoryNode::Data& constant_data, const float min_score) const {
// 计算最远点
float max_point_distance = 0.f;
for (const Eigen::Vector3f& point :
constant_data.high_resolution_point_cloud) {
max_point_distance = std::max(max_point_distance, point.norm());
}
// 构造最大线搜索框
const int linear_window_size =
(width_in_voxels_ + 1) / 2 +
common::RoundToInt(max_point_distance / resolution_ + 0.5f);
// 生成构造函数，使用submap的低分辨率网格和node的低分辨率点云，传入pose,暴力计算该pose下，点云和网格的匹配得分
const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher(
low_resolution_hybrid_grid_, &constant_data.low_resolution_point_cloud);
// 构造搜索框
const SearchParameters search_parameters{
linear_window_size, linear_window_size, M_PI, &low_resolution_matcher};
return MatchWithSearchParameters(
search_parameters,
transform::Rigid3f::Rotation(global_node_rotation.cast<float>()),
transform::Rigid3f::Rotation(global_submap_rotation.cast<float>()),
constant_data.high_resolution_point_cloud,
constant_data.rotational_scan_matcher_histogram,
constant_data.gravity_alignment, min_score);
}
// 返回函数
std::function<float(const transform::Rigid3f&)> CreateLowResolutionMatcher(
const HybridGrid* low_resolution_grid, const sensor::PointCloud* points) {
return [=](const transform::Rigid3f& pose) {
// 计算平均分
float score = 0.f;
for (const Eigen::Vector3f& point :
// 计算位姿转换后的点云
sensor::TransformPointCloud(*points, pose)) {
// TODO(zhengj, whess): Interpolate the Grid to get better score.
// 如上注释所述，应该用插值函数来计算概率，不应该直接取周围网格的概率
score += low_resolution_grid->GetProbability(
low_resolution_grid->GetCellIndex(point));
}
// 暴力得分没有加上权重（和前端的暴力匹配不同）
return score / points->size();
};
}
// 分支定界入口
std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
FastCorrelativeScanMatcher3D::MatchWithSearchParameters(
const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
const transform::Rigid3f& global_node_pose,
const transform::Rigid3f& global_submap_pose,
const sensor::PointCloud& point_cloud,
const Eigen::VectorXf& rotational_scan_matcher_histogram,
const Eigen::Quaterniond& gravity_alignment, const float min_score) const {
// 离散点云
const std::vector<DiscreteScan3D> discrete_scans = GenerateDiscreteScans(
search_parameters, point_cloud, rotational_scan_matcher_histogram,
gravity_alignment, global_node_pose, global_submap_pose);
// 生成低分辨率候选解（打分过的）
const std::vector<Candidate3D> lowest_resolution_candidates =
ComputeLowestResolutionCandidates(search_parameters, discrete_scans);
// 分支定界计算最优解
const Candidate3D best_candidate = BranchAndBound(
search_parameters, discrete_scans, lowest_resolution_candidates,
precomputation_grid_stack_->max_depth(), min_score);
if (best_candidate.score > min_score) {
return common::make_unique<Result>(Result{
best_candidate.score,
GetPoseFromCandidate(discrete_scans, best_candidate).cast<double>(),
discrete_scans[best_candidate.scan_index].rotational_score,
best_candidate.low_resolution_score});
}
return nullptr;
}
std::vector<DiscreteScan3D> FastCorrelativeScanMatcher3D::GenerateDiscreteScans(
const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
const sensor::PointCloud& point_cloud,
const Eigen::VectorXf& rotational_scan_matcher_histogram,
const Eigen::Quaterniond& gravity_alignment,
const transform::Rigid3f& global_node_pose,
const transform::Rigid3f& global_submap_pose) const {
std::vector<DiscreteScan3D> result;
// We set this value to something on the order of resolution to make sure that
// the std::acos() below is defined.
// 计算最远点
float max_scan_range = 3.f * resolution_; // 防止max_scan_range太小
for (const Eigen::Vector3f& point : point_cloud) {
const float range = point.norm();
max_scan_range = std::max(range, max_scan_range);
}
// 和2D一样，计算角度步长
const float kSafetyMargin = 1.f - 1e-2f;
const float angular_step_size =
kSafetyMargin * std::acos(1.f - common::Pow2(resolution_) /
(2.f * common::Pow2(max_scan_range)));
const int angular_window_size = common::RoundToInt(
search_parameters.angular_search_window / angular_step_size);
// 按照步长，分割yaw角（-max,max)
std::vector<float> angles;
for (int rz = -angular_window_size; rz <= angular_window_size; ++rz) {
angles.push_back(rz * angular_step_size);
}
// 计算node和submap之间的相对位姿
const transform::Rigid3f node_to_submap =
global_submap_pose.inverse() * global_node_pose;
// 直方图匹配，通过不断旋转直方图，得到各个角度范围下直方图的匹配得分
const std::vector<float> scores = rotational_scan_matcher_.Match(
rotational_scan_matcher_histogram,
// 要求是重力对齐的，所以需要再去掉重力方向
transform::GetYaw(node_to_submap.rotation() *
gravity_alignment.inverse().cast<float>()),
angles);
for (size_t i = 0; i != angles.size(); ++i) {
// 小于得分的，不是候选解，大于得分的为候选解
if (scores[i] < options_.min_rotational_score()) {
continue;
}
// 角度增量
const Eigen::Vector3f angle_axis(0.f, 0.f, angles[i]);
// It's important to apply the 'angle_axis' rotation between the translation
// and rotation of the 'initial_pose', so that the rotation is around the
// origin of the range data, and yaw is in map frame.
// 约束加上角度增量
const transform::Rigid3f pose(
node_to_submap.translation(),
global_submap_pose.rotation().inverse() *
transform::AngleAxisVectorToRotationQuaternion(angle_axis) *
global_node_pose.rotation());
result.push_back(
// 点云在该pose下离散
DiscretizeScan(search_parameters, point_cloud, pose, scores[i]));
}
return result;
}
DiscreteScan3D FastCorrelativeScanMatcher3D::DiscretizeScan(
const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
const sensor::PointCloud& point_cloud, const transform::Rigid3f& pose,
const float rotational_score) const {
std::vector<std::vector<Eigen::Array3i>> cell_indices_per_depth;
// 获取原始网格
const PrecomputationGrid3D& original_grid =
precomputation_grid_stack_->Get(0);
std::vector<Eigen::Array3i> full_resolution_cell_indices;
for (const Eigen::Vector3f& point :
// 点云按照pose转换
sensor::TransformPointCloud(point_cloud, pose)) {
// 在原始网格下每个点的cell index
full_resolution_cell_indices.push_back(original_grid.GetCellIndex(point));
}
const int full_resolution_depth = std::min(options_.full_resolution_depth(),
options_.branch_and_bound_depth());
CHECK_GE(full_resolution_depth, 1);
// 按照full_resolution_depth深度，生成cell_indices_per_depth，和高分辨地图匹配的数量
for (int i = 0; i != full_resolution_depth; ++i) {
cell_indices_per_depth.push_back(full_resolution_cell_indices);
}
// 和低分辨率地图匹配的数量
const int low_resolution_depth =
options_.branch_and_bound_depth() - full_resolution_depth;
CHECK_GE(low_resolution_depth, 0);
// 计算起点
const Eigen::Array3i search_window_start(
-search_parameters.linear_xy_window_size,
-search_parameters.linear_xy_window_size,
-search_parameters.linear_z_window_size);
// 低分辨率地图每一层的cell index都加进来
for (int i = 0; i != low_resolution_depth; ++i) {
const int reduction_exponent = i + 1;
// 计算window的起点
const Eigen::Array3i low_resolution_search_window_start(
search_window_start[0] >> reduction_exponent,
search_window_start[1] >> reduction_exponent,
search_window_start[2] >> reduction_exponent);
// 为该层添加一个cell index的vector
cell_indices_per_depth.emplace_back();
for (const Eigen::Array3i& cell_index : full_resolution_cell_indices) {
const Eigen::Array3i cell_at_start = cell_index + search_window_start;
// 计算低分辨率的终点
const Eigen::Array3i low_resolution_cell_at_start(
cell_at_start[0] >> reduction_exponent,
cell_at_start[1] >> reduction_exponent,
cell_at_start[2] >> reduction_exponent);
cell_indices_per_depth.back().push_back(
// 在低分辨率网格的cell index
low_resolution_cell_at_start - low_resolution_search_window_start);
}
}
// 返回该pose下，离散点云在各个分辨率地图下的cell index，及得分
return DiscreteScan3D{pose, cell_indices_per_depth, rotational_score};
}
std::vector<Candidate3D>
FastCorrelativeScanMatcher3D::ComputeLowestResolutionCandidates(
const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
const std::vector<DiscreteScan3D>& discrete_scans) const {
// 生成低分辨率候选解
std::vector<Candidate3D> lowest_resolution_candidates =
GenerateLowestResolutionCandidates(search_parameters,
discrete_scans.size());
// 低分辨率候选解打分
ScoreCandidates(precomputation_grid_stack_->max_depth(), discrete_scans,
&lowest_resolution_candidates);
return lowest_resolution_candidates;
}
std::vector<Candidate3D>
FastCorrelativeScanMatcher3D::GenerateLowestResolutionCandidates(
const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
const int num_discrete_scans) const {
// 计算所有可能解的数量
const int linear_step_size = 1 << precomputation_grid_stack_->max_depth();
const int num_lowest_resolution_linear_xy_candidates =
(2 * search_parameters.linear_xy_window_size + linear_step_size) /
linear_step_size;
const int num_lowest_resolution_linear_z_candidates =
(2 * search_parameters.linear_z_window_size + linear_step_size) /
linear_step_size;
const int num_candidates =
num_discrete_scans *
common::Power(num_lowest_resolution_linear_xy_candidates, 2) *
num_lowest_resolution_linear_z_candidates;
std::vector<Candidate3D> candidates;
candidates.reserve(num_candidates);
// 生成所有的候选解
for (int scan_index = 0; scan_index != num_discrete_scans; ++scan_index) {
for (int z = -search_parameters.linear_z_window_size;
z <= search_parameters.linear_z_window_size; z += linear_step_size) {
for (int y = -search_parameters.linear_xy_window_size;
y <= search_parameters.linear_xy_window_size;
y += linear_step_size) {
for (int x = -search_parameters.linear_xy_window_size;
x <= search_parameters.linear_xy_window_size;
x += linear_step_size) {
candidates.emplace_back(scan_index, Eigen::Array3i(x, y, z));
}
}
}
}
CHECK_EQ(candidates.size(), num_candidates);
return candidates;
}
// 打分
void FastCorrelativeScanMatcher3D::ScoreCandidates(
const int depth, const std::vector<DiscreteScan3D>& discrete_scans,
std::vector<Candidate3D>* const candidates) const {
const int reduction_exponent =
std::max(0, depth - options_.full_resolution_depth() + 1);
// 遍历所有的候选解
for (Candidate3D& candidate : *candidates) {
int sum = 0;
// 取出候选帧
const DiscreteScan3D& discrete_scan = discrete_scans[candidate.scan_index];
// 根据深度计算Offset
const Eigen::Array3i offset(candidate.offset[0] >> reduction_exponent,
candidate.offset[1] >> reduction_exponent,
candidate.offset[2] >> reduction_exponent);
CHECK_LT(depth, discrete_scan.cell_indices_per_depth.size());
for (const Eigen::Array3i& cell_index :
// 取出该深度的cell index
discrete_scan.cell_indices_per_depth[depth]) {
// 计算在该深度的低分辨率网格上的位置
const Eigen::Array3i proposed_cell_index = cell_index + offset;
// 计算总分数
sum += precomputation_grid_stack_->Get(depth).value(proposed_cell_index);
}
// 平均分为得分
candidate.score = PrecomputationGrid3D::ToProbability(
sum /
static_cast<float>(discrete_scan.cell_indices_per_depth[depth].size()));
}
// 按照分数从小到大排序
std::sort(candidates->begin(), candidates->end(),
std::greater<Candidate3D>());
}
// 分支定界
Candidate3D FastCorrelativeScanMatcher3D::BranchAndBound(
const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
const std::vector<DiscreteScan3D>& discrete_scans,
const std::vector<Candidate3D>& candidates, const int candidate_depth,
float min_score) const {
if (candidate_depth == 0) {
// 遍历分辨率最高的一层的所有可能解
for (const Candidate3D& candidate : candidates) {
// 分数小于阈值的直接跳过
if (candidate.score <= min_score) {
// Return if the candidate is bad because the following candidate will
// not have better score.
return Candidate3D::Unsuccessful();
}
// 计算点云和submap低分辨率网格的得分
const float low_resolution_score =
(*search_parameters.low_resolution_matcher)(
GetPoseFromCandidate(discrete_scans, candidate));
// 如果当前pose的点云和submap低分辨率网格的匹配得分大于阈值，则结束分支定界
if (low_resolution_score >= options_.min_low_resolution_score()) {
// We found the best candidate that passes the matching function.
Candidate3D best_candidate = candidate;
best_candidate.low_resolution_score = low_resolution_score;
return best_candidate;
}
}
// All candidates have good scores but none passes the matching function.
return Candidate3D::Unsuccessful();
}
Candidate3D best_high_resolution_candidate = Candidate3D::Unsuccessful();
best_high_resolution_candidate.score = min_score;
for (const Candidate3D& candidate : candidates) {
if (candidate.score <= min_score) {
break;
}
// 分支操作
std::vector<Candidate3D> higher_resolution_candidates;
// // 步长减半，进行分支操作（因为网格采样的时候分辨率是除以2的），所有从粗糙层到下一层，其子节点应该有8个
const int half_width = 1 << (candidate_depth - 1);
for (int z : {0, half_width}) {
if (candidate.offset.z() + z > search_parameters.linear_z_window_size) {
break;
}
for (int y : {0, half_width}) {
if (candidate.offset.y() + y >
search_parameters.linear_xy_window_size) {
break;
}
for (int x : {0, half_width}) {
if (candidate.offset.x() + x >
search_parameters.linear_xy_window_size) {
break;
}
higher_resolution_candidates.emplace_back(
candidate.scan_index, candidate.offset + Eigen::Array3i(x, y, z));
}
}
}
// depth -1 并打分
ScoreCandidates(candidate_depth - 1, discrete_scans,
&higher_resolution_candidates);
// 得到最高得分的候选解
best_high_resolution_candidate = std::max(
best_high_resolution_candidate,
BranchAndBound(search_parameters, discrete_scans,
higher_resolution_candidates, candidate_depth - 1,
best_high_resolution_candidate.score));
}
return best_high_resolution_candidate;
}
多分辨率网格构建：
构造：
// unit8:0-255区间
class PrecomputationGrid3D : public HybridGridBase<uint8> {
public:
explicit PrecomputationGrid3D(const float resolution)
: HybridGridBase<uint8>(resolution) {}
// Maps values from [0, 255] to [kMinProbability, kMaxProbability].
static float ToProbability(float value) {
return kMinProbability +
value * ((kMaxProbability - kMinProbability) / 255.f);
}
};
PrecomputationGridStack3D::PrecomputationGridStack3D(
const HybridGrid& hybrid_grid,
const proto::FastCorrelativeScanMatcherOptions3D& options) {
CHECK_GE(options.branch_and_bound_depth(), 1);
CHECK_GE(options.full_resolution_depth(), 1);
precomputation_grids_.reserve(options.branch_and_bound_depth());
// 最高层的网格分辨率和submap的高精度网格分辨率一样
// 把概率值归一化为[0，255]，转化为PrecomputationGrid3D结构
precomputation_grids_.push_back(ConvertToPrecomputationGrid(hybrid_grid));
// 逐层降低分辨率放入管理器中
Eigen::Array3i last_width = Eigen::Array3i::Ones();
for (int depth = 1; depth != options.branch_and_bound_depth(); ++depth) {
const bool half_resolution = depth >= options.full_resolution_depth();
// 如果为true，取原始cell的概率值，否则，取分辨率一半的概率值：CellIndexAtHalfResolution(cell_index)
const Eigen::Array3i next_width = ((1 << depth) * Eigen::Array3i::Ones());
const int full_voxels_per_high_resolution_voxel =
1 << std::max(0, depth - options.full_resolution_depth());
// 计算分辨率增量
const Eigen::Array3i shift = (next_width - last_width +
(full_voxels_per_high_resolution_voxel - 1)) /
full_voxels_per_high_resolution_voxel;
precomputation_grids_.push_back(
// 在上层网格的基础上采样
PrecomputeGrid(precomputation_grids_.back(), half_resolution, shift));
last_width = next_width;
}
}
PrecomputationGrid3D ConvertToPrecomputationGrid(
const HybridGrid& hybrid_grid) {
PrecomputationGrid3D result(hybrid_grid.resolution());
for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) {
// 概率值归一化到0-255之间
const int cell_value = common::RoundToInt(
(ValueToProbability(it.GetValue()) - kMinProbability) *
(255.f / (kMaxProbability - kMinProbability)));
CHECK_GE(cell_value, 0);
CHECK_LE(cell_value, 255);
*result.mutable_value(it.GetCellIndex()) = cell_value;
}
return result;
}
PrecomputationGrid3D PrecomputeGrid(const PrecomputationGrid3D& grid,
const bool half_resolution,
const Eigen::Array3i& shift) {
PrecomputationGrid3D result(grid.resolution());
// 遍历上一层所有网格数据
for (auto it = PrecomputationGrid3D::Iterator(grid); !it.Done(); it.Next()) {
// xyz三个方向分别扩展shift，并且该网格的概率值，取该3个方向上扩展的所有网格的概率最大值
for (int i = 0; i != 8; ++i) {
// We use this value to update 8 values in the resulting grid, at
// position (x - {0, 'shift'}, y - {0, 'shift'}, z - {0, 'shift'}).
// If 'shift' is 2 ** (depth - 1), where depth 0 is the original grid,
// this results in precomputation grids analogous to the 2D case.
const Eigen::Array3i cell_index =
it.GetCellIndex() - shift * PrecomputationGrid3D::GetOctant(i);
// 为true,取原始网格的概率值，为false，取x/2，y/2,z/2的值
auto* const cell_value = result.mutable_value(
half_resolution ? CellIndexAtHalfResolution(cell_index) : cell_index);
// 赋最大值
*cell_value = std::max(it.GetValue(), *cell_value);
}
}
return result;
}
inline int DivideByTwoRoundingTowardsNegativeInfinity(const int value) {
return value >> 1;
}
// xyz方向分别除以2
// Computes the half resolution index corresponding to the full resolution
// 'cell_index'.
Eigen::Array3i CellIndexAtHalfResolution(const Eigen::Array3i& cell_index) {
return Eigen::Array3i(
DivideByTwoRoundingTowardsNegativeInfinity(cell_index[0]),
DivideByTwoRoundingTowardsNegativeInfinity(cell_index[1]),
DivideByTwoRoundingTowardsNegativeInfinity(cell_index[2]));
}
直方图匹配器构造及匹配：
构造：
std::vector<std::pair<Eigen::VectorXf, float>> HistogramsAtAnglesFromNodes(
const std::vector<TrajectoryNode>& nodes) {
std::vector<std::pair<Eigen::VectorXf, float>> histograms_at_angles;
for (const auto& node : nodes) {
histograms_at_angles.emplace_back(
// node的直方图
node.constant_data->rotational_scan_matcher_histogram,
// node的全局yaw角，不带重力对齐的
transform::GetYaw(
node.global_pose *
transform::Rigid3d::Rotation(
node.constant_data->gravity_alignment.inverse())));
}
return histograms_at_angles;
}
RotationalScanMatcher::RotationalScanMatcher(
const std::vector<std::pair<Eigen::VectorXf, float>>& histograms_at_angles)
: histogram_(
Eigen::VectorXf::Zero(histograms_at_angles.at(0).first.size())) {
// submap的直方图为各个node的直方图之和
for (const auto& histogram_at_angle : histograms_at_angles) {
histogram_ +=
RotateHistogram(histogram_at_angle.first, histogram_at_angle.second);
}
}
// 根据角度插值计算新的直方图
Eigen::VectorXf RotateHistogram(const Eigen::VectorXf& histogram,
const float angle) {
const float rotate_by_buckets = -angle * histogram.size() / M_PI;
//本来是四舍五入取整，减去0.5变成向下取整；注意是负数；
int full_buckets = common::RoundToInt(rotate_by_buckets - 0.5f);
//总是一个小于1大于0的数
const float fraction = rotate_by_buckets - full_buckets;
while (full_buckets < 0) {
full_buckets += histogram.size();
}
Eigen::VectorXf rotated_histogram_0 = Eigen::VectorXf::Zero(histogram.size());
Eigen::VectorXf rotated_histogram_1 = Eigen::VectorXf::Zero(histogram.size());
// 当前角度对应的rotate_by_buckets，它位于[full_buckets,full_buckets+1]u区间内，与下界的差值为fraction，所以，分别计算下界对应的直方图，和上界对应的直方图，然后用插值的方法，得到真实角度对应的直方图
for (int i = 0; i != histogram.size(); ++i) {
// 计算下界直方图
rotated_histogram_0[i] = histogram[(i + full_buckets) % histogram.size()];
// 计算上界直方图
rotated_histogram_1[i] =
histogram[(i + 1 + full_buckets) % histogram.size()];
}
// 插值得到真实的直方图
return fraction * rotated_histogram_1 +
(1.f - fraction) * rotated_histogram_0;
}
匹配：
std::vector<float> RotationalScanMatcher::Match(
const Eigen::VectorXf& histogram, const float initial_angle,
const std::vector<float>& angles) const {
std::vector<float> result;
result.reserve(angles.size());
for (const float angle : angles) {
// 按照增大的角度，旋转直方图
const Eigen::VectorXf scan_histogram =
RotateHistogram(histogram, initial_angle + angle);
// 直方图匹配，计算每个角度和当前的submap的直方图之间的得分
result.push_back(MatchHistograms(histogram_, scan_histogram));
}
return result;
}
// 计算两个直方图之间的余弦距离，得到相似性
float MatchHistograms(const Eigen::VectorXf& submap_histogram,
const Eigen::VectorXf& scan_histogram) {
// We compute the dot product of normalized histograms as a measure of
// similarity.
// 归一化
const float scan_histogram_norm = scan_histogram.norm();
const float submap_histogram_norm = submap_histogram.norm();
const float normalization = scan_histogram_norm * submap_histogram_norm;
if (normalization < 1e-3f) {
return 1.f;
}
// 计算点乘，得到两个直方图之间的cos(theta)，即余弦距离
return submap_histogram.dot(scan_histogram) / normalization;
}
626
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
