第二章 Multi-LiDAR Extrinsic Calibration (1)

《 Targetless Extrinsic Calibration of Multiple Small FoV LiDARs and Cameras using Adaptive Voxelization》理论阅读

• 第一章:自适应体素化(1)——译文、理论
• 第一章:自适应体素化(2)——代码阅读
• 第二章:多雷达外参标定(1)——译文
• 第二章:多雷达外参标定(2)——推导损失函数降维
• 第二章:多雷达外参标定(3)——推导二阶闭式导数的两个引理
• 第二章:多雷达外参标定(4)——推导二阶闭式导数
• 第二章:多雷达外参标定(5)——代码阅读、公式补充
• 第三章:雷达相机外参标定——译文、代码阅读

---

---

前言

主要记录论文mlcc关于多雷达外参标定的部分，由于理论过程使用Latex导致字数超过限制，因此将理论推导拆分为多个章节记录。

---

一、文章内容

            With adaptive voxelization, we can obtain a set of voxels of different sizes. Each voxel contains points that are roughly on a plane and creates a planar constraint for all LiDAR poses that have points in this voxel. More specifically, considering the $l-th$ voxel consisting of a group of points ${\mathcal{P}}_l={}_{}^G{P}_{L_i,t_j}$ scanned by $L_i\in \mathcal{L}$ at times $t_j\in \mathcal{T}$.We define a point cloud consistency indicator ${}_{c_l}{(}_{L_i}^G{T}_{t_j})$ which forms a factor on $\mathcal{S}$ and ${\mathcal{E}}_L$ shown in Fig. 4(a). Then, the base LiDAR trajectory and extrinsic are estimated by optimizing the factor graph. A natural choice for the consistency indicator $c_l(\cdot )$ would be the summed Euclidean distance between each ${}_{}^G{P}_{L_i,t_j}$ the plane to be estimated (see Fig. 4(b)). Taking account of all such indicators within the voxel map, we could formulate the problem as $$\arg \underset{\mathcal{S},{\mathcal{E}}_L,n_l,q_l}{\min }\sum _l\underset{l-\mathrm{th}\mathrm{factor}}{\underbrace{\left(\frac{1}{N_l}\sum _{k=1}^{N_l}{\left(n_l^T\left({}^G{p}_k-q_l\right)\right)}^2\right)}}$$， where ${}_{}^G{p}_k\in {\mathcal{P}}_l$, $N_l$ is the total number of points in ${\mathcal{P}}_l$, $n_l$ is the normal vector of the plane and $q_l$ is a point on this plane.

Fig.4 :(a) The $l-th$ factor item relating to $\mathcal{S}$ and ${\mathcal{E}}_L$ with $L_i\in \mathcal{L}$ and $t_j\in \mathcal{T}$ . (b) The distance from the point ${}_{}^G{p}_k$ to the plane $\pi$.

         通过自适应体素化，我们可以获得一组不同大小的体素。每个体素包含大致在一个平面上的点，并为所有包含在此体素内的雷达姿态创建一个平面约束。更具体地说，考虑由$L_i\in \mathcal{L}$在时刻$t_j\in \mathcal{T}$扫描的一组点组成的第$l$个体素。我们定义了一个点云一致性指标${}_{c_l}{(}_{L_i}^G{T}_{t_j})$ ，它在图4(a)中形成了$\mathcal{S}$ 和 ${\mathcal{E}}_L$上的因子。然后，通过优化因子图来估计基准雷达的轨迹和外参。对于一致性指标$c_l(\cdot )$的一个自然选择是计算每个${}_{}^G{P}_{L_i,t_j}$到平面的欧几里得距离之和（见图4(b)）。考虑到体素图中所有这样的指标，我们可以将问题表述为
$$\arg \underset{\mathcal{S},{\mathcal{E}}_L,n_l,q_l}{\min }\sum _l\underset{l-\mathrm{th}\mathrm{factor}}{\underbrace{\left(\frac{1}{N_l}\sum _{k=1}^{N_l}{\left(n_l^T\left({}^G{p}_k-q_l\right)\right)}^2\right)}}$$
，其中 ${}_{}^G{p}_k\in {\mathcal{P}}_l$，$N_l$ 是 ${\mathcal{P}}_l$中所有点的总点数, $n_l$ 是平面的法向量， $q_l$ 是平面中的一点。

