第二章 Multi-LiDAR Extrinsic Calibration（4）

送我上青云|

已于 2025-01-19 11:58:32 修改

阅读量684

点赞数 22

分类专栏：传感器标定 # mlcc 文章标签：自动驾驶算法

于 2025-01-13 20:12:19 首次发布

本文链接：https://blog.youkuaiyun.com/qq_38738362/article/details/145122691

版权

传感器标定同时被 2 个专栏收录

8 篇文章

订阅专栏

mlcc

8 篇文章

订阅专栏

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

文章目录

《 Targetless Extrinsic Calibration of Multiple Small FoV LiDARs and Cameras using Adaptive Voxelization》论文阅读
前言
一、理论推导
- 二阶闭式导数的推导
二、参考文献

前言

本文主要记录论文mlcc关于多激光雷达标定部分相关的理论推导。由于字数限制，本篇紧接着上篇文章完成二阶闭式导数的推导。

一、理论推导

二阶闭式导数的推导

回顾代价函数为 $\arg\min_{\mathcal{S},\mathcal{E}_{L}}\sum_{l}^{}\lambda_{3}(A_{l})\tag{式1}$

，优化问题是非线性的，通过迭代求解，每次迭代中，代价函数被二阶近似，具体的说，我们将 $\lambda_3$ 视为包含所有点 ${^G}p$ 的函数，其中
$^{G}p$ 是一个列向量，每个点 $^{G}p_{k}\in\mathcal{P}_{l}$ ，即：

$^{G}p=\begin{bmatrix}{^{G}p_{1}^{T}} \quad {^{G}p_{2}^{T}} \quad \cdots \quad {^{G}p_{N_{l}}^{T}} \end{bmatrix}\in\mathbb{R} ^{3N_{l}} \tag{式2}$

。 $\lambda{(^Gp)}$ 可被近似为：
$\lambda_{3}\left(^{G}{p}+\delta^{G}{p}\right)\approx\lambda_{3}\left(^{G}{p}\right)+{J}\cdot\delta^{G}{p}+\frac{1}{2} \delta^{G}{p}^{T}\cdot{H}\cdot\delta^{G}{p} \tag{式3}$

假设第 $k$ 个点 $^Gp_{k}$ 在时间 $t_j$ 由雷达 $L_i$ 扫描，则有：
$\begin{align*} ^Gp_k={^{G}_{L_i}T}_{t_j}p_k={^{G}_{L_0}T}_{t_j}\cdot {^{L_{0}}_{L_i}T}\cdot p_{k}\\ ={^{G}_{L_0}R_{t_j}}\left({^{L_0}_{L_i}R\cdot {p_k}+{^{L_0}_{L_i}t}}\right)+{^G_{L_0}t_{t_j}} \end{align*} \tag{式4}$

