平面约束公式推导

在地面运行的小车如果可以认为在平面运动，则可以使用平面约束（注意不能用于斜坡之类的地面，这里相当于绝对水平。）
水平则有z=0，roll和pitch=0,对于四元数q来说，就是只绕z轴旋转yaw角 q = cos(yaw/2) + sin(yaw/2)(0,0,1) 也就是qx = 0 qy = 0
这三个可以成为每一帧的约束。
q转为R

R应该为
$$\begin{array}{ccc}[1-2z^2& 2wz& 0\\ -2wz& 1-2z^2& 0\\ 0& 0& 1\end{array}]$$
这个就是二维平面的转换公式。
我们可以使用矩阵第三列前两行的元素都为0，作为约束（也可以取第三行前两列）我们构造行列式乘法，把这两个元素取出来
$$\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\ast R\ast \begin{array}{c}0\\ 0\\ 1\end{array}$$
这样就可以把这两个元素取出来。
所以我们构建的残差方程为
$$0-\left[\begin{array}{ccc}0& 0& 1\end{array}\right]\ast P$$
$$\left[\begin{array}{c}0\\ 0\end{array}\right]-\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\ast R\ast \begin{array}{c}0\\ 0\\ 1\end{array}$$
我们把上面得的矩阵设为e3 = [0 0 1]和
$$\Lambda =\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}$$
我们优化的变量是$P_i^w$ $R_i^w$ 而平面约束是轮子的坐标。
$$\begin{gathered} R_o^w=R_i^w\ast R_o^b \\ {P}_o^w=R_i^w\ast t_o^i+P_i^w \end{gathered}$$
因此残差为：
$$\begin{gathered} r1=0-(e_3^T\ast （R_i^w\ast t_o^i+P_i^w)) \\ r2=0-(\Lambda \ast R_i^w\ast R_o^b\ast e_3) \end{gathered}$$
求雅克比偏导：
$$\begin{gathered} \frac{\partial r1}{\partial p}=-e_3^T \\ \frac{\partial r1}{\partial \theta }=\underset{\delta \theta \to 0}{\lim }\frac{-e_3^T\ast R_i^w\ast (1+\delta {\theta }^{\Lambda })\ast t_o^i+e_3^T\ast R_i^w\ast t_o^i}{\theta } \\ =-\frac{e_3^T\ast R_i^w\ast \delta {\theta }^{\Lambda }\ast t_o^i}{\theta }=e_3^T\ast R_i^w\ast (t_o^i{)}^{\Lambda } \\ \frac{\partial r2}{\partial p}=0 \\ \frac{\partial r2}{\partial \theta }=\underset{\delta \theta \to 0}{\lim }\frac{-\Lambda \ast R_i^w(1+\delta {\theta }^{\Lambda })\ast R_o^b\ast e_3+(\Lambda \ast R_i^w\ast R_o^b\ast e_3)}{\theta } \\ =\underset{\delta \theta \to 0}{\lim }\frac{-\Lambda \ast R_i^w\delta {\theta }^{\Lambda }\ast R_o^b\ast e_3}{\theta } \\ =\Lambda \ast R_i^w\ast (R_o^b\ast e_3{)}^{\Lambda } \end{gathered}$$

以上就是认为地面是绝对水平的约束。如果不是绝对水平，而是平面的斜坡之类的，可以用两帧之间固定，比如两帧之间z的增量是一个固定值，roll和pitch是一个固定值之类的。