VoxelMap(LIO)原理介绍

一、VoxelMap

VoxelMap是一种紧耦合融合的LIO里程计，本文档旨在介绍其核心创新点地图维护部分以及公式推导部分；

二、整体方案

激光惯性里程计通常将历史点云信息按照KD-Tree进行维护，前面两节也介绍了KD-Tree和体素图的计算方式，KD-Tree可以实现O(logn)级别的KNN搜索。在搜索到足够的最近点之后，进行平面/线拟合，然后计算点面残差，这其中平面拟合相对还是比较耗时，并且KD-tree的KNN搜索也比较耗时。Voxel map地图更新方案如下图所示

VoxelMap维护体素地图，每个体素地图内部维护八叉树，深度一般为3，根据实际情况自行调整，八叉树节点为平面信息。具体为：

• 体素图的key是世界坐标系下的体素ID,value是体素内信息，用八叉树进行表示；八叉树内存着点云的参数化表示，我们这里主要是平面信息，平面包含质心位置q以及法向量n 共6个参数（注：可以优化使用最近点表示，只有3dof,可以降低地图存储），这就意味着根据位置检索到体素图就可以获取到参数化的平面信息。
• 此外，当体素无法成功拟合平面，需要进行八叉树分割，将大体素分成很多小的体素，这里限制最大深度为3，当然，如果可以拟合平面，就不需要进行分割了。（注：这里也有优化空间，假设体素是3m*3m,那么不同大体素可能维护相同平面，其实可以对多个大体素进行合并，进一步减小地图存储空间，同时可以提高大场景平面精度）
• 论文中的地图方案相对于KD-tree的方案计算耗时更低，整体耗时可以降低30%，CPU占用和内存占用也会大幅下降（30%和50%）

个人认为VoxelMap核心内容分为两部分，一部分是如何维护自适应体素地图，一部分是其理论推导，尤其是平面法向量置信度推导，本文对这两部分进行详细介绍。

三、地图维护方案

VoxelMap维护的可以理解为一个全局地图，如果不进行地图删减，那么地图当中是包含全局信息的。

1.地图初始化

在地图初次建立时进行调用，涉及体素地图和八叉树地图的初始化。

a) 体素图初始化

其中VoxelLoc是点云投影在体素中的键值，OctoTree指的是当前体素“键”对应的“值”。

算法1-1 体素地图初始化

输入：世界坐标系下的去畸变点云pvList,其他初始化参数
输出：面体素地图PlaneVoxelMap

for all pv 属于 pvList do{
	VoxelLoc = round(pv/maxVoxelSize)
	if(pv是面特征){
		if（VoxelLoc属于PlaneVoxelMap.key）
			将点缓存在对应的OctoTree的根节点TempPoint中
		else
			创建新的Octotree并载入参数
	}else{
		Error：输入异常特征
	}
}
for（ all PlaneOctoTree属于PlaneVoxelMap.values）{
	调用1.2进行八叉树PlaneOctoTree初始化
}

b) 八叉树初始化

算法1-2 八叉树初始化

输入：八叉树点云类型（面or线，此处只有面）,PlaneVoxelMap.values点云
输出：八叉树根节点

if（点数 > maxPlaneUpdateThreshold）{
	调用算法4-1进行平面拟合
	if（成功拟合平面）{
		达到树底，不调用1-3所示的节点分割
		if（点数 > maxPointsSize）{
			updateEnable = False,这个体素不再更新平面
			释放TempPoint的缓存点云
		}
	}else{
		调用算法1-3进行OctoTree分割
		直到成功生成平面or达到maxLayer
	}
}
return RootNode;

c) 八叉树分割算法

当父节点调用算法4-1（或者4-2）拟合平面(线)失败，且父节点尚未到达底层时，需要进行父节点分割。节点分割会将父体素分割成8个子体素，子体素的信息会保存在八叉树的叶子节点LeafNodeArr中，当叶子节点也拟合失败，会递归调用知道到大最大深度。

算法1-3 八叉树分割

输入：根节点RootNode
输出：子节点数组LeafNodeArr[8]

