ORBSLAM3加入Odometry里程计预积分公式

Odometry预积分

Input

Eigen:Vector3d linnear_velocity //3轴线速度

Eigen:Vector3d angle_rate // 3轴角速度

即${\tilde{\omega }}_k$、${\tilde{v}}_k$

$Bias$

$Bias$部分设置了线速度$b^v$，角速度$b^r$

$Noise$

$Noise$部分设置了线速度${\eta }^v$，角速度${\eta }^r$

Preintegrate

$R_j{v}_j{p}_j$

$$\begin{gathered} R_j=R_i\cdot \prod _{k=i}^{j-1}EXP(({\tilde{\omega }}_k-b_k^r-{\eta }_k^r)\cdot \Delta t) \\ {v}_j=v_i+\sum _{k=i}^{j-1}{R}_k\cdot ({\tilde{v}}_k-b_k^v-{\eta }_k^v) \\ {p}_j=p_i+\sum _{k=i}^{j-1}{v}_k\cdot \Delta t \end{gathered}$$

$\Delta R_{ij}\Delta v_{ij}\Delta p_{ij}$

$$\begin{gathered} \Delta R_{ij}=\prod _{k=i}^{j-1}EXP(({\tilde{\omega }}_k-b_k^r-{\eta }_k^r)\cdot \Delta t) \\ \Delta v_{ij}=\sum _{k=i}^{j-1}{R}_k\cdot ({\tilde{v}}_k-v_i-b_k^v-{\eta }_k^v) \\ \Delta p_{ij}=\sum _{k=i}^{j-1}\Delta v_{ik}\cdot \Delta t \end{gathered}$$

${\eta }^r$ ${\eta }^v$

$\Delta R_{ij}$

$$\begin{gathered} \Delta R_{ij}=\prod _{k=i}^{j-1}EXP(({\tilde{\omega }}_k-b_k^r-{\eta }_k^r)\cdot \Delta t) \\ \Delta {\tilde{R}}_{ij}=\prod _{k=i}^{j-1}EXP(({\tilde{\omega }}_k-b_k^r)\cdot \Delta t) \\ Exp(-\delta {\vec{\varphi }}_{ij})=\prod _{k=i}^{j-1}Exp(-\Delta {\tilde{R}}_{k+i}^T\cdot J_r^k\cdot {\eta }_k^r\Delta t) \\ \delta {\vec{\varphi }}_{ij}=-Log(\prod _{k=i}^{j-1}Exp(-\Delta {\tilde{R}}_{k+i}^T\cdot J_r^k\cdot {\eta }_k^r\Delta t)) \\ \Delta R_{ij}{=}^{\Delta }\Delta {\tilde{R}}_{ij}\cdot Exp(\delta {\vec{\varphi }}_{ij}) \end{gathered}$$

$\Delta v_{ij}$

$$\begin{gathered} \Delta v_{ij}=\sum _{k=i}^{j-1}{R}_k\cdot ({\tilde{v}}_k-v_i-b_k^v-{\eta }_k^v) \\ \Delta {\tilde{v}}_{ij}=\sum _{k=i}^{j-1}\Delta {\tilde{R}}_{ik}\cdot ({\tilde{v}}_k-v_i-b_k^v) \\ \delta v_{ij}=\sum _{k=i}^{j-1}(\Delta {\tilde{R}}_{ik}{\eta }_k^v-\Delta {\tilde{R}}_{ik}\cdot ({\tilde{v}}_k-v_i-b_k^v{)}^{\Lambda }\cdot \delta {\vec{\varphi }}_{ik}) \\ \Delta v_{ij}=\Delta {\tilde{v}}_{ij}-\delta v_{ij} \end{gathered}$$

$\Delta p_{ij}$

$$\begin{gathered} \Delta p_{ij}=\sum _{k=i}^{j-1}\Delta v_{ik}\cdot \Delta t \\ \Delta {\tilde{p}}_{ij}=\sum _{k=i}^{j-1}\Delta {\tilde{v}}_{ik}\cdot \Delta t \\ \delta p_{ij}=\sum _{k=i}^{j-1}\delta v_{ik}\cdot \Delta t \\ \Delta p_{ij}=\Delta {\tilde{p}}_{ij}-\delta p_{ij} \end{gathered}$$

