关于栅格GridMap结构设计的一点思考
1.不同结构体的设计讨论
GridMap常用于lidar-slam建图，泊车freespace跟踪，ros里也有相应接口。最近occ也很火，用的也很多。
一般的，讨论2D GridMap，分辨率为0.1m，长宽各10m，每个格子存储一个int值。
通常的结构体设计可能会有如下
- 稠密的二维数组或一维数组
- 稀疏的map结构
- 压缩的bitmap
其中，稠密图的比较简单，固定大小的栈内存（vector实现为堆内存），查找O(1)。
稀疏图用map或unordered_map实现，底层是红黑树和哈希表，对应查找O(logn)和一般情况下O(1)。动态堆内存。
bitmap内存占用极少，用int的每一位存储，但只能表示0或1，适合极致压缩后的输出结果，需要根据实际应用场景使用。这里暂不做展开。
// 稠密grid
int grid1[100][100] = {0};
int grid2[10000] = {0};
std::array<int, 10000> grid3 = {0};
std::vector<int> grid4(10000, 0);
std::vector<std::vector<int>> grid4_2;
// 稀疏grid
std::map<Index, int> grid5;
std::unordered_map<Index, int, IndexHash> grid6;
// bitmap
const int size = 100 * 100;
std::bitset<size> bit_map1("100001");
std::bitset<size> bit_map2(0);
std::bitset<size> bit_map3(100);
std::cout << bit_map1 << std::endl; // 0000100001
std::cout << bit_map2 << std::endl; // 0000000000
std::cout << bit_map3 << std::endl; // 0001100100
关于稀疏grid的优缺点
- 缺点：一般情况下都是使用稠密结构多一点，关于稀疏结构的实际使用会存在一些问题，比如map的在大量栅格下查找效率不高，而unordered_map的哈希表的内存实际也远远大于红黑树和稠密图，和实际的稀疏减少内存的初衷相违背。
- 优点：但同时带来的优点是不需要考虑key的取值范围。栅格图一般是世界坐标系下的，如果自车的index或者障碍物的index超出规定范围，稠密grid是需要重新映射到另外一个grid中，重新定义一个原点，一般是双层buffer相互交换，减少内存的申请。而稀疏grid的key不需要考虑取值范围是否超过规定size，比较偷懒的做法。虽然不考虑key的取值范围，但是也需要对一些不用的key做删除，防止内存一直扩张。
如果是相同数量的栅格，稀疏结构的内存反而实际是远大于稠密结构的。但正因为是稀疏结构，比如在slam建图应用场景中，如果只需要考虑占据的栅格，不考虑大量的非占据和unknown栅格，那么内存理论上是少于稠密结构的。
另外对于稠密grid：
理论上一维数组模拟的二维数组访问速度更快，但实际性能差异取决于具体的访问模式和编译器优化。在某些情况下，现代编译器能够很好地优化二维数组的访问，使得它们的性能差异不明显。
一维数组在内存中是连续的，这意味着访问一维数组的元素时，缓存命中率更高。二维数组在内存中也是连续存储的，但访问方式涉及更多的偏移计算，二维数组在内存中是连续存储的（按行优先），但由于多维数组的访问涉及更多的计算和潜在的缓存未命中，可能会导致性能稍差。
测试代码有如下：
// 计算 std::unordered_map 的内存使用情况
template<typename K, typename V, typename H>
size_t unordered_map_memory_usage(const std::unordered_map<K, V, H>& um) {
size_t size = sizeof(um); // unordered_map 对象本身的大小
size += um.size() * sizeof(typename std::unordered_map<K, V>::value_type); // 元素的大小
// 估算桶数组的大小
size += um.bucket_count() * sizeof(void*); // 每个桶是一个指针
return size;
}
int main() {
constexpr double size_x = 10.0;
constexpr double size_y = 10.0;
constexpr double resolution = 0.1;
constexpr double inv_resolution = 1 / resolution;
constexpr int large_size_x = static_cast<int>(size_x * inv_resolution);
constexpr int large_size_y = static_cast<int>(size_y * inv_resolution);
// 1. dense grid
int grid1[large_size_x][large_size_y] = {0};
int grid2[large_size_x * large_size_y] = {0};
std::array<int, large_size_x * large_size_y> grid3 = {0};
std::cout << "grid1 size: " << sizeof(grid1) << std::endl; // 40000
std::cout << "grid2 size: " << sizeof(grid2) << std::endl; // 40000
std::cout << "grid3 size: " << sizeof(grid3) << std::endl; // 40000
// 2. sparse gird
using Index = std::pair<int, int>;
struct IndexHash {
size_t operator()(Index const &key) const {
// return std::hash<int64_t>{}(((int64_t)key.first << 32) + (int64_t)key.second);
return std::hash<int>{}(key.first) ^ std::hash<int>{}(key.second);
}
};
std::map<Index, int> grid4;
std::unordered_map<Index, int, IndexHash> grid5;
std::cout << "grid4 init size: " << sizeof(grid4) << std::endl;
std::cout << "grid5 init size: " << sizeof(grid5) << std::endl;
for (int i = 0; i < 50000; ++i){
grid5[Index(i, i)] = 1;
if (grid5.size() > large_size_x * large_size_y){
// todo：改为随机删除
grid5.erase(grid5.begin()->first);
}
// max grid5 size: 202240-10000
std::cout << "grid5 size: " << unordered_map_memory_usage(grid5) << "-" << grid5.size() << std::endl;
}
return 0;
}
那么实际看下来常规的稠密结构，在大量数据应用时，是比较好一点的。唯一的点是需要考虑移动原有grid原点，遍历耗时一点。
那么下面我们用稠密结构一维数组来实现一个可以移动的grid map
2.可移动稠密栅格的实现
需求
建立一个带双buffer一维数组的grid map，实现：
- 初始化固定长宽范围和分辨率的栅格图。
- pos坐标系为前X左Y，pos原点为grid的中心点。index坐标系为右X下Y。
- 通过index或pos获取相应value，超出范围的return false。
- 移动原点复制原有图层相交区域的功能。
- 即有index超出栅格图范围时，可以以该index为原点建立等大的grid，交叠部分的栅格复制到新栅格。
- 双buffer grid实现，调用过程中禁止new新内存。
坐标系转换
三个坐标系：
- 自动驾驶坐标系中的全局global坐标系（前x左y）
- pos坐标系为平移global原点后的坐标系（前x左y）
- index坐标系为grid的坐标系（右x下y），同opencv图像坐标系。
以当前grid中心点为pos原点，对应点O(x0, y0)，那么pos坐标系中任意一点P(x1,y1)，其grid坐标为半个长宽减去对应差值。R为grid的分辨率，WH分别为宽度和高度。
index=(W/2-(x1-x0)/R, H/2-(y1-y0)/R)
那么grid index对应一维数组里的序号为：
arr[index.y * width + index.x]
注意，如果想和index坐标系保持一致，则更换index计算公式即可。根据实际应用场景挑选，这里用第一个。
原点/中心点移动
我们的grid map并不能无限大，都有固定的尺寸。我们可以假设自车在这个grid里运动，快要出尺寸范围时，做一次移动原点操作，让自车回到中心点位置，其看到的障碍物也需要重新移动。或者假设自车一直在grid中心，当自车移动时，移动整个grid，保证自车一直在原点，就能一直更新自车固定范围内的栅格。
为什么不考虑直接使用自车坐标系呢？因为全局坐标系下修改原点只涉及到移动，如果使用自车坐标系，自车转弯时，对应栅格的移动需要平移+旋转，增加了时间复杂度。
将grid从原点O移动到M，使用data和buffer两个grid来固定占用内存。将相交部分的data grid赋值到buffer grid。
建立buffer grid index到data grid index的映射，buffer上的index如果映射到data上，且在尺寸范围内即可赋值过来，然后swap一下，实现交换。即：
data_index_x = x - index_dx;
data_index_y = y - index_dy;
if (!(data_index_x < 0 || data_index_x >= width_ || data_index_y < 0 || data_index_y >= height_)){
buffer_[y * width_ + x] = data_[data_index_y * width_ + data_index_x];
}
对于复制相交部分，如果是小的grid，可以直接遍历赋值；大的grid，可以先求出相交顶点，再遍历相交区域赋值给新的grid。
复现
如下图，建立一个5*5的栅格，分辨率0.1，对角线设置value。以及移动到(0.2, 0.1)后的栅格。
 |  |
