pl_vio线特征·part II

0.引言

• pl_vio线特征·part I

现在CSDN有字数限制了，被迫拆分为两篇文章。

4.线段残差对位姿的导数

• 这一小节理论部分来自这里,当然也是来自原论文。

4.1.直线的观测模型和误差

图2 空间直线投影到像素平面
要想知道线特征的观测模型，我们需要知道线特征从归一化平面到像素平面的投影内参矩阵 $\mathcal{K}$ 。如图2，点 $C$ 和 $D$ 是直线 $\mathcal{L}=({\mathbf{n}}^{?},{\mathbf{d}}^{?}{)}^{?}$ 上两点，点 $c$ 和 $d$ 是它们在像素平面上的投影。 $c=KC$,$d=KD$ ,$K$是相机的内参矩阵。 $\mathbf{n}=[C{]}_{\times }D，l=\left[\begin{array}{ccc}{l}_1& l_2& l_3\end{array}\right]=[c{]}_{\times }d$ 。那么有

$$l=\mathcal{K}\mathbf{n}=\left[\begin{array}{ccc}{f}_y& 0& 0\\ 0& f_x& 0\\ ?f_y{c}_x& ?f_x{c}_y& f_x{f}_y\end{array}\right]\mathbf{n}$$

$$\begin{array}{rl}{l}^i=c{}^i\times d{}^i& =(KC{}^n)\times (KD{}^n)\\ & ={\left[\begin{array}{c}fx{X}_C+cx\\ fy{Y}_C+cy\\ 1\end{array}\right]}_{\times }\left[\begin{array}{c}fx{X}_D+cx\\ fy{Y}_D+cy\\ 1\end{array}\right]\\ & =\left[\begin{array}{ccc}0& ?1& (fy{Y}_C+cy)\\ 1& 0& ?(fx{X}_C+cx)\\ ?(fy{Y}_C+cy)& (fx{X}_C+cx)& 0\end{array}\right]\left[\begin{array}{c}fx{X}_D+cx\\ fy{Y}_D+cy\\ 1\end{array}\right]\\ & =\left[\begin{array}{c}fy(Y_C?Y_D)\\ fx(X_C?X_D)\\ fxfy(X_C{Y}_D?Y_C{X}_D)+fxcy(X_D?X_C)+fycx(Y_D?Y_C)\end{array}\right]\\ & =\left[\begin{array}{ccc}fy& 0& 0\\ 0& fx& 0\\ ?fycx& ?fxcy& fxfy\end{array}\right]\left[\begin{array}{c}{Y}_D?Y_C\\ X_D?X_C\\ X_C{Y}_D?Y_C{X}_D\end{array}\right]\\ & =\left[\begin{array}{ccc}fy& 0& 0\\ 0& fx& 0\\ ?fycx& ?fxcy& fxfy\end{array}\right]{\left[\begin{array}{c}{X}_C\\ Y_C\\ 1\end{array}\right]}_{\times }\left[\begin{array}{c}{X}_D\\ Y_D\\ 1\end{array}\right]\\ & =\mathcal{K}(C{}^n\times D{}^n)\\ & =\mathcal{K}\mathbf{n}\end{array}$$

上式表明，直线的线投影只和法向量有关和方向向量无关。

关于投影的误差，我们不可以直接从两幅图像的线段中得到，因为同一条直线在不同图像线段的长度和大小都是不一样的。衡量线的投影误差必须从空间中重投影回当前的图像中才能定义误差。在给定世界坐标系下的空间直线 ${\mathcal{L}}_l^w$ 和正交表示 ${\mathcal{O}}_l$ ，我们首先使用外参（这也是我们需要优化求解的东西） $T_{cw}=\left[\begin{array}{cc}{R}_{cw}& p_{cw}\\ 0& 1\end{array}\right]$ 将直线变换到相机归一化平面下的观测 $c_i$ 坐标下。然后再将直线利用相机内参投影到成像平面上得到投影线段 $l_l^{c_i}$ ，然后我们就得到了线的投影误差。我们将线的投影误差定义为图像中观测线段的端点到从空间重投影回像素平面的预测直线的距离。

