读ALOAM FLOAM源码笔记

待填坑
ALOAM源代码

FLOAM源代码

robinvista 的LOAM原理详解参考

1.雷达点云畸变补偿

对每个激光点坐标做补偿，补偿量为射出本激光点时，激光原点即雷达坐标相对该帧起始时刻的变化。
若起始时刻，雷达位姿为
$$T_0=\left[\begin{array}{cc}{R}_0& t_0\\ 0& 1\end{array}\right]$$
第i个激光点时，雷达位姿为
$$T_i=\left[\begin{array}{cc}{R}_i& t_i\\ 0& 1\end{array}\right]$$
第$i$个激光点的坐标为
$$P_i={\left[\begin{array}{c}{p}_{ix},p_{iy},p_{iz}\end{array}\right]}^T$$
第$i$个激光点补偿畸变后的坐标应该为
$$P_i^{\prime }=T_0^{-1}{T}_i{P}_i$$
可以通过雷达从0到$i$时刻的平均角速度和速度计算两时刻的相对旋转和平移：
$$\begin{array}{rl}{R}_i& =\omega d_{time}\\ t_i& =V{d}_{time}\end{array}$$
其中$\omega ,V$可从传感器得到，$d_{time}$ 可从下式得到
$$d_{time}=\frac{当前点角度-该帧点云起点角度}{该帧点云终点角度-该帧点云起点角度}\ast 周期$$

2. 每帧点云找特征点

逻辑

• 按照所在线束scanId划分点云，并计算每个点相对起点的$d_{time}$，$intensity=scanId+d_{time}$
• 为每个点计算特征值
• 循环线束(为使特征点分散，不太过密集，在每条线束上取固定数目的sharp,less shap,flat点)
  • 把每条线束按照数目分为6个区间(点云按照距起始点角度排列，相当于0-360度划分6个区间，在每个区间内取固定数目的特征点)
    • 该区域按照曲率排序
    • 取曲率大的固定数目点做sharp和lessSharp点，当确定取点时，改点前5个和后5个曲率较小的点不做flat点考虑
    • 取曲率小的固定数目点做flat，当确定取点时，该点前5个点和后5个曲率较小的点也不做flat点考虑

3.前后帧点线与点面对应

逻辑
点线残差：

• 循环k+1帧sharp点$p_i$
  • 预测k+1帧相对k帧的位姿为k帧相对k-1帧的位姿，将k+1帧上点按照预测位姿转换到k帧坐标系，为${\tilde{p}}_i$点
  • 在k帧lesssharp中找到距${\tilde{p}}_i$最近的点$p_a$，并得到$p_a$所在的线束scanId
  • 按照scanId增加的方向循环k帧中lessSharp点
    • 点的线束与scanId相同，跳过
    • 点的线束 - scanId > 限制范围，终止寻找
    • 找距${\tilde{p}}_i$距离在限制范围内最小的点 做$p_b$
  • 按照scanId减少的方向循环k帧中lessSharp点
    • 点的线束与scanId相同，跳过
    • scanId -点的线束 > 限制范围，终止寻找
    • 找距${\tilde{p}}_i$距离在限制范围内最小的点做$p_b$
  • 将点$p_i$到线$p_a-p_b$的距离做残差，ceres做优化

点面残差：

• 循环k+1帧flat点$p_i$
  • 预测k+1帧相对k帧的位姿为k帧相对k-1帧的位姿，将k+1帧上点按照预测位姿转换到k帧坐标系，为${\tilde{p}}_i$点
  • 在k帧lessflat中找到距${\tilde{p}}_i$最近的点$p_a$，并得到$p_a$所在的线束scanId
  • 按照scanId增加的方向循环k帧中lessflat点
    • 点的线束 - scanId > 限制范围，终止寻找
    • 找同线束上，距${\tilde{p}}_i$距离在限制范围内最小的点做$p_b$
    • 找非同线束上，距${\tilde{p}}_i$距离在限制范围内最小的点做$p_c$
  • 按照scanId减少的方向循环k帧中lessflat点
    • scanId -点的线束 > 限制范围，终止寻找
    • 找同线束上，距${\tilde{p}}_i$距离在限制范围内最小的点做$p_b$
    • 找非同线束上，距${\tilde{p}}_i$距离在限制范围内最小的点做$p_c$
  • 将点$p_i$到面$p_a,p_b,p_c$的距离做残差，ceres做优化