表明 $^Gp_k$ 依赖于 $\mathcal{S}$ 和 $\mathcal{E}_L$ 。为了扰动 $^Gp_k$ ,我们在位姿 $T$ 的进行扰动，
记为 $\delta{T}=\begin{bmatrix}\phi{^T}\quad\delta{t}^T \end{bmatrix}^T\in\mathbb{R}^6$ ，
于在流形上的扰动可以参考论文《A micro Lie theory for state estimation in robotics》。
$T=(R,t)\quad ,T \boxplus\delta{T}=\left(R\exp(\phi^{\wedge} ),t+\delta{t}\right) \tag{式5}$
。将上式带入到代价函数式1中，可得：
$\begin{align*} {}^{G}{p}_{k}+\delta^{G}{p}_{k}= {}_{L_{0}}^{G}R_{t_{j}}\exp\big({}_{L_{0}}^{G}\phi_{t_{j}}^{\wedge}\big) \bigg({}_{L_{i}}^{L_{0}}R\exp\big({}_{L_{i}}^{L_{0}}\phi^{\wedge}\big)p_{k} +{}_{L_{i}}^{L_{0}}{t}+\delta_{L_{i}}^{L_{0}}{t}\bigg)\\ +{}_{L_{0}}^{G}{t}_{t_{j}} +\delta_{L_{0}}^{G}{t}_{t_{j}}\\ ={}_{L_{0}}^{G}R_{t_{j}}\big(I+{}_{L_{0}}^{G}\phi_{t_{j}}^{\wedge}\big) \bigg({}_{L_{i}}^{L_{0}}R\big(I+{}_{L_{i}}^{L_{0}}\phi^{\wedge}\big)p_{k} +{}_{L_{i}}^{L_{0}}{t}+\delta_{L_{i}}^{L_{0}}{t}\bigg)\\ +{}_{L_{0}}^{G}{t}_{t_{j}}+\delta_{L_{0}}^{G}{t}_{t_{j}}\\ ={}_{L_{0}}^{G}{R}_{t_{j}}\big(I+{}_{L_{0}}^{G}\phi_{t_{j}}^{\wedge}\big) \bigg({}_{L_{i}}^{L_{0}}Rp_{k}+{}_{L_{i}}^{L_{0}}R{}_{L_{i}}^{L_{0}}\phi^{\wedge}p_{k} +{}_{L_{i}}^{L_{0}}{t}+\delta_{L_{i}}^{L_{0}}{t}\bigg)\\ +{}_{L_{0}}^{G}{t}_{t_{j}}+\delta_{L_{0}}^{G}{t}_{t_{j}}\\ \approx{}_{L_{0}}^{G}R_{t_{j}}\big({}_{L_{i}}^{L_{0}}R\cdot{p_{k}}+{}_{L_{0}}^{G}{t}_{t_{j}}\big) +{}_{L_{0}}^{G}R_{t_{j}}\big({}_{L_{i}}^{L_{0}}R{}_{L_{i}}^{L_{0}}\phi^{\wedge}p_{k} +\delta_{L_{i}}^{L_{0}}{t}\big)\\ +{}_{L_{0}}^{G}R_{t_{j}}{}_{L_{0}}^{G}\phi_{t_{j}}^{\wedge} \bigg({}_{L_{i}}^{L_{0}}Rp_{k}+{}_{L_{i}}^{L_{0}}{t}\bigg)\\ +{}_{L_{0}}^{G}{t}_{t_{j}}+\delta_{L_{0}}^{G}{t}_{t_{j}}\\ \approx{}_{L_{0}}^{G}R_{t_{j}}\big({}_{L_{i}}^{L_{0}}R\cdot{p_{k}} +{}_{L_{0}}^{G}{t}_{t_{j}}\big)+{}_{L_{0}}^{G}{t}_{t_{j}} +{}_{L_{0}}^{G}R_{t_{j}}\big({}_{L_{i}}^{L_{0}}R\cdot{}_{L_{i}}^{L_{0}}\phi^{\wedge}p_{k} +\delta_{L_{i}}^{L_{0}}{t}\big)\\ +{}_{L_{0}}^{G}R_{t_{j}}{}_{L_{0}}^{G}\phi_{t_{j}}^{\wedge} \bigg({}_{L_{i}}^{L_{0}}Rp_{k} +{}_{L_{i}}^{L_{0}}{t}\bigg)+\delta_{L_{0}}^{G}{t}_{t_{j}}\\ \approx{}_{L_{0}}^{G}R_{t_{j}}\big({}_{L_{i}}^{L_{0}}R\cdot{p_{k}} +{}_{L_{0}}^{G}{t}_{t_{j}}\big)+{}_{L_{0}}^{G}{t}_{t_{j}}\\ -{}_{L_0}^GR_{t_j}{}_{L_{i}}^{L_{0}}R\left(p_k\right)^{\wedge}\cdot_{L_i}^{L_0}\phi+_{L_0}^GR_{t_j}\delta_{L_i}^{L_0}t -{}_{L_{0}}^{G}R_{t_{j}}\bigg({}_{L_{i}}^{L_{0}}Rp_{k} +{}_{L_{i}}^{L_{0}}{t}\bigg)^{\wedge}{}_{L_{0}}^{G}\phi_{t_{j}}+\delta_{L_{0}}^{G}{t}_{t_{j}} \end{align*} \tag{式6}$
结合式4和式6可得 $\delta{^Gp_k}$ ，如下所示：
$\begin{align*} \delta^{G}{p}_{k}\approx-{}_{L_{0}}^{G}R_{t_{j}} \big({}_{L_{i}}^{L_{0}}{R}{p}_{k}+{}_{L_{i}}^{L_{0}}{t}\big)^{\wedge}{}_{L_{0}}^{G}\phi_{t_{j}} +\delta_{L_{0}}^{G}{t}_{t_{j}}-\\{}_{L_{0}}^{G}R_{t_{j}}{}_{L_{i}}^{L_{0}}R\big({p}_{k}\big)^{\wedge}{}_{L_{i}}^{L_{0}}\phi +{}_{L_{0}}^{G}{R}_{t_{j}}\delta_{L_{i}}^{L_{0}}{t}\\ =D\cdot \delta x , \end{align*} \tag{式7}$