获得RootNode中的点云数据纪委pvList
if(到达底层){
	不再进行节点分割
	return nullptr;
}else{
	for(auto& pv: pvList){
		根据体素中心center以及pv,判断八叉树节点索引i
		载入pv至LeafNodeArr[i]
	}
}
for(auto& LeafNode:LeafNodeArr){
	if(点数 > maxPlaneUpdateThreshold){
		调用4-1进行平面拟合；
		if（拟合成功）
			continue;
		else
			递归调用1-3,对LeafNode再次进行OctoTree分割
	}
}
return LeafNodeArr[8];

2. 地图更新

地图创建成功之后，需要使用新的激光scan对地图点进行更新，使其能表示新区域的信息。

a) 体素地图更新

将点云放到对应的体素，然后调用2-2更新八叉树，所有点都更新之后，就完成了体素地图的更新。

算法2-1 体素图更新

输入：世界坐标系下的去畸变点云pvList,其他初始化参数
输出：面体素地图PlaneVoxelMap

for all pv 属于 pvList do{
	VoxelLoc = round(pv/maxVoxelSize)
	
	if（VoxelLoc属于PlaneVoxelMap.key）
		调用2-2进行OctoTree更新
	else
		创建Octotree并载入参数
	
}
return PlaneVoxelMap；

b）八叉树更新

算法2-2 八叉树更新

输入：单个扫描点pv, 八叉树根节点RootNode
输出：八叉树根节点

pv载入RootNode的tempPoint
if(进行RootNode尚未初始化){
	if（点数 > maxPlaneUpdateThreshold）{
		调用算法1-2进行RootNode初始化
	}
}else{
	if（当前节点已成功拟合平面）{
		if(可以进行平面更新）{
			if(newPointSize > updateSizeThreshold){
				调用算法4-1或者4-2重新拟合平面or直线
				newPointSize = 0	
			}
			if(totalPointSize > maPointsSize){
				不再拟合并释放资源tempPoint
			}
		}
	}else{
		根据体素中心center以及pv,判断八叉树节点索引i
		递归调用2-2,进行LeafNode的更新直到最底层
	}
}

return RootNode;

3.点云配准及残差计算

根据Hash找到当前点云对应体素，如果体素拟合平面成功，返回点到平面距离等观测量和计算的不确定度；如果当前大体素没有拟合平面成功，则需要进入到八叉树每个叶子节点计算一遍残差，最终选择残差最小的返回，如果都没有找到平面，那就在相邻体素中计算。

a) 点云配准

算法3-1 点云配准

输入：世界坐标系下去畸变点云pvList
输出：观测量MatchingList(残差，不确定度)

for(auto& pv: pvList){
	VoxelMap = PlaneVoxelMap
	VoxelLoc = round(pv/maxVoxelSize);
	if(VoxelLoc 属于VoxelMap.key）{
		调用算法3-2进行单个观测量MatchingValue计算
		if（MatchingValue计算失败）
			使用最近邻体素，调用算法3-2进行观测量MatchingValue计算
		else
			将MatchingValue并入MatchingList尾部
	}
}
return MatchingList;

b) 单个观测量计算

算法3-2 单个观测量MatchingValue计算

输入：点云在世界坐标系位置pv, 八叉树根节点RootNode
输出：观测量MatchingList(残差，不确定度)

将pv向量化，获取本节点法向量n和质心q
pq = pv -q;
if(当前节点已成功拟合平面){
	计算点到平面的距离d = n*qp
	缓存法向量、质心、距离至MatchingValue
}else{
	进入“所有”LeafNode递归调用算法3-2进行观测量计算
	选择LeafNode中距离d最小的MatchingValue返回
}

return MatchingValue;

4、平面的拟合

前面已经介绍过标准的平面拟合，这里是VoxelMap用的平面拟合方法

算法4-1 平面拟合

输入：点云pointsVec
输出：平面参数

计算质心q
计算点云pointsVec协方差Cov
对Cov进行特征分解L_min, L_middle, L_max，三个特征向量对应点云的正交分解
选取最小特征值对应特征向量作为法向量n
if（L_min < planeThreshold）{
	plane = [n q]
}else{
	平面拟合失败
}