4.点线残差、点面残差求Jacobian

• 点线残差
  $$\begin{array}{rl}{d}_{ϵ}& =\frac{|({\tilde{p}}_i-p_a)\times ({\tilde{p}}_i-p_b)|}{|p_a-p_b|}\\ {\tilde{p}}_i& =T{p}_i=R{p}_i+t\end{array}$$

为方便计算和理解，改变标记，令线特征残差为$f_{ϵ}$，点线距离的平方
$$\begin{array}{rl}{f}_{ϵ}& =|d_{ϵ}{|}^2=d_{ϵ}^T{d}_{ϵ}\\ d_{ϵ}& =\frac{({\tilde{p}}_i-p_b)\times ({\tilde{p}}_i-p_b)}{|p_a-p_b|}\\ {\tilde{p}}_i& =T{p}_i=R{p}_i+t\end{array}$$
雅克比由链式法则求导
$$\begin{array}{rl}J& =\frac{\partial f_{ϵ}}{\partial T}=\frac{\partial f_{ϵ}}{\partial d_{ϵ}}\frac{\partial d_{ϵ}}{\partial {\tilde{p}}_i}\frac{\partial {\tilde{p}}_i}{\partial T}或J=\frac{\partial f_{ϵ}}{\partial \xi }=\frac{\partial f_{ϵ}}{\partial d_{ϵ}}\frac{\partial d_{ϵ}}{\partial {\tilde{p}}_i}\frac{\partial {\tilde{p}}_i}{\partial \xi }(T=exp({\xi }^{\wedge }))\\ \frac{\partial f_{ϵ}}{\partial d_{ϵ}}& =\frac{\partial d_{ϵ}^T{d}_{ϵ}}{\partial d_{ϵ}}=2d_{ϵ}^T\\ \frac{\partial d_{ϵ}}{\partial {\tilde{p}}_i}& =\frac{1}{|p_a-p_b|}\frac{\partial ({\tilde{p}}_i-p_a)\times ({\tilde{p}}_i-p_b)}{\partial {\tilde{p}}_i}=\frac{(p_b-p_a{)}^{\wedge }}{|p_a-p_b|}\\ \frac{\partial {\tilde{p}}_i}{\partial T}& =\frac{\partial {\tilde{p}}_i}{\partial [R,t{]}_{3\times 4}}=[-(R{p}_i+t{)}^{\wedge },I_{3\times 3}{]}_{3\times 6}\\ J& =2d_{ϵ}^T\cdot \frac{(p_b-p_a{)}^{\wedge }}{|p_a-p_b|}\cdot [-(R{p}_i+t{)}^{\wedge },I_{3\times 3}{]}_{3\times 6}\end{array}$$

