Tare_planner 学习教程(二)

Tare_planner 学习教程(二)

这一教程主要介绍该算法中比较重要的几个模块的知识，主要是由grid模块引入的几个重要的内容。在三维空间的体素网格地图的表示中，grid是一个3D网格的最小单元，所有的空间位置都需要表示成一系列的grid的组合。同样障碍物信息和三维的显示都也需要用到三维空间网格的表示。所以本教程主要讲述grid的基础功能和用其表示的地图信息。

1. Class Grid的介绍

Class Grid是一个用来表示是体素网格特征的基础类，其内容的成员函数较为容易，但是为了方便后续的开发，代码引入了大量的重构函数和模板函数，可能对大家的阅读源码造成了很大的困难。

1.1 class grid 的私有变量

int 							dimension_;         // 表示体素网格的维度，默认为三维，dimension_ = 3;
int								cell_number_;		// 表示当前体素网格的个数
Eigen::Vector3d 				origin_;			// 表示当前体素网格的起点
Eigen::Vector3i					size_;				// 表示当前体素网格x，y，z的大小
Eigen::Vector3d					resolution_;		// 表示当前体素网格x，y，z的分辨率
Eigen::Vector3d					resolution_inv_;	// 表示当前体素网格x，y，z的分辨率的倒数，相当于一个1m体素网格的数量
std::vector<_T>					cells_;				// 每个体素网格的内容
std::vector<Eigen::Vector3i>	subs_;				// 每个体素网格的序号

1.2 Grid的构造函数

Grid(const Eigen::Vector3i& size, _T init_value, const Eigen::Vector3d& origin= Eigen::Vector3D(0,0,0),
	 const Eigen::Vector3d& resolution = Eigen::Vector3d(1,1,1), int dimension = 3){}

上述构造函数中，初始化了grid的size、origin、resolution和dimension，然后求解了每个维度信息上的resolution_inv、cell_number_,初始化了cells_和subs_。

1.3 Grid的一些基本成员函数介绍

函数名称	函数说明
GetCellNumber()	返回当前体素网格的数量
GetSize()	返回当前体素网格x，y，z的大小
GetOrigin()	返回当前体素网格的起点位置
SetOrigin()	设置当前体素网格的起点位置
SetResolution()	设置当前体素网格的分辨率
GetResolution()	返回当前体素网格的分辨率
GetResolution_inv()	返回当前体素网格的分辨率的倒数

1.4 Grid的一些重构成员函数

函数名称	函数说明
InRange()	返回输入的参数是否在当前的体素网格内
Ind2Sub()	将体素网格的序号转换为体素网格中的位置编号
Sub2Ind()	将体素网格的位置编号转换为体素网格的序号
Sub2Pos()	根据体素网格的位置编号转换为体素网格的位置
Ind2Pos()	根据体素网格的序号转换为体素网格的位置
Pos2Ind()	根据体素网格的位置返回体素网格的序号
Pos2Sub()	根据体素网格的位置返回体素网格的位置编号

1.5 Grid关于cell操作的成员函数

函数名称	函数说明
GetCell()	返回体素网格内的一个最小单元
GetCellValue()	返回体素网格内的一个最小单元的值
SetCellValue()	设置体素网格内的一个最小单元

tips：
其实GetCell()和GetCellValue()返回的值一样，但是GetCell()返回的是引用，可以用来修改Cell的值

1.6 关于Ind2Sub的函数解析

由于是三维体素网格，x，y，z方向上有固定的size，所以先用ind / (size.x() * size.y())得到z轴的位置，然后用ind减去(size.z() * size.x() * size.y())后得到平面的x和y的大小，然后除以size.y()得到y轴的坐标，最后除以size.x()取余数得到x轴的坐标。

2.Class RollingGrid的介绍

RollingGrid类主要处理Grid的翻滚，主要是由于机器人位置和雷达的坐标系不统一带入的该问题，所以这部分主要是完成坐标系的统一。

2.1 Class RollingGrid的私有变量

Eigen::Vector3i 					size_;				// 体素网格的大小
std::unique_ptr<grid_ns::Grid<int>> grid0_;				// 体素网格0
std::unique_ptr<grid_ns::Grid<int>> grid1_;				// 体素网格1
std::vector<int>  					updated_indices_;	// 更新指数
std::vector<int>					array_ind_to_ind_;	// 指数的变换
bool 								which_grid_;		// 使用哪个体素网格的标志

2.2 RollingGrid的构造函数

RollingGrid(const Eigen::Vector3i& size);

RollingGrid的构造函数主要是初始化了array_ind_to_ind_、grid0_、grid1_、size_等变量

2.3 RollingGrid的基本成员函数

函数名称	函数说明
InRange()	返回当前编号是不是在体素网格内
Ind2Sub()	将体素网格的序号转换为体素网格中的位置编号
Sub2Ind()	将体素网格的位置编号转换为体素网格的序号
GetArrayInd()	返回grid的sub的值
GetInd()	返回array_ind_to_ind_的值

2.4 RollingGrid的主要成员函数

函数名称	函数说明
Roll()	完成grid的内向翻转和array_ind_to_ind_的填充
GetUpdatedIndices()	更新了Indices的值，主要是记录翻滚之后的grid哪些ind发生了变化
GetRolledOutIndices()	对于grid进行了外向翻滚，外向翻过是以翻滚线和终点为轴，向右手侧进行翻滚
GetUpdatedArrayIndices()	更新了array_indices的值
GetFromIdx()	返回翻滚后的相应位置的Indices的值
RollHelper()	帮助进行翻滚轴的判断
GetRolledInIndices()	对于grid进行了内向翻滚，外向翻过是以翻滚线和终点为轴，向左手侧进行翻滚
GetRolledOutIndices()	没有相应的定义(暂时不清楚具体的)
GetIndices()	返回了翻滚后Indices的值