对于第\(i\)帧parking slot 建模为车位开口两个点\(p_0, p_1\),AVM到body坐标系外参为\(T^b_a\), 对于第\(j\)帧同样看到了此库位。因此可以构建误差函数为：
\[
\mathbf{F} = {q^b_a}^{-1} * \left( \left( {q^w_{b_j}}^{-1} * \left( \left( q^w_{b_i} * \left(q^b_a * p^a_{i_n} + t^b_a\right) + t^w_{b_i} \right) - t^w_{b_j}\right) \right) -t^b_a \right) - p^a_{j_n}
\]
这里AVM观测为2D，先写成3D方便推导。
误差函数对\(t^w_{b_i}, q^w_{b_i}\)的jacobian为：
\[
{\frac{\partial F}{\partial \left[ t^w_{b_i}, q^w_{b_i} \right]}}_{3\times6} = \begin{bmatrix} {q^b_a}^{-1} * {q^w_{b_j}}^{-1} & {q^b_a}^{-1} * {q^w_{b_j}}^{-1} * q^w_{b_i} * -1 * \left[ q^b_a * p^a_{i_n} + t^b_a \right]_{\times} \end{bmatrix}
\]
关于\(t^w_{b_i}\)部分，去除无关项后为:
\[
{q^b_a}^{-1} * {q^w_{b_j}}^{-1} * t^w_{b_i}
\]
因此：
\[
\frac{\partial \mathbf{F}}{\partial t^w_{b_i}} = {q^b_a}^{-1} * {q^w_{b_j}}^{-1}
\]
关于\(q^w_{b_i}\)部分，去除无关项后为:
\[
{q^b_a}^{-1} * {q^w_{b_j}}^{-1} * q^w_{b_i} * \left( q^b_a * p^a_{i_n} + t^b_a \right)
\]
因此：
\[
\frac{\partial \mathbf{F}}{\partial q^w_{b_i}} = {q^b_a}^{-1} * {q^w_{b_j}}^{-1} * q^w_{b_i} * -1 * \left[ q^b_a * p^a_{i_n} + t^b_a \right]_{\times}
\]
误差函数对\(t^w_{b_j}, q^w_{b_j}\)的jacobian为：
\[
{\frac{\partial F}{\partial \left[ t^w_{b_j}, q^w_{b_j} \right]}}_{3\times6} = \begin{bmatrix} -1 * {q^b_a}^{-1} * {q^w_{b_j}}^{-1} & {q^b_a}^{-1} * \left[ {q^w_{b_j}}^{-1} * \left( \left( q^w_{b_i} * \left(q^b_a * p^a_{i_n} + t^b_a\right) + t^w_{b_i} \right) - t^w_{b_j}\right) \right]_{\times} \end{bmatrix}
\]
误差函数对\(t^b_a, q^b_a\)的jacobian为：
\[
{\frac{\partial F}{\partial \left[ t^b_a, q^b_a \right]}}_{3\times6} = \begin{bmatrix}
{q^b_a}^{-1} * \left( {q^w_{b_j}}^{-1} * q^w_{b_i} - I\right) \quad
-A \left[p^a_{i_n}\right]_{\times} + \left[ A p^a_{i_n} \right]_{\times}
+ \left[ {q^b_a}^{-1} * {q^w_{b_j}}^{-1} * q^w_{b_i} * q^b_a * p^a_{i_n} \right]_{\times}
\end{bmatrix}
\]
关于\(t^b_a\)部分，去除无关项后,导数为变量前的系数:
\[
\frac{\partial \mathbf{F}}{\partial t^b_a} = {q^b_a}^{-1} * {q^w_{b_j}}^{-1} * q^w_{b_i} - {q^b_c}^{-1} = {q^b_a}^{-1} * \left( {q^w_{b_j}}^{-1} * q^w_{b_i} - I\right)
\]
关于\(q^b_a\)部分, 去除无关项后为：
\[
{q^b_a}^{-1} * {q^w_{b_j}}^{-1} * q^w_{b_i} * q^b_a * p^a_{i_n} +
{q^b_a}^{-1} * \left( {q^w_{b_j}}^{-1} * \left( \left( \left( q^w_{b_i} * t^b_c + t^w_{b_i} \right) - t^w_{b_j} \right) -t^b_c \right) \right)
\]
根据导数加法法则分成两个部分：
\[
(f(x) + g(x))^{'} = {f^{'}}(x) + {g^{'}}(x)
\]
第一部分：
令：
\[
A = {q^b_a}^{-1} * {q^w_{b_j}}^{-1} * q^w_{b_i} * q^b_a
\]
右乘微小量李代数得：
\[
{\left[q^b_a * exp(\theta^{\wedge}) \right] }^{-1} * {q^w_{b_j}}^{-1} * q^w_{b_i} * \left[q^b_a * exp(\theta^{\wedge}) \right] * p^a_{i_n} \\ = (I- \theta^{\wedge}） * A * (I + \theta^{\wedge}) \\ = A + A \theta^{\wedge} - \theta^{\wedge} A - \theta^{\wedge} A \theta^{\wedge} \\
\approx A + A \theta^{\wedge} - \theta^{\wedge} A
\]
其中：
\[
\theta^{\wedge} A \theta^{\wedge}
\]
为二阶无穷小，可省略。
则导数为：
\[
\begin{aligned}
{\frac{\partial \mathbf{F}}{\partial q^b_a}}_1 &= \lim_{\delta \theta^b_c \rightarrow 0} \frac{ \left( A + A \theta^{\wedge} - \theta^{\wedge} A \right) * p^a_{i_n} - A p^a_{i_n} }{\delta \theta^b_c} \\
&= \frac{\left( A\theta^{\wedge} - \theta^{\wedge} A \right) p^a_{i_n}}{\delta \theta^b_c} \\
&= -A \left[p^a_{i_n}\right]_{\times} + \left[ A p^a_{i_n} \right]_{\times}
\end{aligned}
\]
第二部分：
\[
{\frac{\partial \mathbf{F}}{\partial q^b_a}}_2 = \left[ {q^b_a}^{-1} * {q^w_{b_j}}^{-1} * q^w_{b_i} * q^b_a * p^a_{i_n} \right]_{\times}
\]
误差函数对\(p^a_{i_n}\)的jacobian为：
令3D到2D降维矩阵为:
\[
reduce_{3\times2} = \begin{bmatrix} 1 & 0 \\ 0 & 1 \\ 0 & 0 \end{bmatrix}
\]
\[
{\frac{\partial F}{\partial \left[ p^a_{i_n} \right]}}_{3\times2} = \begin{bmatrix} {q^b_a}^{-1} * {q^w_{b_j}}^{-1} * q^w_{b_i} * q^b_a * reduce \end{bmatrix}
\]
Demo
Reference
[0]