其中 $\delta{x}=\begin{bmatrix}\cdots \quad {^G_{L_0}\phi_{t_j}^{T}\quad\delta_{L_0}^{G}t_{t_j}^T}\cdots{^{L_{0}}_{L_i}\phi^{T}\quad\delta_{L_i}^{L_0}t^T} \cdots\end{bmatrix}^{T} \ne\mathbb{R}^{6\left(m+n-2\right)}$ 是优化变量 $x=\begin{bmatrix}\cdots{_{L_0}^GR_{t_j} }\quad{_{L_0}^{G}t_{t_j}}\cdots{_{L_i}^{L_0}R}\quad{_{L_i}^{L_0}t}\cdots\end{bmatrix}$ 的微小扰, $m$ 代表帧数，n代表参考雷达的个数。
并且
$\begin{align*} D =\begin{bmatrix}\vdots&\vdots\\\cdots {D}_{k,p}^{\mathcal{S}}&\cdots {D}_{k,q}^{\varepsilon_{L}}&\cdots\\\vdots&\vdots\end{bmatrix}\in\mathbb{R}^{3N_{l}\times6(m+n-2)} \\ \mathrm{D}_{k,p}^{\mathcal{S}} \begin{cases} \begin{bmatrix}{-{}_{L_{0}}^{G}{R}_{t_{j}}}\left({}_{L_{i}}^{L_{0}}{R}{p}_{k}+{}_{L_{i}}^{L_{0}}{t}\right)^{\wedge}&{I}\end{bmatrix},\quad{\mathrm{if} p=j}\\ {0}_{3\times6},\quad {else} \end{cases}\\ \mathrm{D}_{k,q}^{\mathcal{E}_{L}} \begin{cases} \begin{bmatrix}{-{}_{L_{0}}^{G}{R}_{t_{j}}}{}_{L_{i}}^{L_{0}}{R}({p}_{k})^{\wedge}&{}_{L_{0}}^{G}{R}_{t_{j}}\end{bmatrix},\quad{\mathrm{if~}q=i}\\ {0}_{3\times6},\quad {else} \end{cases} \end{align*}$

注意： 论文中的结果与推导有差异，下面是论文给的结果，待排查！
$\delta^{G}{p}_{k}\approx{\color{red}+}{}_{L_{0}}^{G}R_{t_{j}} \big({}_{L_{i}}^{L_{0}}{R}{p}_{k}+{}_{L_{i}}^{L_{0}}{t}\big)^{\wedge}{}_{L_{0}}^{G}\phi_{t_{j}} +\delta_{L_{0}}^{G}{t}_{t_{j}}{\color{red}+}\\{}_{L_{i}}^{G}R_{t_{j}}\big({p}_{k}\big)^{\wedge}{}_{L_{i}}^{L_{0}}\phi +{}_{L_{0}}^{G}{R}_{t_{j}}\delta_{L_{i}}^{L_{0}}{t}$

上面用到一些近似化：
$\exp(\phi^{\wedge})\approx I+\phi^{\wedge}\tag{式8}$
结合式3到式7有以下结论：
$\begin{align*} \begin{gathered} \lambda_{3}({x}\boxplus\delta{x}) \approx\lambda_{3}({x})+{JD}\delta{x}+\frac{1}{2} \delta{x}^{T}{D}^{T}{HD}\delta{x} \\ =\lambda_{3}({x})+{\bar{J}}\delta{x}+\frac{1}{2}\delta{x}^{T}{\bar{H}}\delta{x}. \end{gathered} \end{align*} \tag{式9}$
根据定理1和定理2的结论可知：