cartographr之占据格地图
文章目录
- 1, grid_2d
- 2,probability_grid
- 3, probability_values
- １,Probability->CorrespondenceCost
- 2, CorrespondenceCost->probability
- 3, Probability->Odds
- 4, Odds -> Probability
- 5, 几个全局变量，最大，小概率，最大最小CorrespondenceCost
- 6,几个截断概率和Cost的函数
- 7, 初始化和更新用的参数
- ８，将float的概率或者correspondenceCost转为Value uint16限制范围[1, 32767]
- 9, 用于将value转为对应概率或者correspondenceCost的查找表，并且需要提前计算kValueToProbability
- １0查表从uint16得到float值
- 11,(1-4)属于float转换，接下来两个是uint16的转换而且存在一些技巧用于更新值
- 12 ComputeLookupTableToApplyCorrespondenceCostOdds和ComputeLookupTableToApplyOdds两者一个１-p的关系
- 4 example
这里有个 工程主要就是为了简单表示cartographer中占据格地图如何生成以及原理
ProbabilityGrid->Grid2D->MapLimits->CellLimits
1, grid_2d
class Grid2D
{
//成员变量
MapLimits limits_; // 表示２d地图的分辨率，大小
std::vector correspondenct_cost // 对应的值大小为celllimits.num_x_cells * celllimits.num_y_cells
float min_correspondece_cost_;
float max_correspondece_cost_;
std::vector update_indices_;
Eigen::AlignedBox2i known_cells_box_;
// 维护更新时候用的值
protected:
const std::vector& correspondence_cost_cells() const
{
return correspondence_cost_cells_;
}
const std::vector& update_indices() const { return update_indices_; }
const Eigen::AlignedBox2i& known_cells_box() const {return known_cells_box_;}
std::vector* mutable_correspondence_cost_cells() {return &correspondence_cost_cells_;}
std::vector* mutable_update_indices() { return &update_indices_; }
Eigen::AlignedBox2i* mutable_known_cells_box() { return &known_cells_box_; }
// Converts a 'cell_index' into an index into 'cells_'.
int ToFlatIndex(const Eigen::Array2i& cell_index) const;
};
2,probability_grid
注意就是应用查找表更新地图
bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index,
const std::vector& table)
{
DCHECK_EQ(table.size(), kUpdateMarker);
const int flat_index = ToFlatIndex(cell_index);
uint16* cell = &(*mutable_correspondence_cost_cells())[flat_index];
if (*cell >= kUpdateMarker)
{
return false;
}
mutable_update_indices()->push_back(flat_index);
*cell = table[*cell];
DCHECK_GE(*cell, kUpdateMarker);
mutable_known_cells_box()->extend(cell_index.matrix());
return true;
}
3, probability_values
这里面主要维护几个表和有几个转换关于Probability和CorrespondenceCost
１,Probability->CorrespondenceCost
inline float ProbabilityToCorrespondenceCost(const float probability)
{
return 1.f - probability;
}
2, CorrespondenceCost->probability
inline float CorrespondenceCostToProbability(const float correspondence_cost)
{
return 1.f - correspondence_cost;
}
3, Probability->Odds
// odds = p/(1-p)
inline float Odds(float probability)
{
return probability / (1.f - probability);
}
4, Odds -> Probability
// p = odds/(odds+1)
inline float ProbabilityFromOdds(const float odds)
{
return odds / (odds + 1.f);
}
5, 几个全局变量，最大，小概率，最大最小CorrespondenceCost
constexpr float kMinProbability = 0.1f; // min probability
constexpr float kMaxProbability = 1.f - kMinProbability; // max probability
constexpr float kMinCorrespondenceCost = 1.f - kMaxProbability; // minc = 1-maxp
constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability; // maxc = 1-minp
6,几个截断概率和Cost的函数
inline float ClampProbability(const float probability)
{
return Clamp(probability, kMinProbability, kMaxProbability);
}
inline float ClampCorrespondenceCost(const float correspondence_cost)
{
return Clamp(correspondence_cost, kMinCorrespondenceCost,kMaxCorrespondenceCost);
}
7, 初始化和更新用的参数
constexpr uint16 kUnknownProbabilityValue = 0;
constexpr uint16 kUnknownCorrespondenceValue = kUnknownProbabilityValue;
constexpr uint16 kUpdateMarker = 1u << 15; // 32768
８，将float的概率或者correspondenceCost转为Value uint16限制范围[1, 32767]
inline uint16 CorrespondenceCostToValue(const float correspondence_cost)
{
return BoundedFloatToValue(correspondence_cost, kMinCorrespondenceCost, kMaxCorrespondenceCost);
}
//converts a probability to a uint16 in the range[1, 32767] range
inline uint16 ProbabilityToValue(const float probability)
{
return BoundedFloatToValue(probability, kMinProbability, kMaxProbability);
}
9, 用于将value转为对应概率或者correspondenceCost的查找表，并且需要提前计算kValueToProbability
kValueToCorrespondenceCost
extern const std::vector* const kValueToProbability;
extern const std::vector* const kValueToCorrespondenceCost;
const std::vector* const kValueToProbability = PrecomputeValueToProbability().release();
const std::vector* const kValueToCorrespondenceCost = PrecomputeValueToCorrespondenceCost().release();
重点介绍
PrecomputeValueToProbability
PrecomputeValueToCorrespondenceCost
均使用了PrecomputeValueToBoundedFloat函数继续计算用于保留更新和不更新时候的表
计算出结果result为６５５３６个元素，里面对应float值
std::unique_ptr> PrecomputeValueToBoundedFloat(const uint16 unknown_value,
const float unknown_result,
const float lower_bound,
const float upper_bound)
{
auto result = make_unique>();
// Repeat two times, so that both values with and without the update marker
// can be converted to a probability.
for (int repeat = 0; repeat != 2; ++repeat)
{
for (int value = 0; value != 32768; ++value)
{
result->push_back(SlowValueToBoundedFloat(value, unknown_value, unknown_result, lower_bound, upper_bound));
}
}
return result;
}
１0查表从uint16得到float值
// converts a uint16 to a probability in the range[kMinProbability, kMaxProbability]
// loop up table
inline float ValueToProbability(const uint16 value)
{
return (*kValueToProbability)[value];
}
// Converts a uint16 (which may or may not have the update marker set) to a
// correspondence cost in the range [kMinCorrespondenceCost,
// kMaxCorrespondenceCost].
inline float ValueToCorrespondenceCost(const uint16 value)
{
return (*kValueToCorrespondenceCost)[value];
}
11,(1-4)属于float转换，接下来两个是uint16的转换而且存在一些技巧用于更新值
举一个例子，CorrespondenceCostValueToProbabilityValue同理
inline uint16 ProbabilityValueToCorrespondenceCostValue(uint16 probability_value)
{
if (probability_value == kUnknownProbabilityValue)
{
return kUnknownCorrespondenceValue;
}
// ！！！！！！！！！！注意
bool update_carry = false;
if (probability_value > kUpdateMarker) // 概率值大于32768
probability_value -= kUpdateMarker; //
update_carry = true;
}
uint16 result = CorrespondenceCostToValue( // correspondcost->uint16
ProbabilityToCorrespondenceCost( // probability->correspondcost
ValueToProbability(probability_value))); // value->probability
if (update_carry) result += kUpdateMarker;
return result;
}
12 ComputeLookupTableToApplyCorrespondenceCostOdds和ComputeLookupTableToApplyOdds两者一个１-p的关系
std::vector ComputeLookupTableToApplyCorrespondenceCostOdds(float odds)
{
std::vector result;
result.push_back(CorrespondenceCostToValue(
ProbabilityToCorrespondenceCost(
ProbabilityFromOdds(odds))) +
kUpdateMarker);
／／公式３
for (int cell = 1; cell != 32768; ++cell)
{
result.push_back(CorrespondenceCostToValue(
ProbabilityToCorrespondenceCost(ProbabilityFromOdds(
odds * Odds(CorrespondenceCostToProbability((*kValueToCorrespondenceCost)[cell]))))) +
kUpdateMarker);
}
return result;
}
4 example
int main(int argc, char *argv[])
{
//模拟一帧激光数据
RangeData range_data;
range_data.returns.emplace_back(-3.5f, 0.5f, 0.f);
range_data.returns.emplace_back(-2.5f, 1.5f, 0.f);
range_data.returns.emplace_back(-1.5f, 2.5f, 0.f);
range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
range_data.returns.emplace_back(-13.5f, 1.5f, 0.f);
range_data.returns.emplace_back(-20.5f, 1.5f, 0.f);
range_data.returns.emplace_back(-1.5f, 9.5f, 0.f);
range_data.returns.emplace_back(-5.5f, 7.5f, 0.f);
range_data.returns.emplace_back(30.5f, 20.5f, 0.f);
range_data.returns.emplace_back(200.5f, 100.5f, 0.f);
range_data.returns.emplace_back(1.5f, 2.5f, 0.f);
range_data.returns.emplace_back(0.5f, 3.5f, 0.f);
range_data.returns.emplace_back(13.5f, 1.5f, 0.f);
range_data.returns.emplace_back(20.5f, 1.5f, 0.f);
range_data.returns.emplace_back(1.5f, 9.5f, 0.f);
range_data.returns.emplace_back(5.5f, 7.5f, 0.f);
range_data.returns.emplace_back(30.5f, 20.5f, 0.f);
range_data.returns.emplace_back(200.5f, 100.5f, 0.f);
range_data.returns.emplace_back(100.5f, 200.5f, 0.f);
range_data.returns.emplace_back(60.5f, 30.5f, 0.f);
range_data.returns.emplace_back(13.5f, 15.f, 0.f);
range_data.returns.emplace_back(20.5f, 150.f, 0.f);
range_data.returns.emplace_back(5.f, 49.5f, 0.f);
range_data.returns.emplace_back(56.5f, 17.5f, 0.f);
range_data.origin.x() = 70.5f;
range_data.origin.y() = 72.5f;
ProbabilityGrid grid(MapLimits(1., Eigen::Vector2d(150., 150.), CellLimits(150, 150))); ／／定义一个ｍａｐ
//将这帧激光数据插入地图中
Insert(range_data, &grid);
constexpr int kUnKnown = 128;
const CellLimits& celllimits = grid.limits().cell_limits();
const int width = celllimits.num_x_cells;
const int height = celllimits.num_y_cells;
cv::Mat image = cv::Mat(width, height, CV_8UC3);
for(const Eigen::Array2i& xy_index: XYIndexRangeIterator(grid.limits().cell_limits()))
{
CHECK(grid.limits().Contains(xy_index));
const unsigned char value = grid.IsKnown(xy_index)? RoundToInt((1-grid.GetProbability(xy_index))*255+0):kUnKnown;
image.at(xy_index.x(), xy_index.y()) = cv::Vec3b(value, value, value);
}
cv::imshow("image", image);
cv::imwrite("image.png", image);
cv::waitKey(0);
return 1;
}
Insert函数， 两个表维护hit和miss
void Insert(const RangeData& range_data, ProbabilityGrid* const probability_grid)
{
const std::vector hit_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
Odds(0.9)));
const std::vector miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
Odds(0.1)));
// 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).
CastRays(range_data, hit_table_, miss_table_, true,
CHECK_NOTNULL(probability_grid));
probability_grid->FinishUpdate();
}