SLAM问题中平面与平面夹角约束的雅可比推导

1.1 理论层面：

• 当拿到平面和平面匹配关系以后，要利用此约束来优化当前的位姿与此平面。所以对于 g2o 实现来说，我们需要一个二元边。
• 此二元边的两个顶点为 VertexPlane 和 VertexSE3Expmap ，其中 VertexSE3Expmap 为 g2o 官方定义的，以 SE3Quat 为优化对象的顶点，而 VertexPlane 为自己定义的，以 Plane3D 类为优化对象的顶点。
• 由于使用 hessian 方法表达平面会有过参数化的问题，也就是在优化的时候会变成一个自带约束（因为四个参数表示平面的三个自由度，所以这四个参数一定是自带约束的）的优化问题，这样会有问题。所以在此优化问题中，使用球坐标参数化形式作为平面的表示方法 (具体参照 球坐标)。
• 由于使用球坐标表达误差求雅可比难求，所以这里误差采用 Hessian 的表达形式
• 总结：对于 g2o 内部运算（或者说最小二乘的运算中）涉及到的增量以及雅各比矩阵都采用球坐标，对于误差的计算采用 Hessian 表达。雅各比中使用链式法则将两者串联（详细见雅各比部分）
• 对于这个误差的计算，想了四种方案，接下来详细介绍

class EdgeParallelPlane : public BaseBinaryEdge<2, Plane3D, VertexPlane, VertexSE3Expmap> 

1.1.1 误差和雅可比计算方案：

• 对于所有的方案，共同点都是会遇到维度问题
• 因为 $T_{wc}$ 是一个 4x4 的矩阵，所以对于这里的向量，应该是用 4x1 的齐次坐标表示，而与 3D 点的齐次坐标 $[x,y,z,1{]}^T$ 不同，向量的齐次坐标应该是 $[a,b,c,0{]}^T$，在之后的式子中使用 $\bar{n}$ 表示 n 向量为齐次坐标的形式，即 $\bar{n}=\left[\begin{array}{c}{n}_x\\ n_y\\ n_z\\ 0\end{array}\right]$，而 $n$ 表示非齐次坐标 $n=\left[\begin{array}{c}{n}_x\\ n_y\\ n_z\end{array}\right]$
• 然而，使用 4x1 的齐次坐标表达向量没办法表示两向量间的夹角
• 这里复习一下向量夹角的表达方式，假设有向量 a,b。点乘：$a\cdot b=a^{T}b=|a||b|\cos \theta$ ; 叉乘：$|a\times b|=|a||b|\sin \theta$
• 按照上述公式，夹角的求法只能使用非齐次向量表示
• 所以需要有一个矩阵 P =${\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\end{array}\right]}_{3\times 4}$ 将变换后的齐次向量 ($T_{wc}{\bar{n}}_{c齐次}(4\times 1)$) 转换成非齐次向量 ($P{T}_{wc}{n}_{c齐次}(3\times 1)$)，然后再用点乘或者叉乘进行夹角的表示
• 注意：对于下面所有的雅可比，都是采用先平移后旋转（与 SLAM 十四讲对应），而 ORB_SLAM 上是先旋转后平移，所以写的时候要把前三列与后三列调换，参考：6. 雅各比中旋转和平移的顺序问题

1.1.1.1 方案一：最终采用

1.1.1.1.1 误差：

此方案的误差使用叉乘来表达

• ！但是对于叉乘来说，得到的是一个向量，也就是说此时的 Error 是三维的，如果按照叉乘求雅各比的话最终优化的结果是 $[0,0,0{]}^T$，幸运的是，当 a 和 b 的模长都等于 1 时（或者不为零），并且叉乘的结果等于 $[0,0,0{]}^T$ 的时候，等价于两向量的夹角 $\sin \theta =0$ 。也就是说两向量叉乘的结果等于 $[0,0,0{]}^T$ 是这两个向量夹角为 0/180 的充分必要条件。
• 所以最终使得 Error 为 0,就可以保证两向量是平行的
• 由于 ORB_SLAM2 里的优化部分，优化的是 Tcw，所以这里的误差最终要化成含有 Tcw 的表达式，以便后面对其求关于 Tcw 的雅可比