$$\begin{gathered} {\mathbf{r}}_l\left({\mathbf{z}}_{{\mathcal{L}}_l}^{c_i},\mathcal{X}\right)=\left[\begin{array}{l}d\left({\mathbf{s}}_l^{c_i},{\mathbf{l}}_l^{c_i}\right)\\ d\left({\mathbf{e}}_l^{c_i},{\mathbf{l}}_l^{c_i}\right)\end{array}\right] \\ d(\mathbf{s},1)=\frac{{\mathbf{s}}^{?}\mathbf{l}}{\sqrt{l_1^2+l_2^2}} \end{gathered}$$

其中 ${\mathbf{s}}_l^{c_i}$ 和 ${\mathbf{e}}_l^{c_i}$ 是图像中观测到的线段端点， ${\mathbf{l}}_l^{c_i}$ 是重投影的预测的直线。

double FeatureManager::reprojection_error( Vector4d obs, Matrix3d Rwc, Vector3d twc, Vector6d line_w ) {

    double error = 0;

    Vector3d n_w, d_w;
    n_w = line_w.head(3);
    d_w = line_w.tail(3);

    Vector3d p1, p2;
    p1 << obs[0], obs[1], 1;
    p2 << obs[2], obs[3], 1;
	// 根据外参将line从世界坐标系转到相机归一化平面坐标系
    Vector6d line_c = plk_from_pose(line_w,Rwc,twc);
    Vector3d nc = line_c.head(3);
    double sql = nc.head(2).norm();
    nc /= sql;

    error += fabs( nc.dot(p1) );
    error += fabs( nc.dot(p2) );
	
    return error / 2.0;
}

这里误差是归一化平面坐标系的误差，因此观测也应该要求是归一化平面，注意中间有个从像素坐标系到归一化平面坐标系的转换，这里没列出来。

误差求解函数在这里。

这个函数实际上只用在了外点剔除这里，真正的优化误差求解是在优化器那里定义的。而且感觉这里的实现坐标有点问题？

4.2.误差雅克比推导

如果要优化的话，需要知道误差的雅克比矩阵：

线特征在VIO下根据链式求导法则：

$${\mathbf{J}}_l=\frac{?{\mathbf{r}}_l}{?{\mathbf{l}}^{c_i}}\frac{?{\mathbf{l}}^{c_i}}{?{\mathcal{L}}^{c_i}}\left[\frac{?{\mathcal{L}}^{c_i}}{?\delta {\mathbf{x}}^i}\frac{?{\mathcal{L}}^{c_i}}{?{\mathcal{L}}^w}\frac{?{\mathcal{L}}^w}{?\delta \mathcal{O}}\right]$$

其中第一项 $\frac{?{\mathbf{r}}_l}{?{\mathbf{l}}^{c_i}}$ ，因为

$$\begin{gathered} {\mathbf{r}}_l=\left[\begin{array}{c}\frac{{\mathbf{s}}^T\mathbf{l}}{\sqrt{l_1^2+l_2^2}}\\ \frac{{\mathbf{e}}^T\mathbf{l}}{\sqrt{l_1^2+l_2^2}}\end{array}\right]=\left[\begin{array}{c}\frac{u_s{l}_1+v_s{l}_2}{\sqrt{l_1^2+l_2^2}}\\ \frac{u_e{l}_1+v_e{l}_2}{\sqrt{l_1^2+l_2^2}}\end{array}\right] \\ \mathbf{s}=\left[\begin{array}{ccc}{u}_s& v_s& 1\end{array}\right] \\ \mathbf{e}=\left[\begin{array}{ccc}{u}_e& v_e& 1\end{array}\right] \\ \mathbf{l}=\left[\begin{array}{ccc}{l}_1& l_2& l_3\end{array}\right] \end{gathered}$$

所以：

