1.调用添加约束函数 ComputeConstraint 中（pose_graph_2d）
调用 MaybeAddConstraint MaybeAddGlobalConstraint
if (maybe_add_local_constraint) {
const transform::Rigid2d initial_relative_pose =
optimization_problem_->submap_data()
.at(submap_id)
.global_pose.inverse() *
optimization_problem_->node_data().at(node_id).global_pose_2d;
constraint_builder_.MaybeAddConstraint(
submap_id, submap, node_id, constant_data, initial_relative_pose);
} else if (maybe_add_global_constraint) {
constraint_builder_.MaybeAddGlobalConstraint(submap_id, submap, node_id,
constant_data);
}submap_id, submap, node_id, constant_data, initial_relative_pose
node 与submap 建立约束，submap为submap_id 的数据 ，constant_data node 数据，
initial_relative_pose node 相对与submap位姿
2.约束：
cartographer/cartographer/mapping/internal/constraints/constraint_builder_2d.cc
1.局部约束
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);代码详解
1.initial_relative_pose 相对位姿 大于max_constraint_distance 返回 （POSE_GRAPH设置 max_constraint_distance = 15）
2.约束的类型：
struct Constraint {
struct Pose {
transform::Rigid3d zbar_ij;
double translation_weight;
double rotation_weight;
};
SubmapId submap_id; // 'i' in the paper.
NodeId node_id; // 'j' in the paper.
// Pose of the node 'j' relative to submap 'i'.
Pose pose;
// Differentiates between intra-submap (where node 'j' was inserted into
// submap 'i') and inter-submap constraints (where node 'j' was not inserted
// into submap 'i').
enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
};3.调度扫描匹配器构造
const auto* scan_matcher =
DispatchScanMatcherConstruction(submap_id, submap->grid());返回下类型
struct SubmapScanMatcher {
const Grid2D* grid = nullptr;
std::unique_ptr<scan_matching::FastCorrelativeScanMatcher2D>
fast_correlative_scan_matcher;
std::weak_ptr<common::Task> creation_task_handle;
};4.计算约束 ComputeConstraint
ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */
constant_data, initial_relative_pose, *scan_matcher,
constraint);2.全局约束：
cartographer/cartographer/mapping/internal/constraints/constraint_builder_2d.cc
void ConstraintBuilder2D::MaybeAddGlobalConstraint(
const SubmapId& submap_id, const Submap2D* const submap,
const NodeId& node_id, const TrajectoryNode::Data* const constant_data) 代码详解
1.调度扫描匹配器构造 DispatchScanMatcherConstruction(submap_id, submap->grid());
2.计算约束 ComputeConstraint 初始位姿为： translation_(Vector::Zero()), rotation_(Rotation2D::Identity())
ComputeConstraint(submap_id, submap, node_id, true, /* match_full_submap */
constant_data, transform::Rigid2d::Identity(),
*scan_matcher, constraint);3.计算约束 ComputeConstraint
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)代码详解
1.计算初始位姿 node相对与轨迹的位姿 子图的局部位姿 * node相对于子图位姿
const transform::Rigid2d initial_pose =
ComputeSubmapPose(*submap) * initial_relative_pose;2.定义 float score transform::Rigid2d pose_estimate
3. if (匹配所有子图) if start submap_scan_matcher.fast_correlative_scan_matcher->MatchFullSubmap
if (submap_scan_matcher.fast_correlative_scan_matcher->MatchFullSubmap(
constant_data->filtered_gravity_aligned_point_cloud,
options_.global_localization_min_score(), &score, &pose_estimate) kGlobalConstraintsFoundMetric->Increment();
kGlobalConstraintScoresMetric->Observe(score);
4.匹配局部子图 else submap_scan_matcher.fast_correlative_scan_matcher->Match
if (submap_scan_matcher.fast_correlative_scan_matcher->Match(
initial_pose, constant_data->filtered_gravity_aligned_point_cloud,
options_.min_score(), &score, &pose_estimate)) {
CHECK_GT(score, options_.min_score());
kConstraintsFoundMetric->Increment();
kConstraintScoresMetric->Observe(score);
5.//扫描匹配器分数的直方图。 score_histogram_.Add(score);
6.//使用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);7. 计算 node相对与子图的位姿关系 子图的局部位姿的逆 * 相对与局部坐标系的位姿
const transform::Rigid2d constraint_transform =
ComputeSubmapPose(*submap).inverse() * pose_estimate;4. CSM 匹配 submap_scan_matcher.fast_correlative_scan_matcher
1.匹配子图 Match
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代码详解
1.设置搜索参数
const SearchParameters search_parameters(options_.linear_search_window(),
options_.angular_search_window(),
point_cloud, limits_.resolution());2.用这个参数进行匹配
return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
point_cloud, min_score, score,
pose_estimate);2.匹配所有子图 MatchFullSubmap
bool FastCorrelativeScanMatcher2D::MatchFullSubmap(
const sensor::PointCloud& point_cloud, float min_score, float* score,
transform::Rigid2d* pose_estimate) const代码详解
1.围绕包含它的子图的中心计算搜索整个窗口
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());10 6次，角度180
2. 计算搜索窗口的中点 把这个中点作为搜索的起点
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));3.匹配
MatchWithSearchParameters(search_parameters, center, point_cloud,
min_score, score, pose_estimate);5.根据搜索窗口和初始位置进行scan-match来进行位姿的优化。
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 参数说明：
这个函数是最终的匹配函数 所有的东西都是通过这个函数来进行匹配的。 得到的新的位姿的分数必须大于min_score
这个函数被Match()和MatchFullSubmap()调用
* @param search_parameters 主要设置搜索窗口的大小
* @param initial_pose_estimate 初始的位姿
* @param point_cloud 对应的激光数据
* @param min_score 接受位姿的最小的得分
* @param score 最优位姿的得分
* @param pose_estimate 最优位姿
代码说明：
1. 把激光数据旋转到世界坐标系中的0度的位置
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())));2. 生成一系列的rotated scans GenerateRotatedScans
const std::vector<sensor::PointCloud> rotated_scans =
GenerateRotatedScans(rotated_point_cloud, search_parameters);3. 把上面的rotated scans转换到世界坐标系中 然后转换到地图坐标系中这里之后，所有激光点的坐标走在世界坐标系中了或者说地图坐标系中。这里的离散激光点是在最细的分辨率的地图上面 DiscretizeScans
const std::vector<DiscreteScan2D> 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());4. 计算最低分辨率中的所有的候选解 最低分辨率是通过搜索树的层数、地图的分辨率计算出来的。
对于地图坐标系来说 最低分辨率=1<<h h表示搜索树的总的层数
这里不但对最低分辨率的所有候选解的得分进行了计算 同时还按照从大到小排列 ComputeLowestResolutionCandidates
const std::vector<Candidate2D> lowest_resolution_candidates =
ComputeLowestResolutionCandidates(discrete_scans, search_parameters);5.用分枝定界方法来计算最优的候选解 BranchAndBound
const Candidate2D best_candidate = BranchAndBound(
discrete_scans, search_parameters, lowest_resolution_candidates,
precomputation_grid_stack_->max_depth(), min_score);6.//如果计算出来的解大于最小的阈值 则认为匹配成功，返回对应的位姿
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;
}1.GenerateRotatedScans
std::vector<sensor::PointCloud> GenerateRotatedScans(
const sensor::PointCloud& point_cloud,
const SearchParameters& search_parameters)整体叙述
* 根据搜索参数把激光数据进行多次旋转 相当于在进行compute 2d slice的时候进行最外层的角度循环
* 这里的激光进行转化之后，虽然角度和世界坐标系重合，但是原点依然是不重合的
* @param point_cloud 初始角度为0的点云数据
* @param search_parameters 对应的搜索函数
* @return 经过一系列旋转之后的激光点云
代码详解：
1.定义旋转激光容器
std::vector<sensor::PointCloud> rotated_scans;
rotated_scans.reserve(search_parameters.num_scans);2.搜索的角度的起点 -offset
double delta_theta = -search_parameters.num_angular_perturbations *
search_parameters.angular_perturbation_step_size;3.生成旋转后的激光数据
for (int scan_index = 0; scan_index < search_parameters.num_scans;
++scan_index,
delta_theta += search_parameters.angular_perturbation_step_size) {
rotated_scans.push_back(sensor::TransformPointCloud(
point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(
delta_theta, Eigen::Vector3f::UnitZ()))));
}2.DiscretizeScans
std::vector<DiscreteScan2D> DiscretizeScans(
const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
const Eigen::Translation2f& initial_translation)整体叙述 ：
把一系列的激光数据scans转换到世界坐标系中的坐标(initial_translation)。
因为在进行rotated scans生成的时候，激光数据的角度已经和世界坐标系重合了。
因此这里进行转换的时候只需要进行平移就可以了。
然后把世界坐标系中的物理坐标离散化，转换为地图坐标系的坐标
这个函数返回一些列地图坐标系中的激光扫描数据
经过这个函数之后，各个激光数据角度都和世界坐标系对齐了。
原点都和世界坐标系重合了。
代码详解：
//转换到世界坐标系中
//把世界坐标系中的点转换到地图坐标系中
std::vector<DiscreteScan2D> discrete_scans;
discrete_scans.reserve(scans.size());
for (const sensor::PointCloud& scan : scans) {
discrete_scans.emplace_back();
discrete_scans.back().reserve(scan.size());
for (const sensor::RangefinderPoint& point : scan) {
const Eigen::Vector2f translated_point =
Eigen::Affine2f(initial_translation) * point.position.head<2>();
discrete_scans.back().push_back(
map_limits.GetCellIndex(translated_point));
}
}
return discrete_scans;3.ComputeLowestResolutionCandidates
std::vector<Candidate2D>
FastCorrelativeScanMatcher2D::ComputeLowestResolutionCandidates(
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters) const 整体叙述 ：
计算所有的最低分辨率的可行解 & 计算每个可行解的得分 & 按照从大到小的顺序排列
代码详解：
1.计算最低分辨率的所有的候选解 GenerateLowestResolutionCandidates
std::vector<Candidate2D> lowest_resolution_candidates =
GenerateLowestResolutionCandidates(search_parameters);2.计算每个候选解的分数 注意用来评分的地图是最低分辨率的地图 ScoreCandidates
ScoreCandidates(
precomputation_grid_stack_->Get(precomputation_grid_stack_->max_depth()),
discrete_scans, search_parameters, &lowest_resolution_candidates);3.返回地分辨率候选 return lowest_resolution_candidates;
4.GenerateLowestResolutionCandidates
std::vector<Candidate2D>
FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates(
const SearchParameters& search_parameters) const整体叙述 ：
生成最低分辨率的所有的可行解 这个和一般的计算Candidates的不同在于在计算线性步长的时候要考虑分辨率的影响。
线性步长不再是1 而是2^(h) h从0开始 最低分辨率的解的个数为:(linear_search_window/linear_step_size)^2 * num_scans
代码详解：
1. 计算步长的增量 在最高的分辨率中增量为1 在最低的分辨率中增量为2^(h) h从0开始
const int linear_step_size = 1 << precomputation_grid_stack_->max_depth();2. 计算有多少个可行解 按照计算好的分辨率 x有多少的步长 y有多少的步长
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;
}3.三层for循环 把每一个可行解都存入candidates中
std::vector<Candidate> 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);
}
}
}4.返回 return candidates;
5.ScoreCandidates
void FastCorrelativeScanMatcher2D::ScoreCandidates(
const PrecomputationGrid2D& precomputation_grid,
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters,
std::vector<Candidate2D>* const candidates) const 整体叙述 ：
* 计算每个Candidates的得分 根据传入的地图在这个地图上进行搜索来计算得分
* 这个函数被ComputeLowestResolutionCandidates()调用
* @param precomputation_grid 用来计算分数的地图
* @param discrete_scans 离散化的rotated scans
* @param search_parameters 搜索的参数
* @param candidates 所有的候选解
代码详解：
1.每个候选解 枚举所有的激光点 累计占用概率log-odd
这里都是固定的角度的，因此激光点的坐标就等于激光点在车体坐标系的坐标加上候选解的坐标
这里的xy_index是已经激光雷达原始坐标计算出来的，因此需要加上这个候选解相对于原始位置的偏移
for (Candidate2D& candidate : *candidates) {
int sum = 0;
for (const Eigen::Array2i& xy_index :
discrete_scans[candidate.scan_index]) {2.激光点在地图坐标系中的坐标 precomputation_grid.GetValue(proposed_xy_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);3.求平均并转换为概率
candidate.score = precomputation_grid.ToScore(
sum / static_cast<float>(discrete_scans[candidate.scan_index].size()));4.按照从大到小的顺序排列所有的candidates
6.BranchAndBound
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整体叙述 ：
* @param discrete_scans 离散的旋转激光数据 discretescan rotate scan
* @param search_parameters 搜索窗口的相应的参数
* @param candidates 所有的可行解
* @param candidate_depth 地图的层数(Multi-Level里面有多少个Level) 当前节点的深度 也就是当前节点的地图的层数
* @param min_score 能接受的最小的分数(也可以认为是当前的最优解的得分 凡是比当前最优解低的分数 一律不要)
* 在分枝定界的方法中，一节node只表示一个角度。
* 因此实际构造的束的根节点下面有N个1层子节点，N=rotated scans的数量。
* 然后每个1层的节点下面都是4个子节点
* 在进行更新best_socre的时候，只有叶子节点的得分才能被用来更新best_score。
* best_score体现的是当前的最优解的得分(只有叶子节点才能被当做解)
代码详解：
1. 如果只有一层 那么最低分辨率中最好的就是全局最好的，直接返回
相当于是叶子节点 这个分数会用来更新父节点的best_score。
这个在返回之后 会用来更新bestScore
if (candidate_depth == 0)
{
// Return the best candidate.
return *candidates.begin();
}2.初始化最高分辨率
Candidate best_high_resolution_candidate(0, 0, 0, search_parameters);
best_high_resolution_candidate.score = min_score;3.枚举所有的候选解 从高到低枚举
for (const Candidate& candidate : candidates) //for start
4. 如果某个候选解小于min_score可不需要再进行计算了。这里的min_score相当于当前搜索过的所有解中的最优解的得分
这里相当于定界 如果当前这颗字数的分数小于最优解的分数 则这颗子树可以直接被减枝
因为候选解是按照分数从大到小排列的
在进行迭代的时候，这个min_score是会不断的进行更新的。因为会不断的进行搜索。每次在子节点搜索到更优的解。这个值就会被更新。
min_score只有最最底层的叶子节点的时候，才会进行更新。
if (candidate.score <= min_score)
{
break;
}5. 开始进行分支
std::vector<Candidate> higher_resolution_candidates;
const int half_width = 1 << (candidate_depth - 1);6.该节点分解为四个子节点 这里就是分枝
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;
}
//把这个可能存在更优解的地方放入队列中
higher_resolution_candidates.emplace_back(
candidate.scan_index, candidate.x_index_offset + x_offset,
candidate.y_index_offset + y_offset, search_parameters);
}
}7.计算所有的候选解(4个)的得分
ScoreCandidates(precomputation_grid_stack_->Get(candidate_depth - 1),
discrete_scans, search_parameters,
&higher_resolution_candidates);8. 进行迭代求解最优值 这里相当于传进去最新的best_score来作为子节点的min_score
注意这个best_score相当于这颗子树下面的所有叶子节点的best_score //end for
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));9.return best_high_resolution_candidate;
7.class PrecomputationGridStack2D
该类在构造FastCorrelativeScanMatcher2D类时被构造：
一般会有好几个不同的分辨率的栅格地图。这个类被PrecomputationGridStack里面引用
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_;
};构造函数：
1.最粗的分辨率 这个是由分枝定界的深度决定的。
CHECK_GE(options.branch_and_bound_depth(), 1);
const int max_width = 1 << (options.branch_and_bound_depth() - 1);2.GridStack中地图的个数 即地图分为多少分辨率的
precomputation_grids_.reserve(options.branch_and_bound_depth());
3.可以重复使用的中间栅格
std::vector<float> reusable_intermediate_grid;
const CellLimits limits = probability_grid.limits().cell_limits();
reusable_intermediate_grid.reserve((limits.num_x_cells + max_width - 1) *
limits.num_y_cells); 4.构造各个不同分辨率的栅格地图 width表示不同分辨率的栅格
1表示最细的分辨率 origin_resolutino 2表示2*origin_resolution 4表示4*origin_resolution 8表示8*origin_resolution
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);
}成员函数：
取高度的地图
const PrecomputationGrid2D& Get(int index) {
return precomputation_grids_[index];
}获取最大深度
int max_depth() const { return precomputation_grids_.size() - 1; }8.class PrecomputationGrid2D
这个类表示栅格地图，这个栅格地图是用来加速Multi-Level Resolution来作用的
预先计算的网格，在每个单元格（x0，y0）中包含最大值
由 x0 <= x <x0 +宽度 和 x定义的宽度x宽度区域中的概率 y0 <= y <y0。
构造函数：
PrecomputationGrid2D(const Grid2D& grid, const CellLimits& limits, int width,
std::vector<float>* reusable_intermediate_grid)
:offset_(-width + 1, -width + 1),
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)这个函数就是在原始地图的基础上 生成分辨率位width*origin_resolution的地图
这个函数就是地图分成的最基础的函数 用来生成一系列不同分辨率的地图
* @param probability_grid 对应的概率地图(原始地图)
* @param limits 地图的参数(原始地图的参数 大小 x方向和y方向的cell的数量)
* @param width 地图的宽度(地图是正方形的) 可以认为是地图的分辨率 width*width个原始地图栅格合成一个
* @param reusable_intermediate_grid 可以重复使用的中间栅格 用来计算最大值的一个中间值
代码：
1.地图的一行的栅格数量
const int stride = wide_limits_.num_x_cells;这 个地图的栅格数量在原始地图的基础上增加了width-1个 ，因为原始地图的边界元素也需要做同样的修改
2.保持y的分辨率不变 把x0分割为长度为width的线段 并且求解每个线段的中的最大值
std::vector<float>& intermediate = *reusable_intermediate_grid;3.重新设置大小 大小为地图的大小
intermediate.resize(wide_limits_.num_x_cells * limits.num_y_cells);4.枚举原始地图所有的y 这里是把每一行分为width的段，求解出每个width的段的最大值
for (int y = 0; y != limits.num_y_cells; ++y) {SlidingWindowMaximum 类
// 用来储存一些列数据的结构体，这个结构体可以维护这一些列数据中的最大值。
// 及时对数据进行删除和插入操作 也依然可以得到最大值
// 这个sliding_window是按照从大到小依次排列的，最大的元素位于队首 最小的位于队尾
// 这个滑动窗口主要是用来生成地图的时候 求解一个范围内的所有元素的最大值
current_values 设置0
SlidingWindowMaximum current_values;
current_values.AddValue(
1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(0, y))));求x=(1,width-1)区间的最大值 上面已经加入了0 因此总的东西是(0,width-1)
for (int x = -width + 1; x != 0; ++x)
{
intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
if (x + width < limits.num_x_cells)
{
current_values.AddValue(
probability_grid.GetProbability(Eigen::Array2i(x + width, y)));
}
}
求x=(width,limits.num_x_cells)区间的最大值
for (int x = 0; x < limits.num_x_cells - width; ++x)
{
intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
//求出(x,y)加入(x_width,y) 相当于窗口进行滑动，在x轴进行滑动
current_values.RemoveValue(
probability_grid.GetProbability(Eigen::Array2i(x, y)));
current_values.AddValue(
probability_grid.GetProbability(Eigen::Array2i(x + width, y)));
} 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(
probability_grid.GetProbability(Eigen::Array2i(x, y)));
}5.对应每个(x,y)，我们都需要计算周围width*width的范围对应的栅格的最大值 作为当前的值
// 这里枚举的是要生成的地图的每一个x的栅格值
// 经过上面的计算 intermediate的每一个行元素存储的都是一个width线段的最大值。
// 这里的width线段是当前点i和i+width-1这一段的值
// 下面的功能基本上是一样的，不过因为intermediate已经计算出来了，是width的线段的最大值
// 这里计算的就是width*width里面的最大值
for (int x = 0; x != wide_limits_.num_x_cells; ++x)
{
SlidingWindowMaximum current_values;
current_values.AddValue(intermediate[x]);
//求解第x列的 (0,width-1)的最大值 并且赋值给cells_
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]);
}
}
//窗口开始滑动 每入一个 就出一个 按照y的方向滑动.
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();
}成员函数：
1.GetValue
int GetValue(const Eigen::Array2i& xy_index) const // Returns a value between 0 and 255 to represent probabilities between
// kMinProbability and kMaxProbability.
// 返回对应下标的CellValue
// 可以认为这里传入的index是 概率地图的index
// 要转换成PrecomputationGrid的index的话 要加上offset
// 因为precomputationGrid相比与原始地图来说 要
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];
}2.ToScore 把CellValue转换为占用概率
float ToScore(float value) const {
return min_score_ + value * ((max_score_ - min_score_) / 255.f);
}3.ComputeCellValue 把占用概率转换为CellValue
const int cell_value = common::RoundToInt(
(probability - min_score_) * (255.f / (max_score_ - min_score_)));
CHECK_GE(cell_value, 0);
CHECK_LE(cell_value, 255);
return cell_value;4.成员变量
1.const Eigen::Array2i offset_;
预计算地图 和 概率地图的唯一关系 预计算地图比概率地图来说多了width-1的cells
预计算的栅格地图 相对于 原始地图 的offset
2.const CellLimits wide_limits_; 地图的大小 这个一般是在原始的地图大小上 加入 width
3.std::vector<uint8> cells_; 地图的数据
5056
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
