LOAM点云匹配算法解析与雅克比矩阵推导

前言

LOAM点云匹配部分极为经典，可以说是LOAM整个框架的核心，其运算速度快，精度高，自14年发布，并在后续拿下kitti冠军后，直到现在仍然被广泛使用，但在后续的推广中仍然有一些问题，这里做一些解析并记录下自己应用中的问题。

算法解析

算法流程

算法具体解析的文章实在太多了，这里不做赘述，只简单介绍下相关流程和原理，大致流程如下：

Yes

No

提取线/面特征

提取线/面地图点云

迭代

计算线特征与线地图残差与对应优化向量

融合残差与优化向量

计算面特征与面地图#残差与对应优化向量

更新雅克比矩阵并求解迭代方程

更新状态量

大于迭代次数或更新量小于阈值

输出

更新线/面特征

线/面特征与线/面地图的残差与对应优化向量计算

读过LOAM的都知道点云匹配部分是一个点到线ICP加一个点到面ICP，优化时可以理解为都是计算点到点的残差，只不过一个是在对应线上找到目标点，一个是在对应面上找到目标点，然后分别计算源点到目标点的残差，这样两种ICP的残差计算方程就统一了，后续计算雅克比矩阵时需要用到源点到目标点的距离（即残差）以及源点到目标点的单向量，具体为什么需要它们会在雅克比推导中给出。

• 当前帧线特征与地图线特征残差计算
  对当帧线特征中的某个点A，依次从地图线特征中找到其最近的5个点，用这个5个点拟合一条直线L，计算A与L的距离D以及A垂直于L的向量V，该距离即为残差量，V为优化方向，保存下来待后续使用，对当帧线特征的所有点执行以上过程。
• 当前帧面特征与地图面特征残差计算
  对当帧面特征中的某个点A，依次从地图线特征中找到其最近的5个点，用这个5个点拟合一个平面P，计算A与P的距离D以及A垂直于P的向量V，该距离即为残差量，V为优化方向，保存下来待后续使用，对当帧线特征的所有点执行以上过程。

后面的过程要了解下LM算法才能继续。

LM算法推导

LM算法的推导参考LOAM-LM方法
雅克比推导参考了LeGO-LOAM中的数学公式推导，这里我重新捋了一遍，并且推导了基于Tait-Bryan xyz extrinsic rotations的欧拉角的雅克比矩阵，原版LOAM中雅克比为基于Tait-Bryan zyx extrinsic rotations的欧拉角。

主体部分推导

重点在于每个点的残差计算方程即$f_i(X)$以及它对系统状态$X$的偏导数$f_i^{\prime }(X)$，其中$f_i(X)$指每个点到线/面对应点的残差计算方程，$X$指当前系统状态，即当前求解的位姿，由旋转R和平移T构成，表示为$x,y,z,roll,pitch,yaw$，ROS中，欧拉角为Tait-Bryan xyz extrinsic rotations类型，即绕固定轴，先绕x轴，再绕y轴，最后绕z轴旋转roll，pitch，yaw角度的方式，首先定义$f_i(X)$的形式，我们先去掉i，就以单一一个点来推导：
$$\begin{gathered} p^w=G(p^l,X)=R{p}^l+T \\ f(X)=D(p^w,p^t) \end{gathered}$$
其中$D(p^w,p^t)$为两点间距离，$p^l$为当前帧的点在局部坐标系下坐标，$p^w$为当前帧的点在世界坐标系下坐标，$p^t$为对应线/面上目标点在世界坐标系的坐标。开始计算偏导：
$$\frac{\partial f}{\partial X}=\frac{\partial f}{\partial G(p^l,X)}\ast \frac{\partial G(p^l,X)}{\partial X}$$

其中：
$$\begin{gathered} D(p^w,p^t)=[(p^w-p^t{)}^T(p^w-p^t){]}^{\frac{1}{2}}=(p^{wT}{p}^w-p^{wT}{p}^t-p^{tT}{p}^w-p^{tT}{p}^t){]}^{\frac{1}{2}} \\ \frac{\partial f}{\partial G(p^l,X)}=\frac{\partial D(p^w,p^t)}{\partial p^w}=\frac{1}{2}\ast \frac{2(p^w-p^t)}{D(p^w,p^t)}=(p^w-p^t).normlize()=V \end{gathered}$$
可以看到，这个V就是两点间方向的单位向量，求残差时已经得到了。偏导剩余部分要分为对平移量T的求导和对欧拉角的求导两部分，先看平移部分：
$$\frac{\partial G(p^l,X)}{\partial T}=\frac{\partial (R{p}^l+T)}{\partial T}=E$$
旋转部分比较麻烦，以对roll的求导来看，我们用ex代替roll、ey代替pitch、ez代替yaw简化表示：
$$\frac{\partial G(p^l,X)}{\partial ex}=\frac{\partial (R{p}^l+T)}{\partial ex}=\frac{\partial R}{\partial ex}\ast p^l$$
实际上就是就求旋转矩阵分别对roll、pitch、yaw的偏导。