$$\begin{array}{rl}\frac{?{\mathbf{r}}_l}{?\mathbf{l}}& =\left[\begin{array}{lll}\frac{?r_1}{?l_1}& \frac{?r_1}{?l_2}& \frac{?r_1}{?l_3}\\ \frac{?r_2}{?l_1}& \frac{?r_2}{?l_2}& \frac{?r_2}{?l_3}\end{array}\right]\\ & ={\left[\begin{array}{ccc}\frac{?l_1{\mathbf{s}}_l^{?}\mathbf{l}}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{3}{2}\right)}}+\frac{u_s}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{1}{2}\right)}}& \frac{?l_2{\mathbf{s}}_l^{?}\mathbf{l}}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{3}{2}\right)}}+\frac{v_s}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{1}{2}\right)}}& \frac{1}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{1}{2}\right)}}\\ \frac{?l_1{\mathbf{e}}_l^{?}\mathbf{l}}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{3}{2}\right)}}+\frac{e_s}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{1}{2}\right)}}& \frac{?l_2{\mathbf{e}}_l^{?}\mathbf{l}}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{3}{2}\right)}}+\frac{v_e}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{1}{2}\right)}}& \frac{1}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{1}{2}\right)}}\end{array}\right]}_{2\times 3}\end{array}\text{\hspace{1em}?}$$

第二项 $\frac{?{\mathbf{l}}^{c_i}}{?{\mathcal{L}}^{c_i}}$（像素坐标到相机归一化坐标，相差一个映射矩阵） ，因为

$$\begin{gathered} \mathbf{l}=\mathcal{K}\mathbf{n} \\ \mathcal{L}=\left[\begin{array}{cc}\mathbf{n}& \mathbf{d}\end{array}\right] \end{gathered}$$

所以：

$$\begin{array}{rl}\frac{?{\mathrm{l}}^{c_i}}{?{\mathcal{L}}_i^{c_i}}& =\left[\begin{array}{cc}\frac{?\mathbf{l}}{\mathbf{n}}& \frac{?\mathbf{l}}{\mathbf{d}}\end{array}\right]\\ & ={\left[\begin{array}{ll}\mathcal{K}& 0\end{array}\right]}_{3\times 6}\end{array}\text{\hspace{1em}?}$$

最后一项矩阵包含两个部分，一个是相机坐标系下线特征对的旋转和平移的误差导数，第二个是直线对正交表示的四个参数增量的导数

第一部分中，

$$\delta {\mathbf{x}}_i=\left[\delta \mathbf{p},\delta \mathbf{\theta },\delta \mathbf{v},\delta {\mathbf{b}}_a^{b_i},\delta {\mathbf{b}}_g^{b_i}\right]$$

在VIO中，如果要计算线特征的重投影误差，需要将在世界坐标系 $w$ 下的线特征变换到IMU坐标系 $b$ 下，再用外参数 ${\mathbf{T}}_{\mathbf{bc}}$ 变换到相机坐标系 $c$ 下。所以

$$\begin{array}{rl}{\mathcal{L}}_c& ={\mathcal{T}}_{bc}^{?1}{\mathcal{T}}_{wb}^{?1}{\mathcal{L}}_w\\ & ={\mathcal{T}}_{bc}^{?1}{\left[\begin{array}{c}{\mathbf{R}}_{wb}^{?}\left({\mathbf{n}}^w+\left[{\mathbf{d}}^w\right]\times {\mathbf{p}}_{wb}\right)\\ {\mathbf{R}}_{wb}^{?}{\mathbf{d}}^w\end{array}\right]}_{6\times 1}\end{array}$$

其中
$$\begin{gathered} {\mathcal{T}}_{bc}=\left[\begin{array}{cc}{\mathbf{R}}_{bc}& {\left[{\mathbf{p}}_{bc}\right]}_{\times }{\mathbf{R}}_{bc}\\ 0& {\mathbf{R}}_{bc}\end{array}\right] \\ {\mathcal{T}}_{bc}^{?1}=\left[\begin{array}{cc}{\mathbf{R}}_{\mathbf{bc}}^{?}& ?{\mathbf{R}}_{\mathbf{bc}}^{?}[{\mathbf{p}}_{\mathbf{bc}}{]}_{\times }\\ 0& {\mathbf{R}}_{\mathbf{bc}}^{?}\end{array}\right] \end{gathered}$$
$$?[a{]}_{\times }b=[b{]}_{\times }a$$
线特征 $\mathcal{L}$ 只优化状态变量中的位移和旋转，所以只需要对位移和旋转求导，其他都是零。下面我们来具体分析旋转和位移的求导。首先是线特征对旋转的求导：