${\eta }_{ij}^{\Delta }$

$${\eta }_{ij}^{\Delta }=[\delta {\vec{\varphi }}_{ij}^T\delta {\vec{v}}_{ij}^T\delta {\vec{p}}_{ij}^T]$$

$\delta {\vec{\varphi }}_{ij-1}\to \delta {\vec{\varphi }}_{ij}$

$$\begin{gathered} \delta {\vec{\varphi }}_{ij}=-Log(\prod _{k=i}^{j-1}Exp(-\Delta {\tilde{R}}_{k+i}^T\cdot J_r^k\cdot {\eta }_k^r\Delta t)) \\ \delta {\vec{\varphi }}_{ij}=\Delta {\tilde{R}}_{jj-1}\delta {\vec{\varphi }}_{ij-1}+J_r^{j-1}{\eta }_{j-1}^r\Delta t \end{gathered}$$

$\delta v_{ij-1}\to \delta v_{ij}$

$$\begin{gathered} \delta v_{ij}=\sum _{k=i}^{j-1}(\Delta {\tilde{R}}_{ik}{\eta }_k^v-\Delta {\tilde{R}}_{ik}\cdot ({\tilde{v}}_k-v_i-b_k^v{)}^{\Lambda }\cdot \delta {\vec{\varphi }}_{ik}) \\ \delta v_{ij}=\delta v_{ij-1}+\Delta {\tilde{R}}_{ij-1}{\eta }_{j-1}^v-\Delta {\tilde{R}}_{ij-1}\cdot ({\tilde{v}}_{j-1}-v_i-b_i^v{)}^{\Lambda }\cdot \delta {\vec{\varphi }}_{ij-1} \end{gathered}$$

$\delta p_{ij-1}\to \delta p_{ij}$

$$\begin{gathered} \delta p_{ij}=\sum _{k=i}^{j-1}\delta v_{ik}\cdot \Delta t \\ \delta p_{ij}=\delta p_{ij-1}+\delta v_{ij-1}\cdot \Delta t \end{gathered}$$

$A_{j-1}$

$$A_{j-1}=\left[\begin{array}{llc}\Delta {\tilde{R}}_{ij-1}& 0& 0\\ -\Delta {\tilde{R}}_{ij-1}({\tilde{v}}_{j-1}-v_i-b_i^v{)}^{\Lambda }& I& 0\\ 0& \Delta tI& I\end{array}\right]$$

$B_{j-1}$

$$B_{j-1}=\left[\begin{array}{ll}{J}_r^{j-1}\Delta t& 0\\ 0& \Delta {\tilde{R}}_{ij-1}\\ 0& 0\end{array}\right]$$

${\eta }_{ij}^{\Delta }$

$$\begin{gathered} {\eta }_{ij}^{\Delta }=\left[\begin{array}{llc}\Delta {\tilde{R}}_{ij-1}& 0& 0\\ -\Delta {\tilde{R}}_{ij-1}\dot{(}{\tilde{v}}_{j-1}-v_i-b_i^v{)}^{\Lambda }& I& 0\\ 0& \Delta tI& I\end{array}\right]{\eta }_{ij-1}^{\Delta }\dots \\ +\left[\begin{array}{ll}{J}_r^{j-1}\Delta t& 0\\ 0& \Delta {\tilde{R}}_{ij-1}\\ 0& 0\end{array}\right]{\eta }_{j-1}^d \\ {\eta }_{ij}^{\Delta }=A_{j-1}{\eta }_{ij-1}^{\Delta }+B_{j-1}{\eta }_{j-1}^d \end{gathered}$$

${\Sigma }_{ij}$

$${\Sigma }_{ij}=A_{j-1}{\Sigma }_{ij-1}{A}_{j-1}^T+B_{j-1}{\Sigma }_{\eta }+B_{j-1}^T$$