$$\begin{array}{rl}\frac{\partial G(p^l,X)}{\partial ex}& =\frac{\partial (R{p}^l+T)}{\partial ex}\\ & =\frac{\partial R}{\partial ex}\ast p^l\end{array}$$

基于Tait-Bryan xyz extrinsic rotations的雅克比推导

不同类型的欧拉角表达方式其对应的旋转矩阵是不同的，可以参考Euler angles，中给出的结，这里直接贴出Tait-Bryan xyz extrinsic rotations的欧拉角对应的旋转矩阵

123对应ZYX，c为cos，s为sin，那么偏导就容易了：
$$\begin{gathered} \frac{\partial R}{\partial ex}=\left[\begin{array}{ccc} \\ 0& c_z{s}_y{c}_x+s_x{s}_z& s_z{c}_x-c_z{s}_x{s}_y)\\ 0& -c_z{s}_x+s_z{s}_y{c}_x& -s_x{s}_z{s}_y-c_z{c}_x\\ 0& c_y{c}_x& -c_y{s}_x)\end{array}\right] \\ \frac{\partial R}{\partial ey}=\left[\begin{array}{ccc} \\ -c_z{s}_y& c_z{c}_y{s}_x& c_z{c}_x{c}_y)\\ -s_y{s}_z& s_z{c}_y{s}_x& c_x{s}_z{c}_y\\ -c_y& -s_y{s}_x& -s_y{c}_x)\end{array}\right] \\ \frac{\partial R}{\partial ez}=\left[\begin{array}{ccc} \\ -s_z{c}_y& -s_z{s}_y{s}_x-c_x{c}_z& c_z{c}_x{c}_y)\\ c_y{c}_z& -s_z{c}_x+c_z{s}_y{s}_x& c_x{c}_z{s}_y+s_z{s}_x\\ 0& 0& 0)\end{array}\right] \end{gathered}$$
于是：
$$\begin{gathered} \begin{array}{rl}\frac{\partial f}{\partial X}& =\frac{\partial f}{\partial G(p^l,X)}\ast \frac{\partial G(p^l,X)}{\partial X}\\ & =V\ast [\frac{\partial R}{\partial ex}\ast p^l,\frac{\partial R}{\partial y}\ast p^l,\frac{\partial R}{\partial z}\ast p^l,E]\end{array} \\ J=[{\frac{\partial f_0}{\partial X}}^T,{\frac{\partial f_1}{\partial X}}^T,{\frac{\partial f_2}{\partial X}}^T,...,{\frac{\partial f_n}{\partial X}}^T] \end{gathered}$$
至此，雅克比就被求解了，剩下的套LM公式就好。

代码剖析