return plane;

四、理论推导

1、Local系下点云${}^L{p}_i$位置协方差

激光雷达通过扫描电机的编码器测量方向 $w$，通过计算激光飞行时间来测量深度$d$ 。这两部分相互独立，最后相乘得到点云在惯性系下的坐标${}^L{p}_i=d_i\ast w_i$。因此可以对这他们分开建立误差模型。

• 测距距离建模相对比较简单。假设$d_i$是深度测量值并且深度误差${\delta }_{d_i}\sim N(0,{\sum }_{d_i})$，那么测距真值可通过简单线性相加得到$d_i^{gt}=d_i+{\delta }_{d_i}$。

• 测量方向建模涉及S2流形。假设$w_i$是方向测量值并且为${\delta }_{w_i}\sim N(0,{\sum }_{w_i})$测量量$w_i$在切平面下的方向误差，那么方向真值可以通过SO2上的加法实现，其中$N(w_i{)}_{3\ast 2}$为$w_i$切平面下的标准正交基。

• 

• 
  结合方向和距离测量，可以对Local系下的3D点云的误差进行建模，如下：
  
  据此，可以得到 3D 点云的误差建模模型${\delta }_{{}^L{p}_i}\sim N(0,{\sum }_{{}^L{p}_i})$,为深度误差和方向误差线性变换之后的高斯分布。
  

对应代码实现如下：注意，这里方向向量的选择，是选择了N1,然后N1.cross(w)得到N2，此外，这部分协方差随着距离的增加而增加，因此越远的点置信度越低

void calcBodyCov(Eigen::Vector3d &pb, const float range_inc,
                 const float degree_inc, Eigen::Matrix3d &cov) {
  float range = sqrt(pb[0] * pb[0] + pb[1] * pb[1] + pb[2] * pb[2]);
  float range_var = range_inc * range_inc;
  Eigen::Matrix2d direction_var;
  direction_var << pow(sin(DEG2RAD(degree_inc)), 2), 0, 0,
      pow(sin(DEG2RAD(degree_inc)), 2);
  Eigen::Vector3d direction(pb);
  direction.normalize();
  Eigen::Matrix3d direction_hat;
  direction_hat << 0, -direction(2), direction(1), direction(2), 0,
      -direction(0), -direction(1), direction(0), 0;
  Eigen::Vector3d base_vector1(1, 1,
                               -(direction(0) + direction(1)) / direction(2));
  base_vector1.normalize();
  Eigen::Vector3d base_vector2 = base_vector1.cross(direction);
  base_vector2.normalize();
  Eigen::Matrix<double, 3, 2> N;
  N << base_vector1(0), base_vector2(0), base_vector1(1), base_vector2(1),
      base_vector1(2), base_vector2(2);
  Eigen::Matrix<double, 3, 2> A = range * direction_hat * N;
  cov = direction * range_var * direction.transpose() +
        A * direction_var * A.transpose();
};

2、世界系下点云${}^W{p}_i$位置协方差

这部分比较简单，转换过程${}^W{p}_i{=}_L^W{R}_i{\ast }^L{p}_i{+}_L^W{t}_i$,这里转换时需要考虑位姿${}_L^W{T}_i$的不确定度，计算过程如下：

因此，世界系下点云的误差${\delta }_{{}^w{p}_i}\sim N(0,{\sum }_{{}^w{p}_i})$为：

这个不确定度已经和滤波器状态相关了，因此是系统状态不确定度+Lidar测量的总不确定度，因此会比局部坐标系下点的误差更大一些。

3、VoxelMap中拟合平面${[}^{W}n{,}^{W}q]$的协方差

这部分是重点，地图中保存世界系下的平面，通常表示为法向量$n_p$和质心${}^W{q}_p$，也会保存并更新平面协方差，平面协方差就是估计的n和q的协方差。前面可以看到n和q的计算过程，是对点云最新范围的一簇历史点进行特征分解得到，假设拟合平面使用N个点，${}^W{p}_1{,}^W{p}_2,....{,}^W{p}_N$, 平面的法向量$n_q$为点云协方差矩阵最小特征值${\lambda }_m$对应的特征向量${\text{u}}_m$.
显然[n,q]能表示为点云的函数

