cartographer 源码中实际上提供grid map两种形式,一种为probability,另一种为tsdf。
grid map
MapLimits limits_; // 地图大小边界,包括x和y最大值, 分辨率, x和yu方向栅格数
std::vector<uint16> correspondence_cost_cells_; //grid 地图存储值,采用uint16类型,
float min_correspondence_cost_; // 对应实际意义的最大和最小边界
float max_correspondence_cost_;
std::vector<int> update_indices_; // 记录已经更新过的索引
Eigen::AlignedBox2i known_cells_box_; // 已知栅格状态的大小
const std::vector<float>* value_to_correspondence_cost_table_; // 此为查询表格,主要因为grid存储的uint16格式,可根据此表格将其对应成实际意义的如probability
GrowLimits用于扩展此gridmap的大小,即当加入一个已知栅格状态时,需要判断并扩展当前gridmap
// 更新地图大小,同时将原grid中数据按照位置放入新放大的grid中
void Grid2D::GrowLimits(const Eigen::Vector2f& point,
const std::vector<std::vector<uint16>*>& grids,
const std::vector<uint16>& grids_unknown_cell_values) {
CHECK(update_indices_.empty());
//如果当前的存在point不在范围内,即需要更新,采用迭代方法放大地图边界,
while (!limits_.Contains(limits_.GetCellIndex(point))) {
//获取原来的地图大小的中心坐标,即栅格索引
const int x_offset = limits_.cell_limits().num_x_cells / 2;
const int y_offset = limits_.cell_limits().num_y_cells / 2;
// grid最大值更新原来的一半, 地图总大小放大一倍。 即从地图中心位置上下左右均放大原大小一半
const MapLimits new_limits(
limits_.resolution(),
limits_.max() +
limits_.resolution() * Eigen::Vector2d(y_offset, x_offset),
CellLimits(2 * limits_.cell_limits().num_x_cells,
2 * limits_.cell_limits().num_y_cells));
const int stride = new_limits.cell_limits().num_x_cells;// 行数,用于转换1维索引
const int offset = x_offset + stride * y_offset;//新的offset
const int new_size = new_limits.cell_limits().num_x_cells *
new_limits.cell_limits().num_y_cells; //新大小
//更新grid概率,即将原来的概率赋值在新的grid中
for (size_t grid_index = 0; grid_index < grids.size(); ++grid_index) {
std::vector<uint16> new_cells(new_size,grids_unknown_cell_values[grid_index]);
for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) {
for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) {
new_cells[offset + j + i * stride] =
(*grids[grid_index])[j + i * limits_.cell_limits().num_x_cells];
}
}
*grids[grid_index] = new_cells;
}
limits_ = new_limits;
// 重新计算有效栅格空间边界,即由于起点发生改变,则矩形框的坐标需进行转换
if (!known_cells_box_.isEmpty()) {
known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset));
}
}
}
下图可以表示这个函数的过程,地图的介绍就到这。匹配时候用的就是Grid2D这个类。
probability map
ProbabilityGrid是继承Grid2D,实际是具体描述了Grid2D单元存储内容的具体意义,即为概率。一个单元格最终想表示的障碍物是否存在,即三个状态:占有(hit),未占有(miss),未知。由于传感器噪声和栅格分辨率的问题,因此状态不会为确定值。因此我们将其以概率形式表示,包括占有概率和未占有概率。
Grid2D基类也提过每个单元存储的为uint16的的整数,因此probability_values.h文件中也将对应概率值转换为1~32767之间,而0表示未知状态。
具体转换公式:value =(float_value - lower_bound) * (32766.f / (upper_bound -ower_bound)) + 1; 取整
//float_value:概率值
upper_bound:最大概率
ower_bound:最小概率
probability map cell 概率栅格图初始化
即设置grid格的probability和获取对应的probability,其代码实现在cartographer/mapping/2d/probability_grid.c中
// 设置对应index的概率值, 在cell单元存储的是对应的free 占有率(uint16整形数)
void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index,
const float probability) {
// cell_index栅格地图中对应索引
// ToFlatIndex 二维坐标转换为1维索引
// mutable_correspondence_cost_cells() 返回的grid中grid地图
uint16& cell = (*mutable_correspondence_cost_cells())[ToFlatIndex(cell_index)];
CHECK_EQ(cell, kUnknownProbabilityValue);
// 将概率值转换对应的整型数,然后存入对应的索引的单元中
cell = CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(probability));
//返回的是有效概率值的矩形框,每更新一个cell,则更新矩形框box
//采用eigen库extend,自动更新有效框
mutable_known_cells_box()->extend(cell_index.matrix());
}
cartographer的grid cell概率实际存储的是free probability--空闲概率。
主要是匹配操作时采用类似最小二乘的优化思想,需要将最佳匹配值改为求取最小状态,因此采取free的概率进行匹配,则最小值则为最佳匹配。
probability grid地图更新
Free状态的概率:p(free)
Occupied状态的概率:p(occ)
p(free)+p(occ)=1
引入两者的比值来作为此cell的状态:
引入观测值Z, Z的状态是hit或是miss.其观测的更新概率可以看做为p(s=occ|z)和p(s=free|z)
即在当前观测z(占hit或misss)的情况下,一个栅格实际为hit的概率和miss的概率,即为条件概率。因此可以用贝叶斯公式进行求取,如下:
现在假设原来cell为odd(s), 而对应的更新odd则为
根据贝叶斯公式进行代入可获取如下公式:
如此可清晰看出更新后的cell应为
其中k-1为上一时刻的odds,而k为经过一次观测后的odds,显然仅是相差了C(z)这个系数
其中系数为
其中p(z|s=occ)表示的是当cell为障碍时,即z=1,传感器认为是障碍的概率,显然是传感器本身属性决定的,因此为一常数。同样其他3种组合也均为常数,因此其系数C(z)是根据观测hit和miss决定的两个常数,即Cmiss 和Chit 两个常数。
简单总结:其更新过程可认为根据传感器观测的状态,对原cell的概率进行更新,假设原cell的值为value。当传感器测到此cell为hit时,则新的value_new=Chit * value; 如果传感器测的此cell为miss时,则新的value_new=Cmiss * value
函数ApplyLookupTable是用来通过查表来更新栅格单元的占用概率的
// 使用查找表对指定栅格进行栅格值的更新
bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index,
const std::vector<uint16>& 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);
// 更新bounding_box
mutable_known_cells_box()->extend(cell_index.matrix());
return true;
}