$$\begin{array}{rl}\frac{?{\mathcal{L}}_c}{?\delta {\theta }_{b{b}^{\prime }}}& ={\mathcal{T}}_{bc}^{?1}\left[\begin{array}{c}\frac{?\left(\mathbf{I}?{\left[\delta {\mathbf{\theta }}_{b{b}^{\prime }}\right]}_{\times }\right){\mathbf{R}}_{wb}^{?}\left({\mathbf{n}}^w+{\left[{\mathbf{d}}^w\right]}_{\times }{\mathbf{p}}_{wb}\right)}{?\delta {\mathbf{\theta }}_{b{b}^{\prime }}}]\\ \frac{?\left(\mathbf{I}?{\left[\delta {\mathbf{\theta }}_{b{b}^{\prime }}\right]}_{\times }^{?}\right){\mathbf{R}}_{wb}^{?}{\mathbf{d}}^w}{?\delta {\mathbf{\theta }}_{b{b}^{\prime }}}\end{array}\right]\\ & ={\mathcal{T}}_{bc}^{?1}{\left[\begin{array}{c}{\left[{\mathbf{R}}_{wb}^{?}\left({\mathbf{n}}^w+{\left[{\mathbf{d}}^w\right]}_{\times }{\mathbf{p}}_{wb}\right)\right]}_{\times }]\\ {\left[{\mathbf{R}}_{wb}^{?}{\mathbf{d}}^w\right]}_{\times }\end{array}\right]}_{6\times 3}\end{array}\text{\hspace{1em}?}$$

然后是线特征对位移的求导：

$$\begin{array}{rl}\frac{?{\mathcal{L}}_c}{?\delta {\mathbf{p}}_{\mathbf{b}{\mathbf{b}}^{\prime }}}& ={\mathcal{T}}_{bc}^{?1}\left[\begin{array}{c}\frac{?{\mathbf{R}}_{wb}^{?}\left({\mathbf{n}}^w+{\left[{\mathbf{d}}^w\right]}_{\times }\left({\mathbf{p}}_{wb}+\delta {\mathbf{p}}_{b{b}^{\prime }}\right)\right)}{?\delta {\mathbf{p}}_{b{b}^{\prime }}}\\ \frac{?{\mathbf{R}}_{wb}^{?}{\mathbf{d}}^w}{?\delta {\mathbf{p}}_{b{b}^{\prime }}}\end{array}\right]\\ & ={\mathcal{T}}_{bc}^{?1}{\left[\begin{array}{c}{\mathbf{R}}_{wb}^{?}{\left[{\mathbf{d}}^w\right]}_{\times }\\ 0\end{array}\right]}_{6\times 3}\end{array}\text{\hspace{1em}?}$$

第二部分中 $\frac{?{\mathcal{L}}^{c_i}}{?{\mathcal{L}}^w}\frac{?{\mathcal{L}}^w}{?\delta \mathcal{O}}$ ，先解释第一个 $\frac{?{\mathcal{L}}^{c_i}}{?{\mathcal{L}}^w}$

$${\mathcal{L}}^c={\mathcal{T}}_{wc}^{?1}{\mathcal{L}}^w$$

所以 $\frac{?{\mathcal{L}}^{c_i}}{?{\mathcal{L}}^w}={\mathcal{T}}_{wc}^{?1}$

然后后面的 $\frac{?{\mathcal{L}}^w}{?\delta \mathcal{O}}$ 有两种思路，先介绍第一种：