• 点面残差
  $$d_H=|({\tilde{p}}_i-p_j)\cdot \frac{(p_l-p_j)\times (p_m-p_j)}{|(p_l-p_j)\times (p_m-p_j)|}|$$
  为方便计算和理解，改变标记，令面特征残差为$f_H$，点面距离的平方
  $$\begin{array}{rl}{f}_H& =|d_H{|}^2=d_H^T{d}_H\\ d_H& =({\tilde{p}}_i-p_j)\cdot \frac{(p_l-p_j)\times (p_m-p_j)}{|(p_l-p_j)\times (p_m-p_j)|}\\ & =({\tilde{p}}_i-p_j{)}^T\frac{(p_l-p_j{)}^{\wedge }(p_m-p_j)}{|(p_l-p_j)\times (p_m-p_j)|}\\ {\tilde{p}}_i& =T{p}_i=R{p}_i+t\end{array}$$
  雅克比由链式法则求导
  $$\begin{array}{rl}J& =\frac{\partial f_H}{\partial T}=\frac{\partial f_H}{\partial d_H}\frac{\partial d_H}{\partial {\tilde{p}}_i}\frac{\partial {\tilde{p}}_i}{\partial T}\\ \frac{\partial f_H}{\partial d_H}& =\frac{\partial d_H^T{d}_H}{\partial d_H}=2d_H^T\\ \frac{\partial d_H}{\partial {\tilde{p}}_i}& =[\frac{(p_l-p_j{)}^{\wedge }(p_m-p_j)}{|(p_l-p_j)\times (p_m-p_j)|}{]}^T\\ \frac{\partial {\tilde{p}}_i}{\partial T}& =\frac{\partial {\tilde{p}}_i}{\partial [R,t{]}_{3\times 4}}=[-(R{p}_i+t{)}^{\wedge },I_{3\times 3}{]}_{3\times 6}\\ J& =2d_H^T\cdot [\frac{(p_l-p_j{)}^{\wedge }(p_m-p_j)}{|(p_l-p_j)\times (p_m-p_j)|}{]}^T\cdot [-(R{p}_i+t{)}^{\wedge },I_{3\times 3}{]}_{3\times 6}\end{array}$$
  因为$d_H$维数是$1\times 1$，所以标量表示的点面距离残差和矢量表示的残差无异，仅相差一个$2d_H$倍数。

5.根据李代数直接求出四元数和平移向量

• se3
  $SE(3)$ 元素$T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]$；
  $se(3)$ 元素 $\xi =[\varphi ,\rho {]}_{6\times 1}^T$，
  $$\begin{array}{rl}exp({\xi }^{\wedge })& =\left[\begin{array}{cc}R& J\rho \\ 0& 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\\ J& =\frac{\sin \theta }{\theta }I+(1-\frac{\sin \theta }{\theta })a{a}^T+\frac{1-\cos \theta }{\theta }{a}^{\wedge }\end{array}$$
• so3
  $SO(3)$元素R;
  $so(3)$元素$\varphi ,{\varphi }^{\wedge }=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ \varphi 3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]$
  $$\exp ({\varphi }^{\wedge })=\exp (\theta a^{\wedge })=\cos \theta I+(1-\cos \theta )a{a}^T+\sin \theta a^{\wedge }$$
  并且$\varphi =\theta a$是$R$的旋转向量，$|\varphi |$是$R$的旋转角$\theta$
• 求解
  有李代数${\xi }_{6\times 1}$时，取$\varphi =\xi [0:3],\rho =\xi [3:6]$
  • 计算旋转角
    $\theta =|\varphi |=\varphi .norm()$
  • 由旋转角计算四元数q
    $\{\begin{array}{l}\theta =2arc\cos q_0\\ a^T=[n_x,n_y,n_z{]}^T=[q_1,q_2,q_3{]}^T/\sin \frac{\theta }{2}\end{array}$
    得到
    $\{\begin{array}{l}{q}_0=\cos \frac{\theta }{2}\\ {\left[\begin{array}{c}{q}_1,q_2,q_3\end{array}\right]}^T=\frac{{\varphi }^T}{\theta }\sin \frac{\theta }{2}\end{array}$
    当$\theta$不为0，直接计算q，当$\theta$十分小，泰勒展开得
    $\frac{\sin \theta }{\theta }=\frac{1}{\theta }[\frac{\theta }{2}-\frac{1}{3!}\frac{{\theta }^3}{2^3}+\frac{1}{5!}\frac{{\theta }^5}{2^5}]$近似求解
  • 计算平移量t
    $\begin{array}{rl}J& =\frac{\sin \theta }{\theta }I+(1-\frac{\sin \theta }{\theta })a{a}^T+\frac{1-\cos \theta }{\theta }{a}^{\wedge }\\ & =I+\frac{\theta -\sin \theta }{\theta }{a}^{\wedge }{a}^{\wedge }+\frac{1-\cos \theta }{\theta }{a}^{\wedge }\\ & =I+\frac{\theta -\sin \theta }{{\theta }^3}{\varphi }^{\wedge }{\varphi }^{\wedge }+\frac{1-\cos \theta }{{\theta }^2}{\varphi }^{\wedge }\\ t& =J\rho \end{array}$