注意对double/float栅格化成int时，必须加ceil/floor/round取整，static_cat会进行截断，并非四舍五入或向上向下取整。
static_cat(0.9) = 0
static_cat(-0.9) = 0
grid_map.h
#pragma once
#include <iostream>
#include <vector>
#include <cmath>
struct GridIndex {
GridIndex() : x(0), y(0) {}
GridIndex(int _x, int _y) : x(_x), y(_y) {}
int x = 0;
int y = 0;
};
// index coordinate and position coordinate.
// ------------------------->index x
// | pos x
// | ^
// | |
// | pos y<------|
// |
// index y
class GridMap {
public:
GridMap(int width, int height, double resolution, int default_value = 0);
~GridMap();
inline bool GetIndex(double pos_x, double pos_y, GridIndex &index);
bool GetValue(double pos_x, double pos_y, int &value);
bool GetValue(GridIndex index, int &value);
bool SetValue(double pos_x, double pos_y, int value);
bool SetValue(GridIndex index, int value);
void MoveOrigin(double new_pos_x, double new_pos_y);
void MoveOriginV2(double new_pos_x, double new_pos_y);
inline bool IsValidIndex(int index_x, int index_y);
void Print();
void ToImage();
private:
int width_;
int height_;
double origin_pos_x_;
double origin_pos_y_;
double resolution_;
double inv_resolution_;
int default_value_;
int size_;
int *data_;
int *buffer_;
};
grid_map.cpp
#include "grid_map.h"
#include <iomanip>
GridMap::GridMap(int width, int height, double resolution, int default_value) : width_(width), height_(height),
origin_pos_x_(0), origin_pos_y_(0), resolution_(resolution),
inv_resolution_(1 / resolution), default_value_(default_value), size_(width * height) {
data_ = new int[size_];
buffer_ = new int[size_];
std::fill_n(data_, size_, default_value_);
std::fill_n(buffer_, size_, default_value_);
}
GridMap::~GridMap() {
delete[] data_;
delete[] buffer_;
}
bool GridMap::GetIndex(double pos_x, double pos_y, GridIndex &index) {
// U=W/2 - (y1-y0)
// V=H/2 - (x1-x0)
// ------------------------->index x(width)
// | pos x
// | ^
// | |
// | pos y<------|
// |
// index y(height)
// must have ceil or floor. cast(0.9)=0 cast(-0.9)=0
int index_x = (width_ >> 1) - static_cast<int>(std::ceil((pos_y - origin_pos_y_) * inv_resolution_));
int index_y = (height_ >> 1) - static_cast<int>(std::ceil((pos_x - origin_pos_x_) * inv_resolution_));
if (IsValidIndex(index_x, index_y)) {
index.x = index_x;
index.y = index_y;
return true;
}
return false;
}
bool GridMap::GetValue(double pos_x, double pos_y, int &value) {
GridIndex index;
if (GetIndex(pos_x, pos_y, index)) {
value = data_[index.y * width_ + index.x];
return true;
}
return false;
}
bool GridMap::GetValue(GridIndex index, int &value) {
if (IsValidIndex(index.x, index.y)) {
value = data_[index.y * width_ + index.x];
return true;
}
return false;
}
bool GridMap::SetValue(double pos_x, double pos_y, int value) {
GridIndex index;
if (GetIndex(pos_x, pos_y, index)) {
data_[index.y * width_ + index.x] = value;
return true;
}
return false;
}
bool GridMap::SetValue(GridIndex index, int value) {
if (IsValidIndex(index.x, index.y)) {
data_[index.y * width_ + index.x] = value;
return true;
}
return false;
}
void GridMap::MoveOrigin(double new_pos_x, double new_pos_y) {
int index_dx = std::ceil((new_pos_y - origin_pos_y_) * inv_resolution_);
int index_dy = std::ceil((new_pos_x - origin_pos_x_) * inv_resolution_);
origin_pos_x_ = new_pos_x;
origin_pos_y_ = new_pos_y;
std::fill_n(buffer_, size_, default_value_);
int data_index_x = 0;
int data_index_y = 0;
#pragma omp parallel for
for (int y = 0; y < height_; ++y) {
for (int x = 0; x < width_; ++x) {
// buffer grid index to data grid index
data_index_x = x - index_dx;
data_index_y = y - index_dy;
if (IsValidIndex(data_index_x, data_index_y)) {
buffer_[y * width_ + x] = data_[data_index_y * width_ + data_index_x];
}
}
}
std::swap(buffer_, data_);
}
void GridMap::MoveOriginV2(double new_pos_x, double new_pos_y) {
int index_dx = std::ceil((new_pos_y - origin_pos_y_) * inv_resolution_);
int index_dy = std::ceil((new_pos_x - origin_pos_x_) * inv_resolution_);
int half_width = width_ >> 1;
int half_height = height_ >> 1;
int data_index_x = half_width - index_dx;
int data_index_y = half_height - index_dy;
// calculate intersection area
int x_left = std::max(data_index_x - half_width, 0);
int x_right = std::min(data_index_x + half_width, width_);
int y_top = std::max(data_index_y - half_height, 0);
int y_bottom = std::min(data_index_y + half_height, height_);
bool is_intersected = (x_left <= x_right) && (y_top <= y_bottom);
origin_pos_x_ = new_pos_x;
origin_pos_y_ = new_pos_y;
std::fill_n(buffer_, size_, default_value_);
if (is_intersected) {
printf("[%d %d] [%d %d] \n", x_left, x_right, y_top, y_bottom);
int buffer_index_x = 0;
int buffer_index_y = 0;
#pragma omp parallel for
for (int y = y_top; y <= y_bottom; ++y) {
for (int x = x_left; x <= x_right; ++x) {
// data grid index to buffer grid index
buffer_index_x = x + index_dx;
buffer_index_y = y + index_dy;
buffer_[buffer_index_y * width_ + buffer_index_x] = data_[y * width_ + x];
}
}
}
std::swap(buffer_, data_);
}
void GridMap::Print() {
for (int y = 0; y < height_; ++y) {
for (int x = 0; x < width_; ++x) {
std::cout << std::setw(2) << data_[y * width_ + x] << " ";
}
std::cout << std::endl;
}
std::cout << std::endl;
}
bool GridMap::IsValidIndex(int index_x, int index_y) {
if (index_x < 0 || index_x >= width_ || index_y < 0 || index_y >= height_) {
return false;
}
return true;
}
拓展
如果用opencv的cv::mat来实现则更为简单，只需要加一个映射世界坐标系到图片坐标系的函数即可，原点的移动调用opencv的warpAffine移动接口就行，还能直接可视化。
如果用Eigen::Matrix 来实现则更为高效，矩阵行列求和，移动时使用上/下三角直接赋值。
当然最省事的还是调用现有的grid_map库，封装的Matrix底层实现，也支持cv::mat的相互转换，以及ros可视化，及其他实用接口。https://github.com/ANYbotics/grid_map
扫一扫
1602
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