一阶泰勒展开：
$$[n^{gt},q^{gt}{]}^T\approx [n,q{]}^T+\sum _{i=1}^N\frac{\partial f}{{\partial }^W{p}_i}{\delta }_{{}^w{p}_i}$$

最终的协方差为：

下面介绍$\frac{\partial f}{{\partial }^W{p}_i}$怎么求。

• 质心${}^{W}q$的协方差计算：质心是所有点云的平均值
  
  可以得到质心的误差服从${\delta }_q\sim N(0,{\sum }_q)$
  为这些3D点covariance的线性组合：
  

• 特征向量${\text{u}}_m$的协方差计算：平面的法向量$n_q$为点云协方差矩阵最小特征值${\lambda }_m$对应的特征向量，与点云${}^W{p}_1{,}^W{p}_2,....{,}^W{p}_N$之间的关系比较复杂，我们一步一步来进行推导：
  对于点云${}^W{p}_1{,}^W{p}_2,....{,}^W{p}_N$的协方差矩阵A
  
  A有如下特征分解：

由于A为实对称矩阵，因此特征向量${\text{u}}_1$，${\text{u}}_2$，${\text{u}}_3$彼此正交，因此$U^{T}U=I$成立，$U^{T}U=I$两边对${}^W{p}_i$的三维坐标$(x_i,y_i,z_i)$中任意$v$求偏导，则下式成立：

对于特征分解$\Lambda =U^{T}AU$两边也对$v$求偏导，得到：

为了表示成${}^{v}C$的方程，将$U\Lambda =AU$带入，可得：

带入${}^{v}C$得到：

这里$\Lambda$是对角矩阵，因此$\frac{\partial \Lambda }{\partial v}$的所有非对角元素为0。 因此上述方程的非对角元素为0。因此下等式恒成立：

其中${}^v{C}_{m,n}$为${}^{v}C$的m行n列元素。对${}^v{C}_{m,n}$合并同类项，并结合${}^{v}C{+}^v{C}^T=0$，可得${}^{v}C$中所有元素的解析表达式：

结合${}^{V}C=U^T\frac{\partial U}{\partial v}$,则下式成立：

其中$e_k$代表第k个元素为1其他为0的方向向量。则带入v为$x,y,z$的任意值，则：

为了使上式更加简洁，引入行向量

则$\frac{\partial {\text{u}}_k}{\partial {\text{p}}_j}$为：

对于${\text{F}}_{m,n}^{p_j}$,可以近似为：

对于$\frac{\partial {\text{u}}_m^T\text{A}{\text{u}}_n}{\partial {\text{p}}_j}$的求解如下：

最后，代入$\frac{\partial {\text{u}}_m^T\text{A}{\text{u}}_n}{\partial {\text{p}}_j}$到${\text{F}}_{m,n}^{p_j}$，可以得到${\text{F}}_{m,n}^{p_j}$的解析表达式：

这样，我们就可以得到特征向量$\text{u}$关于点云${}^W{p}_i$的偏导数：

对平面法向量来说，令${\text{u}}_k$为最小特征值对应的特征向量${\text{u}}_m$即可。最后平面法向量${\text{u}}_m$的协方差如下：

注：这里可以很方便的扩展到直线，直线方向向量为最大特征值对应特征向量，替换${\text{u}}_k$为最大特征向量即可。
此外，如果新增几个点之后，需要重新拟合平面，这部分可以进行增量计算，因此上述计算也只需要增量计算即可，可以避免一些重复计算。
增量更新一个高斯分布：

均值显而易见，下面简单说下方差推导

中间两项为零，于是

合并后:

对应代码：