$$\begin{gathered} \frac{?{\mathcal{L}}^w}{?\delta \mathcal{O}}=\left[\begin{array}{cccc}\frac{?{\mathcal{L}}^w}{?{\psi }_1}& \frac{?{\mathcal{L}}^w}{?{\psi }_2}& \frac{?{\mathcal{L}}^w}{?{\psi }_3}& \frac{?{\mathcal{L}}^w}{??}\end{array}\right] \\ \frac{?{\mathcal{L}}^w}{?{\psi }_1}=\frac{?{\mathcal{L}}^w}{?\mathbf{U}}\frac{?\mathbf{U}}{?{\psi }_1} \\ \frac{?{\mathcal{L}}^w}{??}=\frac{?{\mathcal{L}}^w}{?\mathbf{w}}\frac{?\mathbf{w}}{?{?}_1} \end{gathered}$$

其中 $\mathcal{L}$ 对 $\mathbf{U}$ 和 $\mathbf{w}=[w_1,w_2]$ 求导，因为 ${\mathcal{L}}^w={\left[\begin{array}{cc}{w}_1{\mathbf{u}}_1^{?}& w_2{\mathbf{u}}_2^{?}\end{array}\right]}^{?}$ ，所以

$$\begin{array}{rl}\frac{?\mathcal{L}}{?\mathbf{U}}& ={\left[\begin{array}{ccc}\frac{?\mathcal{L}}{?{\mathbf{U}}_1}& \frac{?\mathcal{L}}{?{\mathbf{U}}_2}& \frac{?\mathcal{L}}{?{\mathbf{U}}_3}\end{array}\right]}_{6\times 9}\\ & =\left[\begin{array}{ccc}{w}_{1(3\times 3)}& 0& 0\\ 0& w_{2(3\times 3)}& 0\end{array}\right]\end{array}\text{\hspace{1em}?}$$

$$\begin{array}{rl}\frac{?\mathcal{L}}{?\mathbf{w}}& ={\left[\begin{array}{cc}\frac{?\mathcal{L}}{?w_1}& \frac{?\mathcal{L}}{?w_2}\end{array}\right]}_{6\times 2}\\ & =\left[\begin{array}{cc}{\mathbf{u}}_1& 0\\ 0& {\mathbf{u}}_2\end{array}\right]\end{array}\text{\hspace{1em}?}$$

然后是 $\mathbf{U}$ 对 $\psi$ 和 $\mathbf{W}$ 对 $?$ 的求导，

因为 $\begin{array}{rl}{\mathbf{U}}^{\prime }& \approx \mathbf{U}\left(\mathbf{I}+[\delta \psi {]}_{\times }\right)\end{array}$ ，所以

$$\begin{gathered} {\left[\begin{array}{ccc}{\mathbf{u}}_1& {\mathbf{u}}_2& {\mathbf{u}}_3\end{array}\right]}^{\prime }=\left[\begin{array}{ccc}{\mathbf{u}}_1& {\mathbf{u}}_2& {\mathbf{u}}_3\end{array}\right]+{\left[\begin{array}{ccc}{\mathbf{u}}_1& {\mathbf{u}}_2& {\mathbf{u}}_3\end{array}\right]}_{\times }\delta \psi \\ \frac{{\left[\begin{array}{ccc}{\mathbf{u}}_1& {\mathbf{u}}_2& {\mathbf{u}}_3\end{array}\right]}^{\prime }?\left[\begin{array}{ccc}{\mathbf{u}}_1& {\mathbf{u}}_2& {\mathbf{u}}_3\end{array}\right]}{\delta \psi }={\left[\begin{array}{ccc}{\mathbf{u}}_1& {\mathbf{u}}_2& {\mathbf{u}}_3\end{array}\right]}_{\times } \\ \frac{?\mathbf{U}}{?{\psi }_1}=\left[\begin{array}{ccc}0& {\mathbf{u}}_3& ?{\mathbf{u}}_2\end{array}\right] \\ \frac{?\mathbf{U}}{?{\psi }_2}=\left[\begin{array}{ccc}?{\mathbf{u}}_3& 0& {\mathbf{u}}_1\end{array}\right] \\ \frac{?\mathbf{U}}{?{\psi }_1}=\left[\begin{array}{ccc}{\mathbf{u}}_2& ?{\mathbf{u}}_1& 0\end{array}\right] \\ \frac{?\mathbf{w}}{??}=\left[\begin{array}{c}?w_2\\ w_1\end{array}\right] \end{gathered}$$