代码太多了，而且很多文字都对应剖析了，这里就提下雅克比部分就好：

	//求roll、pitch、yaw对应的sin和cos，用以求上面提到R对它们的求导
      float srx = _transformTobeMapped.rot_x.sin();
      float crx = _transformTobeMapped.rot_x.cos();
      float sry = _transformTobeMapped.rot_y.sin();
      float cry = _transformTobeMapped.rot_y.cos();
      float srz = _transformTobeMapped.rot_z.sin();
      float crz = _transformTobeMapped.rot_z.cos();

      size_t laserCloudSelNum = _laserCloudOri.size();
      if (laserCloudSelNum < 50)
         continue;

      Eigen::Matrix<float, Eigen::Dynamic, 6> matA(laserCloudSelNum, 6);
      Eigen::Matrix<float, 6, Eigen::Dynamic> matAt(6, laserCloudSelNum);
      Eigen::Matrix<float, 6, 6> matAtA;
      Eigen::VectorXf matB(laserCloudSelNum);
      Eigen::VectorXf matAtB;
      Eigen::VectorXf matX;
	//对每个点依次求解
      for (int i = 0; i < laserCloudSelNum; i++)
      {
      	 //文中提到p^l
         pointOri = _laserCloudOri.points[i];
         //源点到目标点的方向单位向量，即V
         coeff = _coeffSel.points[i];
		 //雅克比中对roll求导部分
         float arx = (crx*sry*srz*pointOri.x + crx * crz*sry*pointOri.y - srx * sry*pointOri.z) * coeff.x
            + (-srx * srz*pointOri.x - crz * srx*pointOri.y - crx * pointOri.z) * coeff.y
            + (crx*cry*srz*pointOri.x + crx * cry*crz*pointOri.y - cry * srx*pointOri.z) * coeff.z;
		 //雅克比中对pitch求导部分
         float ary = ((cry*srx*srz - crz * sry)*pointOri.x
                      + (sry*srz + cry * crz*srx)*pointOri.y + crx * cry*pointOri.z) * coeff.x
            + ((-cry * crz - srx * sry*srz)*pointOri.x
               + (cry*srz - crz * srx*sry)*pointOri.y - crx * sry*pointOri.z) * coeff.z;
		 //雅克比中对yaw求导部分
         float arz = ((crz*srx*sry - cry * srz)*pointOri.x + (-cry * crz - srx * sry*srz)*pointOri.y)*coeff.x
            + (crx*crz*pointOri.x - crx * srz*pointOri.y) * coeff.y
            + ((sry*srz + cry * crz*srx)*pointOri.x + (crz*sry - cry * srx*srz)*pointOri.y)*coeff.z;

         matA(i, 0) = arx;
         matA(i, 1) = ary;
         matA(i, 2) = arz;
         雅克比中对x，y，z求导部分直，V*E等于V
         matA(i, 3) = coeff.x;
         matA(i, 4) = coeff.y;
         matA(i, 5) = coeff.z;
         //残差
         matB(i, 0) = -coeff.intensity;
      }

      matAt = matA.transpose();
      matAtA = matAt * matA;
      matAtB = matAt * matB;
	  //求解LM的更新公式JT*J*x=-JT*f
      matX = matAtA.colPivHouseholderQr().solve(matAtB);
	  //作者实际实现的是高斯-牛顿法，可能出现matATA奇异的问题，这里做了一些防止退化的操作，有专门论文讲解，原理是根据特征值和特征向量判断出更新量的向量空间，认为特征值很小的基上的更新量是不可靠的，因此会干掉该分量，有空再写文章记录。
      if (iterCount == 0)
      {
         Eigen::Matrix<float, 1, 6> matE;
         Eigen::Matrix<float, 6, 6> matV;
         Eigen::Matrix<float, 6, 6> matV2;

         Eigen::SelfAdjointEigenSolver< Eigen::Matrix<float, 6, 6> > esolver(matAtA);
         matE = esolver.eigenvalues().real();
         matV = esolver.eigenvectors().real();
		 //这儿有个bug，eigen特征向量是列向量，处理是按照行向量处理的
         matV2 = matV;

         isDegenerate = false;
         float eignThre[6] = { 100, 100, 100, 100, 100, 100 };
         for (int i = 0; i < 6; i++)
         {
            if (matE(0, i) < eignThre[i])
            {
               for (int j = 0; j < 6; j++)
               {
                  matV2(i, j) = 0;
               }
               isDegenerate = true;
            }
            else
            {
               break;
            }
         }
         matP = matV.inverse() * matV2;
      }

      if (isDegenerate)
      {
         Eigen::Matrix<float, 6, 1> matX2(matX);
         matX = matP * matX2;
      }
	  //更新状态量
      _transformTobeMapped.rot_x += matX(0, 0);
      _transformTobeMapped.rot_y += matX(1, 0);
      _transformTobeMapped.rot_z += matX(2, 0);
      _transformTobeMapped.pos.x() += matX(3, 0);
      _transformTobeMapped.pos.y() += matX(4, 0);
      _transformTobeMapped.pos.z() += matX(5, 0);
	  //单词更新小于阈值，则判断收敛
      float deltaR = sqrt(pow(rad2deg(matX(0, 0)), 2) +
                          pow(rad2deg(matX(1, 0)), 2) +
                          pow(rad2deg(matX(2, 0)), 2));
      float deltaT = sqrt(pow(matX(3, 0) * 100, 2) +
                          pow(matX(4, 0) * 100, 2) +
                          pow(matX(5, 0) * 100, 2));

      if (deltaR < _deltaRAbort && deltaT < _deltaTAbort)
         break;
   }