cartographer的栅格地图更新
栅格地图的更新就是把激光雷达数据插入到栅格地图里面
首先找到插入激光雷达到栅格地图的位置，
//Local_trajectory_builder_2d.cc
LocalTrajectoryBuilder2D::AddRangeData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& unsynchronized_data) {
.......
return AddAccumulatedRangeData(
time,
// 将点云变换到local原点处, 且姿态为0
//就是把点云转到local原点处，且把点云
//把激光点云转到base_link原点处，也就是从sensor_bridge传入的状态
//然后用预测的姿态，把点云转到该姿态下
TransformToGravityAlignedFrameAndFilter(
gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
accumulated_range_data_),
gravity_alignment, sensor_duration);
}
Local_trajectory_builder_2d.cc中调用InsertIntoSubmap（）函数，将激光雷达数据插入到submap
// 将校正后的雷达数据写入submap
//Local_trajectory_builder_2d.cc
std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
time, range_data_in_local, filtered_gravity_aligned_point_cloud,
pose_estimate, gravity_alignment.rotation());
InsertIntoSubmap（）函数定义在Local_trajectory_builder_2d.cc中的LocalTrajectoryBuilder2D类，该函数是LocalTrajectoryBuilder2D与2D栅格地图的桥梁
std::unique_ptr<InsertionResult> InsertIntoSubmap(
common::Time time, const sensor::RangeData& range_data_in_local,
const sensor::PointCloud& filtered_gravity_aligned_point_cloud,
const transform::Rigid3d& pose_estimate,
const Eigen::Quaterniond& gravity_alignment);
InsertIntoSubmap（）具体实现在Local_trajectory_builder_2d.cc 中，核心功能是调用了InsertRangeData（）函数
//Local_trajectory_builder_2d.cc
// 将点云数据写入到submap中
//这个点云已经在local坐标系下 submaps的原点就是local坐标系的原点
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
active_submaps_.InsertRangeData(range_data_in_local);
InsertRangeData（）函数具体实现在sumap_2d.cc，核心功能是调用了 submap->InsertRangeData(range_data, range_data_inserter_.get());
//sumap_2d.cc
// 将点云数据写入到submap中
std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::InsertRangeData(
const sensor::RangeData& range_data) {
// 如果第二个子图插入节点的数据等于num_range_data时,就新建个子图
// 因为这时第一个子图应该已经处于完成状态了
if (submaps_.empty() ||
submaps_.back()->num_range_data() == options_.num_range_data()) {
AddSubmap(range_data.origin.head<2>());
}
// 将一帧雷达数据同时写入两个子图中
for (auto& submap : submaps_) {
submap->InsertRangeData(range_data, range_data_inserter_.get());
}
// 第一个子图的节点数量等于2倍的num_range_data时,第二个子图节点数量应该等于num_range_data
if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) {
submaps_.front()->Finish();
}
return submaps();
}
submap与submaps_类型一致，submaps_定义在sumap_2d.cc中 的ActiveSubmaps2D类中，
//sumap_2d.cc
std::vector<std::shared_ptr<Submap2D>> submaps_;
同时在ActiveSubmaps2D类中，定义了InsertRangeData（）函数，
//sumap_2d.cc
std::vector<std::shared_ptr<const Submap2D>> InsertRangeData(
const sensor::RangeData& range_data);
InsertRangeData（）函数，具体实现，核心的是 range_data_inserter->Insert（）函数
//sumap_2d.cc
// 将雷达数据写到栅格地图中
void Submap2D::InsertRangeData(
const sensor::RangeData& range_data,
const RangeDataInserterInterface* range_data_inserter) {
CHECK(grid_);
CHECK(!insertion_finished());
// 将雷达数据写到栅格地图中
range_data_inserter->Insert(range_data, grid_.get());
// 插入到地图中的雷达数据的个数加1
set_num_range_data(num_range_data() + 1);
}
跳转到Insert（）函数的具体实现，在probability_grid_range_data_inserter_2d.cc中，在ProbabilityGridRangeDataInserter2D类中定义
//probability_grid_range_data_inserter_2d.cc
void ProbabilityGridRangeDataInserter2D::Insert(
const sensor::RangeData& range_data, GridInterface* const grid) const {
ProbabilityGrid* const probability_grid = static_cast<ProbabilityGrid*>(grid);
CHECK(probability_grid != nullptr);
// By not finishing the update after hits are inserted, we give hits priority
// (i.e. no hits will be ignored because of a miss in the same cell).
// param: insert_free_space
CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(),
probability_grid);
probability_grid->FinishUpdate();
}
分析一下ProbabilityGridRangeDataInserter2D类，ProbabilityGridRangeDataInserter2D继承RangeDataInserterInterface 类
//probability_grid_range_data_inserter_2d.h
class ProbabilityGridRangeDataInserter2D : public RangeDataInserterInterface {
public:
explicit ProbabilityGridRangeDataInserter2D(
const proto::ProbabilityGridRangeDataInserterOptions2D& options);
ProbabilityGridRangeDataInserter2D(
const ProbabilityGridRangeDataInserter2D&) = delete;
ProbabilityGridRangeDataInserter2D& operator=(
const ProbabilityGridRangeDataInserter2D&) = delete;
// Inserts 'range_data' into 'probability_grid'.
virtual void Insert(const sensor::RangeData& range_data,
GridInterface* grid) const override;
private:
const proto::ProbabilityGridRangeDataInserterOptions2D options_;
const std::vector<uint16> hit_table_;
const std::vector<uint16> miss_table_;
};
分析一下ProbabilityGridRangeDataInserter2D类的构造函数，
主要是创建查找表hit_table_与miss_table_，调用了ComputeLookupTableToApplyCorrespondenceCostOdds（）函数
//probability_grid_range_data_inserter_2d.cc
// 写入器的构造, 新建了2个查找表
ProbabilityGridRangeDataInserter2D::ProbabilityGridRangeDataInserter2D(
const proto::ProbabilityGridRangeDataInserterOptions2D& options)
: options_(options),
// 生成更新占用栅格时的查找表 // param: hit_probability
hit_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
Odds(options.hit_probability()))), // 0.55
// 生成更新空闲栅格时的查找表 // param: miss_probability
miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
Odds(options.miss_probability()))) {} // 0.49
ComputeLookupTableToApplyCorrespondenceCostOdds（）函数在probability_values.cc中
该函数返回更新查找表，在分析ComputeLookupTableToApplyCorrespondenceCostOdds（）函数之前，先分析一下Odds（）函数，Odds（）函数定义在probability_values.h中，公式
cell被占用概率越大，odds越大，因为p取值大于0.5，所以odds是大于1
options.hit_probability()为0.55传进来的话， 返回 0.55 / 1-0.55
通过函数Odds转换为更新系数Chit和Cmiss。hit_probability的设置不能小于0.5，miss_probability的设置不能大于0.5
代码如下
//probability_values.h
// 概率值转Odds
inline float Odds(float probability) {
return probability / (1.f - probability);
}
因为odds是大于1，所以Mnew（x）越来越增加
通过odd值转概率，
//probability_values.h
// odd值转概率
inline float ProbabilityFromOdds(const float odds) {
return odds / (odds + 1.f);
}
小结：使用options.hit_probability()大于0.5，对cell进行更新，则cell对应的占用概率值越来越大
使用options.miss_probability()小于0.5，对cell进行更新，则cell对应的占用概率值越来越小
回到ComputeLookupTableToApplyCorrespondenceCostOdds（）函数
//probability_values.cc
// 将栅格是未知状态与odds状态下, 将更新时的所有可能结果预先计算出来
std::vector<uint16> ComputeLookupTableToApplyCorrespondenceCostOdds(
float odds) {
std::vector<uint16> result;
result.reserve(kValueCount); // 32768
// 当前cell是unknown情况 直接把odds转成value存进来
result.push_back(CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(
ProbabilityFromOdds(odds))) +
kUpdateMarker); // 加上kUpdateMarker作为一个标志, 代表这个栅格已经被更新了
// 计算更新时 从1到32768的所有可能的 更新后的结果
for (int cell = 1; cell != kValueCount; ++cell) {
result.push_back(
CorrespondenceCostToValue(
ProbabilityToCorrespondenceCost(ProbabilityFromOdds(
odds * Odds(CorrespondenceCostToProbability(
(*kValueToCorrespondenceCost)[cell]))))) +
kUpdateMarker);
}
return result;
}
接下来一点点分析
创建一个result转换表
预留32768个空间
ProbabilityFromOdds(odds) odds 转成概率值， 也就是options.hit_probability()为0.55传进来的话，odd值为0.55/1-0.55， 该函数返回0.55
//probability_values.h
// odd值转概率
inline float ProbabilityFromOdds(const float odds) {
return odds / (odds + 1.f);
}
取反 hit_probability()为0.55，返回0.45
//probability_values.h
// probability与CorrespondenceCost的关系, CorrespondenceCost代表free的概率 hit_probability()为0.55，返回0.45
inline float ProbabilityToCorrespondenceCost(const float probability) {
return 1.f - probability;
}
CorrespondenceCostToValue（）函数，根据概率值去查表，比如 概率0.45去查表得到 369，[0. 1 ~ 0.9 ] 对应 [1, 32767]
//probability_values.h
// 将浮点数correspondence_cost转成[1, 32767]范围内的 uint16 整数
inline uint16 CorrespondenceCostToValue(const float correspondence_cost) {
return BoundedFloatToValue(correspondence_cost, kMinCorrespondenceCost,
kMaxCorrespondenceCost);
}
在probability_values.h中BoundedFloatToValue（）函数，该函数就是根据概率值去查表的具体实现， 比如 概率0.45去查表得到 369（不一定对，就是那个意思），value值加上kUpdateMarker放到result
//probability_values.h
// 为了避免浮点运算, 将[0~0.9]的浮点数转成[1~32767]之间的值
inline uint16 BoundedFloatToValue(const float float_value,
const float lower_bound,
const float upper_bound) {
const int value =
common::RoundToInt(
(common::Clamp(float_value, lower_bound, upper_bound) - lower_bound) *
(32766.f / (upper_bound - lower_bound))) +
1;
// DCHECK for performance.
DCHECK_GE(value, 1);
DCHECK_LE(value, 32767);
return value;
}
分析for循环
//probability_values.cc
// 计算更新时 从1到32768的所有可能的 更新后的结果
for (int cell = 1; cell != kValueCount; ++cell) {
result.push_back(
CorrespondenceCostToValue(
ProbabilityToCorrespondenceCost(ProbabilityFromOdds(
odds * Odds(CorrespondenceCostToProbability(
(*kValueToCorrespondenceCost)[cell]))))) +
kUpdateMarker);
}
调用CorrespondenceCostToProbability（）
//probability_values.h
inline float CorrespondenceCostToProbability(const float correspondence_cost) {
return 1.f - correspondence_cost;
}
此处应该详细一点的，后面更新
回到Insert（），insert()是虚函数，且override， RangeDataInserterInterface* range_data_inserter这里面实现了多态，父类的指针指向子类的对象
static_cast强制类型转换GridInterface* const grid转成ProbabilityGrid*
//probability_grid_range_data_inserter_2d.cc
void ProbabilityGridRangeDataInserter2D::Insert(
const sensor::RangeData& range_data, GridInterface* const grid) const {
ProbabilityGrid* const probability_grid = static_cast<ProbabilityGrid*>(grid);
CHECK(probability_grid != nullptr);
// By not finishing the update after hits are inserted, we give hits priority
// (i.e. no hits will be ignored because of a miss in the same cell).
// param: insert_free_space
CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(),
probability_grid);
probability_grid->FinishUpdate();
}
调用函数CastRays（），本篇博客太长了，留在下一篇继续讲解
欢迎大家一起学习交流~
每天进步一点点，时光不负有心人❤
1756
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