所以，可得

$$\begin{array}{rl}\frac{?{\mathcal{L}}^w}{?\delta \mathcal{O}}& =\left[\begin{array}{cccc}\frac{?{\mathcal{L}}^w}{?{\psi }_1}& \frac{?{\mathcal{L}}^w}{?{\psi }_2}& \frac{?{\mathcal{L}}^w}{?{\psi }_3}& \frac{?{\mathcal{L}}^w}{??}\end{array}\right]\\ & =\left[\begin{array}{cccc}\frac{?{\mathcal{L}}^w}{?\mathbf{U}}\frac{?\mathbf{U}}{?{\psi }_1}& \frac{?{\mathcal{L}}^w}{?\mathbf{U}}\frac{?\mathbf{U}}{?{\psi }_2}& \frac{?{\mathcal{L}}^w}{?\mathbf{U}}\frac{?\mathbf{U}}{?{\psi }_3}& \frac{?{\mathcal{L}}^w}{?\mathbf{w}}\frac{?\mathbf{w}}{??}\end{array}\right]\\ & ={\left[\begin{array}{cccc}0& ?w_1{\mathbf{u}}_3& w_1{\mathbf{u}}_2& ?w_2{\mathbf{u}}_1\\ w_2{\mathbf{u}}_3& 0& ?w_2{\mathbf{u}}_1& w_1{\mathbf{u}}_2\end{array}\right]}_{6\times 4}\end{array}\text{\hspace{1em}?}$$

---

4.3.误差雅可比求导简洁版(不含imu坐标系转换)

• $L^W$表示在世界坐标系的表示，$L^C$表示在相机坐标系下的表示；
• $L^n$表示归一化平面上的线，$L^I$表示在图像坐标系下的线；

图中的$I_L$表示直线$\mathcal{L}$在图像平面的投影，所以定义误差项为（就是简单的两个点到直线的距离）：

$${\mathbf{r}}_{\mathbf{L}}=\left[\begin{array}{c}{\mathbf{r}}_1\\ {\mathbf{r}}_2\end{array}\right]=\left[\begin{array}{c}\frac{c^{T}I}{\sqrt{l_1^2+l_2^2}}\\ \frac{d^{T}I}{\sqrt{l_1^2+l_2^2}}\end{array}\right]\text{\hspace{1em}(15)}$$

求解Jacobian
跟对3D点的优化问题一样，就是从误差不停的递推到位姿以及直线表示上，用到最最最基本的求导的链式法则：

通用的公式如下：

$$\frac{?{\mathbf{r}}_{\mathbf{L}}}{?X}=\frac{?{\mathbf{r}}_{\mathbf{L}}}{?L^I}\frac{?L^I}{?L^n}\frac{?L^n}{?L^c}\begin{array}{r}?\begin{array}{ll}\frac{?L^c}{?\theta }& \text{?X=}\theta \\ \frac{?L^c}{?t}& \text{?X=t}\\ \frac{?L^c}{?L^w}\frac{?L^w}{?(\theta ,?)}& \text{?X=}{L}^w\end{array}\end{array}\text{\hspace{1em}(16)}$$

先对前面最通用的部分进行求解：

第一部分：