Fig.4 :(a) 第$l$ 个因子项，涉及$\mathcal{S}$ 和 ${\mathcal{E}}_L$，其中 $L_i\in \mathcal{L}$ 且 $t_j\in \mathcal{T}$ 。 (b)点 ${}_{}^G{p}_k$到平面$\pi$的距离.

              It is noticed that the optimization variables $(n_l,q_l)$ in (2) could be analytically solved (see Appendix A and the resultant cost function (3) is over the LiDAR pose ${}_{L_i}^G{T}_{t_j}$ (hence the base LiDAR trajectory $\mathcal{S}$ and extrinsic ${\mathcal{E}}_L$) only, as follows $$\arg \underset{\mathcal{S},{\mathcal{E}}_L}{\min }\sum _l{\lambda }_3(A_l)$$ where ${\lambda }_3(A_l)$ denotes the minimal eigenvalue of matrix $A_l$ defined as $$A_l=\frac{1}{N_l}\sum _{k=1}^{N_l}{}_{}^G{p}_k{\cdot }_{}^G{p}_k^T-q_l^{\ast }\cdot {q_l^{\ast }}^T,q_l^{\ast }=\frac{1}{N_l}\sum _{k=1}^{N_l}{}_{}^G{p}_k$$
. To allow efficient optimization in (3), we derive the closedform derivatives w.r.t the optimization variable $x$ up to secondorder (the detailed derivation from (3) to (5) is elaborated in Appendix B):
$${\lambda }_3(x⊞\delta x)\approx {\lambda }_3(x)+\bar{J}\delta x+\frac{1}{2}\delta x^T\bar{H}\delta x$$
,where $\bar{J}$ is the Jacobian matrix, and $\bar{H}$ is the Hessian matrix.The $\delta x$ is a small perturbation of the optimization variable $x$:
$$x=[\underset{\mathcal{S}}{\underbrace{{\cdots }_{L_0}^G{R}_{t_j}{L}_0^G{t}_{t_j}\cdots }}\underset{{\mathcal{E}}_L}{\underbrace{\cdots {}_{L_i}^{L_0}R{}_{L_i}^{L_0}t\cdots }}]$$
.Then the optimal $x^{\ast }$ could be determined by iteratively solving (6) with the LM method and updating the $\delta x$ to $x$.
$$(\bar{H}+\mu I)\delta x=-{\bar{J}}^T$$

         注意到优化变量$(n_l,q_l)$在方程(2)中可以解析求解（详见附录A）,由此得到的损失函数(3)仅关于雷达姿态${}_{L_i}^G{T}_{t_j}$(即基准雷达轨迹$\mathcal{S}$和外参${\mathcal{E}}_L$),如下所示：$$\arg \underset{\mathcal{S},{\mathcal{E}}_L}{\min }\sum _l{\lambda }_3(A_l)$$其中${\lambda }_3(A_l)$表示矩阵 $A_l$的最小特征值， $A_l$定义为$$A_l=\frac{1}{N_l}\sum _{k=1}^{N_l}{}_{}^G{p}_k{\cdot }_{}^G{p}_k^T-q_l^{\ast }\cdot {q_l^{\ast }}^T,q_l^{\ast }=\frac{1}{N_l}\sum _{k=1}^{N_l}{}_{}^G{p}_k$$
。为了使式(3)中的优化高效，我们推导了优化变量$x$的二阶闭式导数(从(3)到(5)的详细推导见附录B)：
$${\lambda }_3(x⊞\delta x)\approx {\lambda }_3(x)+\bar{J}\delta x+\frac{1}{2}\delta x^T\bar{H}\delta x$$
。其中$\bar{J}$是雅可比矩阵，$\bar{H}$是海森矩阵。$\delta x$是优化变量$x$的小扰动：
$$x=[\underset{\mathcal{S}}{\underbrace{{\cdots }_{L_0}^G{R}_{t_j}{L}_0^G{t}_{t_j}\cdots }}\underset{{\mathcal{E}}_L}{\underbrace{{\cdots }_{L_i}^{L_0}R{}_{L_i}^{L_0}t\cdots }}]$$
。然后，最优解$x^{\ast }$可以通过迭代求解公式(6)并使用LM的方法更新$\delta x$到$x$来确定。
$$(\bar{H}+\mu I)\delta x=-{\bar{J}}^T$$

二、 参考文献

[1]《Targetless Extrinsic Calibration of Multiple Small FoV LiDARs and Cameras using Adaptive Voxelization》

[2]《BALM: Bundle Adjustment for Lidar Mapping》

[3]《BALM论文阅读》——epsilonjohn的博客文章

[4]《A micro Lie theory for state estimation in robotics》

[5]《多个LiDAR-Camera无目标跨视角联合标定方法》

[6]《如何用法向量求点到平面距离_【立体几何】用空间向量求点到面的距离》