$$\begin{array}{rl}a\times b& =|a||b|\sin \theta n\\ Error& =(P{T}_{wc}\overline{n_c})\times n_w\\ & =(P{T}_{cw}^{-1}\overline{n_c})\times n_w\end{array}$$

1.1.1.1.2 雅各比矩阵 (对 $T_{cw}$）最终采用

1.1.1.1.2.1 结果

$$\begin{array}{r}\frac{\partial (E=(P{T}_{cw}^{-1}\overline{n_c})\times n_w)}{\partial T_{cw}}={\left[\begin{array}{cc}{0}_{3\times 3}& -\left[\begin{array}{ccc}0& -n_{wz}& n_{wy}\\ n_{wz}& 0& -n_{wx}\\ -n_{wy}& n_{wx}& 0\end{array}\right]\cdot R_{cw}^T\cdot \left[\begin{array}{ccc}0& -n_{cz}& n_{cy}\\ n_{cz}& 0& -n_{cx}\\ -n_{cy}& n_{cx}& 0\end{array}\right]\end{array}\right]}_{3\times 6}\end{array}$$

1.1.1.1.2.2 推导过程:

对于 $T^{-1}$ 求法所用到的性质可以看 雅各比和优化问题/SLAM中李代数的扰动模型如何对逆变换矩阵，或者逆旋转矩阵求导？ - 知乎

$$\begin{array}{rl}Error& =(P\cdot T_{cw}^{-1}\cdot \overline{n_c})\times n_w\\ & =(P\cdot T_{cw}^{-1}\cdot \overline{n_c}{)}^{\wedge }\cdot n_w\\ & =-{n_w}^{\wedge }\cdot (P\cdot T_{cw}^{-1}\cdot \overline{n_c})\\ & =-{n_w}^{\wedge }\cdot P\cdot T_{cw}^{-1}\cdot \overline{n_c}\\ & \\ \frac{\partial (E=(P{T}_{cw}^{-1}\overline{n_c})\times n_w)}{\partial T_{cw}}& =\frac{\partial Error}{\partial \delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{-n_w^{\wedge }P[\exp (\delta {\xi }^{\wedge })\exp ({\xi }^{\wedge }){]}^{-1}\overline{n_c}+n_w^{\wedge }P[\exp ({\xi }^{\wedge }){]}^{-1}\overline{n_c}}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{-n_w^{\wedge }P[\exp ({\xi }^{\wedge }){]}^{-1}[\exp (\delta {\xi }^{\wedge }){]}^{-1}\overline{n_c}+n_w^{\wedge }P[\exp ({\xi }^{\wedge }){]}^{-1}\overline{n_c}}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{-n_w^{\wedge }P\exp (-{\xi }^{\wedge })\exp (-\delta {\xi }^{\wedge })\overline{n_c}+n_w^{\wedge }P\exp (-{\xi }^{\wedge })\overline{n_c}}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{-n_w^{\wedge }P\exp (-{\xi }^{\wedge })(I-\delta {\xi }^{\wedge })\overline{n_c}+n_w^{\wedge }P\exp (-{\xi }^{\wedge })\overline{n_c}}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{n_w^{\wedge }P\exp (-{\xi }^{\wedge })\delta {\xi }^{\wedge }\overline{n_c}}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{n_w^{\wedge }\cdot {\left[\begin{array}{cc}{I}_{3\times 3}& 0_{3\times 1}\end{array}\right]}_{3\times 4}\cdot {\left[\begin{array}{cc}{R}_{cw}^T& -R_{cw}^{T}t\\ 0& 1\end{array}\right]}_{4\times 4}{\left[\begin{array}{cc}\delta {\varphi }^{\wedge }& \delta \rho \\ 0^T& 0\end{array}\right]}_{4\times 4}\cdot {\left[\begin{array}{c}{n}_c\\ 0\end{array}\right]}_{4\times 1}}{{\left[\begin{array}{cc}\delta \rho & \delta \varphi \end{array}\right]}^T}\\ & =\underset{\delta \xi \to 0}{\lim }\frac{n_w^{\wedge }\cdot R_{cw}^T\delta {\varphi }^{\wedge }{n}_c}{{\left[\begin{array}{cc}\delta \rho & \delta \varphi \end{array}\right]}^T}\\ & =\underset{\delta \xi \to 0}{\lim }\frac{-n_w^{\wedge }\cdot R_{cw}^T{n}_c^{\wedge }\delta \varphi }{{\left[\begin{array}{cc}\delta \rho & \delta \varphi \end{array}\right]}^T}\\ & ={\left[\begin{array}{cc}{0}_{3\times 3}& -n_w^{\wedge }\cdot R_{cw}^T{n}_c^{\wedge }\end{array}\right]}_{3\times 6}\\ & =\left[\begin{array}{cc}{0}_{3\times 3}& -\left[\begin{array}{ccc}0& -n_{wz}& n_{wy}\\ n_{wz}& 0& -n_{wx}\\ -n_{wy}& n_{wx}& 0\end{array}\right]\cdot R_{cw}^T\cdot \left[\begin{array}{ccc}0& -n_{cz}& n_{cy}\\ n_{cz}& 0& -n_{cx}\\ -n_{cy}& n_{cx}& 0\end{array}\right]\end{array}\right]\end{array}$$

1.1.1.1.3 雅各比矩阵 (对 $T_{wc}$)

1.1.1.1.3.1 结果

$$\begin{array}{r}\frac{\partial (E=(P{T}_{wc}\overline{n_c})\times n_w)}{\partial T_{wc}}=\left[\begin{array}{cccccc}0& 0& 0& -n_{wy}y-n_{wz}z& n_{wy}x& n_{wz}x\\ 0& 0& 0& n_{wx}y& -n_{wx}x-n_{wz}z& n_{wz}y\\ 0& 0& 0& n_{wx}z& n_{wy}z& -n_{wx}x-n_{wy}y\end{array}\right]\end{array}$$

1.1.1.1.3.2 推导过程:

$$\begin{array}{rl}Error& =(P\cdot T_{wc}\cdot \overline{n_c})\times n_w\\ & =(P\cdot T_{wc}\cdot \overline{n_c}{)}^{\wedge }\cdot n_w\\ & =-{n_w}^{\wedge }\cdot (P\cdot T_{wc}\cdot \overline{n_c})\\ & =-{n_w}^{\wedge }\cdot P\cdot T_{wc}\cdot \overline{n_c}\\ & \\ \frac{\partial (E=(P{T}_{wc}\overline{n_c})\times n_w)}{\partial T_{wc}}& =\frac{\partial Error}{\partial \delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{-n_w^{\wedge }P\exp (\delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })\overline{n_c}+n_w^{\wedge }P\exp ({\xi }^{\wedge })\overline{n_c}}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{-n_w^{\wedge }P(I+\delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })\overline{n_c}+n_w^{\wedge }P\exp ({\xi }^{\wedge })\overline{n_c}}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{-n_w^{\wedge }P\delta {\xi }^{\wedge }\exp ({\xi }^{\wedge })\overline{n_c}}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{-n_w^{\wedge }\cdot {\left[\begin{array}{cc}{I}_{3\times 3}& 0_{3\times 1}\end{array}\right]}_{3\times 4}\cdot {\left[\begin{array}{cc}\delta {\varphi }^{\wedge }& \delta \rho \\ 0^T& 0\end{array}\right]}_{4\times 4}\cdot {\left[\begin{array}{c}R{n}_c\\ 0\end{array}\right]}_{4\times 1}}{{\left[\begin{array}{cc}\delta \rho & \delta \varphi \end{array}\right]}^T}\\ & =\underset{\delta \xi \to 0}{\lim }\frac{-n_w^{\wedge }\cdot \left[\begin{array}{cc}I& 0\end{array}\right]\cdot \left[\begin{array}{c}\delta {\varphi }^{\wedge }(R{n}_c)\\ 0\end{array}\right]}{{\left[\begin{array}{cc}\delta \rho & \delta \varphi \end{array}\right]}^T}\\ & =\underset{\delta \xi \to 0}{\lim }\frac{-n_w^{\wedge }\cdot \delta {\varphi }^{\wedge }(R{n}_c)}{{\left[\begin{array}{cc}\delta \rho & \delta \varphi \end{array}\right]}^T}\\ & =\underset{\delta \xi \to 0}{\lim }\frac{n_w^{\wedge }\cdot (R{n}_c{)}^{\wedge }\delta \varphi }{{\left[\begin{array}{cc}\delta \rho & \delta \varphi \end{array}\right]}^T}\\ & ={\left[\begin{array}{cc}{0}_{3\times 3}& n_w^{\wedge }\cdot (R{n}_c{)}^{\wedge }\end{array}\right]}_{3\times 6}\\ & =\left[\begin{array}{cc}{0}_{3\times 3}& \left[\begin{array}{ccc}0& -n_{wz}& n_{wy}\\ n_{wz}& 0& -n_{wx}\\ -n_{wy}& n_{wx}& 0\end{array}\right]\cdot \left[\begin{array}{ccc}0& -z& y\\ z& 0& -x\\ -y& x& 0\end{array}\right]\end{array}\right]\\ & =\left[\begin{array}{cccccc}0& 0& 0& -n_{wy}y-n_{wz}z& n_{wy}x& n_{wz}x\\ 0& 0& 0& n_{wx}y& -n_{wx}x-n_{wz}z& n_{wz}y\\ 0& 0& 0& n_{wx}z& n_{wy}z& -n_{wx}x-n_{wy}y\end{array}\right]\end{array}$$

1.1.1.2 方案二：

1.1.1.2.1 误差：

此方案的误差使用点乘来表达，并且要使用两个约束

• 由于是点乘，所以此时的 Error 是一个标量
• 并且由于点乘最后与角度相关的是 cos，所以当 Error=0 时，角度是接近 90/270 度的
• 基于上面的特性，所以使用两个约束，最终使用两个边，也就是要保证当前的向量与其余的两个曼哈顿向量都垂直
• 这样就可以保证当前向量与其中一个曼哈顿轴平行

$$\begin{array}{rl}a\cdot b& =a^{T}b=|a||b|\cos \theta \\ & \\ Error& =n_w^T\cdot (P{T}_{wc}\overline{n_c})\end{array}$$

1.1.1.2.2 雅各比矩阵（对 $T_{cw}$）

结果

$$\begin{array}{rl}\frac{\partial (E=n_w^T\cdot (P{T}_{cw}^{-1}\overline{n_c}))}{\partial T_{cw}}& =\left[\begin{array}{cccc}0& 0& 0& [n_w^T{R}_{cw}^T{n}_c^{\wedge }{]}_{(1\times 3)}\end{array}\right]\\ & =\left[\begin{array}{cccc}0& 0& 0& \left[\begin{array}{ccc}{n}_{wx}& n_{wy}& n_{wz}\end{array}\right]\cdot R_{cw}^T\cdot \left[\begin{array}{ccc}0& -n_{cz}& n_{cy}\\ n_{cz}& 0& -n_{cx}\\ -n_{cy}& n_{cx}& 0\end{array}\right]\end{array}\right]\end{array}$$

推导过程略

1.1.1.2.3 雅各比矩阵（对 $T_{wc}$）

结果

$$\frac{\partial (E=n_w^T\cdot (P{T}_{wc}\overline{n_c}))}{\partial T_{wc}}=\left[\begin{array}{cccccc}0& 0& 0& -n_{wy}z+n_{wz}y& n_{wx}z-n_{wz}x& -n_{wx}y+n_{wy}x\end{array}\right]$$

推导过程:

$$\begin{array}{rl}\frac{\partial (E=n_w^T\cdot (P{T}_{wc}\overline{n_c}))}{\partial T_{wc}}& =\frac{\partial Error}{\partial \delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{n_w^{T}P\exp (\delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })\overline{n_c}-n_w^{T}P\exp ({\xi }^{\wedge })\overline{n_c}}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{n_w^{T}P(I+\delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })\overline{n_c}-n_w^{T}P\exp ({\xi }^{\wedge })\overline{n_c}}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{n_w^{T}P\delta {\xi }^{\wedge }\exp ({\xi }^{\wedge })\overline{n_c}}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{n_w^T\cdot {\left[\begin{array}{cc}{I}_{3\times 3}& 0_{3\times 1}\end{array}\right]}_{3\times 4}\cdot {\left[\begin{array}{cc}\delta {\varphi }^{\wedge }& \delta \rho \\ 0^T& 0\end{array}\right]}_{4\times 4}\cdot {\left[\begin{array}{c}R{n}_c\\ 0\end{array}\right]}_{4\times 1}}{{\left[\begin{array}{cc}\delta \rho & \delta \varphi \end{array}\right]}^T}\\ & =\underset{\delta \xi \to 0}{\lim }\frac{n_w^T\cdot \left[\begin{array}{cc}I& 0\end{array}\right]\cdot \left[\begin{array}{c}\delta {\varphi }^{\wedge }(R{n}_c)\\ 0\end{array}\right]}{{\left[\begin{array}{cc}\delta \rho & \delta \varphi \end{array}\right]}^T}\\ & =\underset{\delta \xi \to 0}{\lim }\frac{n_w^T\cdot \delta {\varphi }^{\wedge }(R{n}_c)}{{\left[\begin{array}{cc}\delta \rho & \delta \varphi \end{array}\right]}^T}\\ & =\underset{\delta \xi \to 0}{\lim }\frac{-n_w^T\cdot (R{n}_c{)}^{\wedge }\delta \varphi }{{\left[\begin{array}{cc}\delta \rho & \delta \varphi \end{array}\right]}^T}\\ & ={\left[\begin{array}{cc}{0}_{1\times 3}& -n_w^T\cdot (R{n}_c{)}^{\wedge }\end{array}\right]}_{1\times 6}\\ & =\left[\begin{array}{cc}{0}_{3\times 3}& -\left[\begin{array}{ccc}{n}_{wx}& n_{wy}& n_{wz}\end{array}\right]\cdot \left[\begin{array}{ccc}0& -z& y\\ z& 0& -x\\ -y& x& 0\end{array}\right]\end{array}\right]\\ & =\left[\begin{array}{cccccc}0& 0& 0& -n_{wy}z+n_{wz}y& n_{wx}z-n_{wz}x& -n_{wx}y+n_{wy}x\end{array}\right]\end{array}$$