$$\begin{array}{rl}\frac{?{\mathbf{r}}_{\mathbf{L}}}{?L^I}& =\left[\begin{array}{ccc}\frac{?r1}{?l_1}& \frac{?r1}{?l_2}& \frac{?r1}{?l_3}\\ \frac{?r2}{?l_1}& \frac{?r2}{?l_2}& \frac{?r2}{?l_3}\end{array}\right]\\ & ={\left[\begin{array}{ccc}\frac{?l_1{c}^T{L}^I}{(l_1^2+l_2^2{)}^{\frac{3}{2}}}+\frac{u_c}{(l_1^2+l_2^2{)}^{\frac{1}{2}}}& \frac{?l_2{c}^T{L}^I}{(l_1^2+l_2^2{)}^{\frac{3}{2}}}+\frac{v_c}{(l_1^2+l_2^2{)}^{\frac{1}{2}}}& \frac{1}{(l_1^2+l_2^2{)}^{\frac{1}{2}}}\\ \frac{?l_1{d}^T{L}^I}{(l_1^2+l_2^2{)}^{\frac{3}{2}}}+\frac{u_d}{(l_1^2+l_2^2{)}^{\frac{1}{2}}}& \frac{?l_2{d}^T{L}^I}{(l_1^2+l_2^2{)}^{\frac{3}{2}}}+\frac{v_d}{(l_1^2+l_2^2{)}^{\frac{1}{2}}}& \frac{1}{(l_1^2+l_2^2{)}^{\frac{1}{2}}}\end{array}\right]}_{2\times 3}\end{array}\text{\hspace{1em}(17)}$$

其中：

$l1,l2,l3$表示图像坐标系下直线的三个参数；
$u_c,v_c$表示点$c$的$xy$坐标值，$u_d,v_d$同理；
第二部分：

根据公式（13）可知：

$$\frac{?L^I}{?L^n}={\mathcal{K}}_{3\times 3}\text{\hspace{1em}(18)}$$

第三部分：

由公式（6）和（13）可知，直线的Plucker表示在归一化平面上只用了其中的法向量部分，因此若有$\mathcal{L}{}^c={\left[\mathbf{n}{}^{\mathbf{c}},\mathbf{d}{}^{\mathbf{c}}\right]}^T$，那么$\mathcal{L}{}^n=\mathbf{n}{}^{\mathbf{c}}$，所以求导有：

$$\frac{?L^n}{?L^c}={\left[\begin{array}{cc}{\mathbf{I}}_{3\times 3}& 0_{3\times 3}\end{array}\right]}_{3\times 6}\text{\hspace{1em}(19)}$$

第四部分就分这几种情况进行讨论：

对于位姿的姿态部分
根据公式（7）有：

$$\frac{?L{}^c}{?\theta }=\left[\begin{array}{c}\frac{?n_c}{?\theta }\\ \frac{?d_c}{?\theta }\end{array}\right]=\left[\begin{array}{c}\frac{?(R_{wc}^T(n_w+[t_{wc}{]}_{\times }{b}_w))}{?\theta }\\ \frac{?R_{wc}^T{b}_w}{?\theta }\end{array}\right]={\left[\begin{array}{c}[R_{wc}^T(n_w+[t_{wc}{]}_{\times }{b}_w){]}_{\times }\\ [R_{wc}^T{b}_w{]}_{\times }\end{array}\right]}_{6\times 3}\text{\hspace{1em}(20)}$$

上述的推导使用了李群的右扰动模型，即$(R_{wc}Exp(\theta ){)}^T=Exp(?\theta )R_{wc}^T$。

对于位姿的位移部分
同样根据公式（7）有：

$$\frac{?L{}^c}{?t}=\left[\begin{array}{c}\frac{?n_c}{?t}\\ \frac{?d_c}{?t}\end{array}\right]=\left[\begin{array}{c}\frac{?(R_{wc}^T(n_w+[t_{wc}{]}_{\times }{b}_w))}{?t}\\ \frac{?R_{wc}^T{b}_w}{?t}\end{array}\right]={\left[\begin{array}{c}?R_{wc}^T[b_w{]}_{\times }\\ 0\end{array}\right]}_{6\times 3}\text{\hspace{1em}(21)}$$

对于世界坐标系下直线表示部分
这部分按照公式（16）的步骤，依旧分两个部分：