// check is plane , calc plane parameters including plane covariance
  void init_plane(const std::vector<pointWithCov> &points, Plane *plane) {
    plane->plane_cov = Eigen::Matrix<double, 6, 6>::Zero();
    plane->covariance = Eigen::Matrix3d::Zero();
    plane->center = Eigen::Vector3d::Zero();
    plane->normal = Eigen::Vector3d::Zero();
    plane->points_size = points.size();
    plane->radius = 0;
    for (auto pv : points) {
      plane->covariance += pv.point * pv.point.transpose();
      plane->center += pv.point;
    }
    plane->center = plane->center / plane->points_size;
    plane->covariance = plane->covariance / plane->points_size -
                        plane->center * plane->center.transpose();
    Eigen::EigenSolver<Eigen::Matrix3d> es(plane->covariance);
    Eigen::Matrix3cd evecs = es.eigenvectors();
    Eigen::Vector3cd evals = es.eigenvalues();
    Eigen::Vector3d evalsReal;
    evalsReal = evals.real();
    Eigen::Matrix3f::Index evalsMin, evalsMax;
    evalsReal.rowwise().sum().minCoeff(&evalsMin);
    evalsReal.rowwise().sum().maxCoeff(&evalsMax);
    int evalsMid = 3 - evalsMin - evalsMax;
    Eigen::Vector3d evecMin = evecs.real().col(evalsMin);
    Eigen::Vector3d evecMid = evecs.real().col(evalsMid);
    Eigen::Vector3d evecMax = evecs.real().col(evalsMax);
    // plane covariance calculation
    Eigen::Matrix3d J_Q;
    J_Q << 1.0 / plane->points_size, 0, 0, 0, 1.0 / plane->points_size, 0, 0, 0,
        1.0 / plane->points_size;
    if (evalsReal(evalsMin) < planer_threshold_) {
      std::vector<int> index(points.size());
      std::vector<Eigen::Matrix<double, 6, 6>> temp_matrix(points.size());
      for (int i = 0; i < points.size(); i++) {
        Eigen::Matrix<double, 6, 3> J;
        Eigen::Matrix3d F;
        for (int m = 0; m < 3; m++) {
          if (m != (int)evalsMin) {
            Eigen::Matrix<double, 1, 3> F_m =
                (points[i].point - plane->center).transpose() /
                ((plane->points_size) * (evalsReal[evalsMin] - evalsReal[m])) *
                (evecs.real().col(m) * evecs.real().col(evalsMin).transpose() +
                 evecs.real().col(evalsMin) * evecs.real().col(m).transpose());
            F.row(m) = F_m;
          } else {
            Eigen::Matrix<double, 1, 3> F_m;
            F_m << 0, 0, 0;
            F.row(m) = F_m;
          }
        }
        J.block<3, 3>(0, 0) = evecs.real() * F;
        J.block<3, 3>(3, 0) = J_Q;
        plane->plane_cov += J * points[i].cov * J.transpose();
      }
}

4、观测更新

a) 平面匹配-$3\sigma$校验

给定一个Lidar 3D点${}^W{p}_i$(通过位姿先验预测到世界坐标系)，我们首先找到其root Voxel(使用coarse地图分辨率，大体素)位于的Hash Kay. 然后所有子体素都有可能是一个有效的匹配，这里假设某个子体素包含平面[n,q]，我们可以计算点到平面距离的不确定度，当前scan的点到VoxelMap中获取的平面的距离如下：

对于$f_p$ 进行协方差传递：

当系统没有误差时有：

则上式可以简化为：

则点到平面距离d的方差为：

这里需要判断点到平面的距离是否<$3\sigma$(注：上式写错了结果是${\sigma }^2$),满足这个条件的被认为是有效的匹配，如果大体素里面的多个平面都满足$3\sigma$准则，那么就选择概率最高的平面进行匹配。如果没有平面满足该准则，把这个点扔掉。

b) 测量更新

点到平面距离观测方程如下：

测量噪声为：

其中：

${\text{T}}_k$是状态变量，观测矩阵为${\text{J}}_{{\text{T}}_i}$. 知道观测矩阵和观测噪声，然后基于IEKF进行更新即可。

五、优化思路

• 这篇论文只考虑了面特征，感兴趣的可以对线特征进行类似的推导，原理并不复杂，可以复用面计算的一些结果
• 目前VoxelMap地图还是存在冗余，如果是一个大的平面，地图中还是会存在多个相同的平面，可以对这些平面进行合并，减小地图尺寸