1.1.1.3 方案三：

1.1.1.3.1 误差：

此方案的误差使用叉乘的模长（范数）来表达

• 由于是叉乘的模长，所以此时的 Error 是一个标量
• 由于不知道怎么对一个范数求对位姿的雅可比，所以使用链式法则 $\frac{\partial e_{(范数)}}{\partial T_{cw}}=\frac{\partial e_{(范数)}}{\partial e_{非范数}}\cdot \frac{\partial \delta e_{(非范数)}}{\partial T_{cw}}$
• 非范数的雅可比见方案一

$$\begin{array}{rl}\Vert a\times b\Vert & =\Vert |a||b|\sin \theta n\Vert \\ Error& =\Vert (P{T}_{wc}\overline{n_c})\times n_w\Vert \\ & =\Vert (P{T}_{cw}^{-1}\overline{n_c})\times n_w\Vert \end{array}$$

1.1.1.3.2 雅各比矩阵

略

1.1.1.4 方案四：

1.1.1.4.1 误差：

**此方案的误差使用点乘的平方来表达，但是后面要 -1，这样可以用点乘表达平行关系

• 由于是点乘，所以此时的 Error 是一个标量
• 并且由于点乘最后与角度相关的是 cos，所以要让点乘的结果接近于 1/-1 才是平行
• 对于点乘进行平方处理，这样可以保证点乘平方接近于 1 就是平行