$\frac{?L^c}{?L^w}$部分：
$$\frac{?L^c}{?L^w}=\left[\begin{array}{cc}\frac{?{\mathbf{n}}^{\mathbf{C}}}{?{\mathbf{n}}^{\mathbf{W}}}& \frac{?{\mathbf{n}}^{\mathbf{C}}}{?{\mathbf{b}}^{\mathbf{W}}}\\ \frac{?{\mathbf{d}}^{\mathbf{C}}}{?{\mathbf{n}}^{\mathbf{W}}}& \frac{?{\mathbf{d}}^{\mathbf{C}}}{?{\mathbf{b}}^{\mathbf{W}}}\end{array}\right]=\left[\begin{array}{cc}{\mathrm{R}}_{wc}^T& {\mathrm{R}}_{wc}^T{\left[{\mathbf{t}}_{wc}\right]}_{\times }\\ 0& {\mathrm{R}}_{wc}^T\end{array}\right]\text{\hspace{1em}(22)}$$
$\frac{?L^w}{?(\theta ,?)}$部分，这部分其实还可以继续分，如下：
$$\frac{?L^w}{?(\theta ,?)}=\left[\frac{?L^w}{?\theta },\frac{?L^w}{??}\right]=\left[\frac{?L^w}{?U}\frac{?U}{?\theta },\frac{?L^w}{?W}\frac{?W}{??}\right]$$第一部分
$$\begin{array}{rl}\frac{?L^w}{?U}\frac{?U}{?\theta }& =\frac{?\left[\begin{array}{c}w1{\mathbf{u}}_1\\ w2{\mathbf{u}}_2\end{array}\right]}{?[{\mathbf{u}}_1,{\mathbf{u}}_2,{\mathbf{u}}_3]}\frac{?[{\mathbf{u}}_1,{\mathbf{u}}_2,{\mathbf{u}}_3]}{?\theta }\\ & ={\left[\begin{array}{ccc}w1& 0& 0\\ 0& w2& 0\end{array}\right]}_{6\times 9}{\left[\begin{array}{ccc}0& ?u3& u2\\ u3& 0& ?u1\\ ?u2& u1& 0\end{array}\right]}_{9\times 3}\\ & ={\left[\begin{array}{ccc}0& ?w1u3& w1u2\\ ?w2u3& 0& ?w2u1\end{array}\right]}_{6\times 3}\end{array}\text{\hspace{1em}(23)}$$ 第二部分
$$\begin{array}{rl}\frac{?L^w}{?W}\frac{?W}{??}& =\frac{?\left[\begin{array}{c}w1{\mathbf{u}}_1\\ w2{\mathbf{u}}_2\end{array}\right]}{?[w1,w2{]}^T}\frac{?[w1,w2{]}^T}{??}\\ & ={\left[\begin{array}{cc}u1& 0\\ 0& u2\end{array}\right]}_{6\times 2}{\left[\begin{array}{c}?w2\\ w1\end{array}\right]}_{2\times 1}\\ & ={\left[\begin{array}{c}?w2u1\\ w1u2\end{array}\right]}_{6\times 1}\end{array}\text{\hspace{1em}(24)}$$ 其中$$w1=cos(?),w2=sin(?)$$。
两个部分合起来为：
$$\frac{?L^w}{?(\theta ,?)}={\left[\begin{array}{cc}{\mathrm{R}}_{wc}^T& {\mathrm{R}}_{wc}^T{\left[{\mathbf{t}}_{wc}\right]}_{\times }\\ 0& {\mathrm{R}}_{wc}^T\end{array}\right]}_{6\times 6}{\left[\begin{array}{cccc}0& ?w1u3& w1u2& ?w2u1\\ ?w2u3& 0& ?w2u1& w1u2\end{array}\right]}_{6\times 4}\text{\hspace{1em}(25)}$$

最后就是上述推导过程中确实有很多地方向量的notation没有统一，可能有些比较容易混淆，这里确实是因为各个论文的表示不太一样，导致写公式的时候不太一样，自己又偷了个懒，不过该注释的地方都进行了注释。目前比较流行的表示应该是 $L=[\mathbf{n},\mathbf{d}{]}^T$ 或者 $L=[\mathbf{n},\mathbf{v}{]}^T$ ，其中 $\mathbf{n}$ 表示法向量， $\mathbf{d}$ 或者 $\mathbf{v}$ 表示方向向量。

---

4.4.相关代码

优化入口，ceres,主要实现在这里。

• todo:导数填充