$$\begin{array}{rl}a\cdot b& =a^{T}b=|a||b|\cos \theta \\ & \\ Error& =(n_w^T\cdot (P{T}_{wc}\overline{n_c}){)}^2-1\end{array}$$

1.1.1.4.2 雅各比矩阵

略

1.2 代码层面：重载边成员方法

具体实现可参照 从高斯牛顿法理解如何自定义G2O

对于自定义的边，有以下函数需要重载以实现自定义的功能

virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void computeError();
virtual void linearizeOplus();

1.2.1 read 和 wirte

这两个成员方法是用来 存盘和读盘,可以留空。

1.2.2 computeError 定义误差计算方式

如果直接使用球坐标的方式求误差，那么对位姿 SE3 的雅各比会特别难求，以下为球坐标表达的 Error:

$$\{\begin{array}{l}\varphi =\arctan \left(\frac{T_{wc}\cdot n_{c\_y}}{T_{wc}\cdot n_{c\_x}}\right)\\ \theta =\arcsin \left(T_{wc}\cdot n_{c\_z}\right)\\ R_{当前坐标\to 基坐标}=\left[\begin{array}{ccc}\cos (\theta )& -\sin (\theta )& 0\\ \sin (\theta )& \cos (\theta )& 0\\ 0& 0& 1\end{array}\right]\cdot \left[\begin{array}{ccc}\cos (\varphi )& 0& \sin (\varphi )\\ 0& 1& 0\\ -\sin (\varphi )& 0& \cos (\varphi )\end{array}\right]\\ Error=R_{当前坐标\to 基坐标}^T\cdot n_2\end{array}$$

所以直接还是使用 Hessian 方法表示平面，并且使用上面 方案一 的方式求误差。但是由于 $\Delta x$ 增量更新用的还是球坐标，所以在计算误差前，要将球坐标转换为 Hessian 表达 $(n_x,n_y,n_z,d)$。

$$\begin{array}{rl}& \{\begin{array}{l}{n}_x=\cos \varphi \cos \theta \\ n_y=\cos \varphi \sin \theta \\ n_z=\sin \varphi \end{array}\\ & Error=(P{T}_{cw}^{-1}\overline{n_c})\times n_w\end{array}$$

1.2.3 linearizeOplus 定义误差关于状态量的偏导数 (雅可比矩阵).

1.2.3.1 误差对平面雅各比：

由于 Error 采用 Hessian 方式表达，所以误差对平面雅各比需要使用链式法则求:

$$
\begin{align}
&{\partial e_{hessian} \over \partial n_{球} } = {\partial e_{hessian} \over \partial n_{hessian}} \cdot {\partial n_{hessian} \over \partial n_{球}}
\
&{\partial n_{hessian} \over \partial n_{球}} =
\begin{bmatrix}
-\cos \phi \sin \theta & -\sin \phi \cos \theta & 0 \
\cos \phi \cos \theta & -\sin \phi \sin \theta & 0 \
0 & \cos \phi & 0 \
0 & 0 & 1
\end{bmatrix}

\end{align}
$$

1.2.3.2 误差对位姿 SE3 雅各比：

见 1.1 理论层面