Visual Map Point

Reprojection error of map points.

Use parameterization right perturbation in SE(3), #local variables : 6, # global variables : 6)

\[\xi \oplus \delta \xi = Log(Exp(\xi)Exp(\delta \xi))\]

Residual

\[e_{k} = \frac{1}{z_{c}}e^{[\xi_{cb}]_{\times}}e^{[\xi_{bw}]_{\times}}p_{w, k} - u_{k}\]

Where :

  • \(\xi_{cb}\) is the camera-base extrinsic parameters: transformation from base reference frame to camera reference frame.
  • \(\xi_{bw}\) is the base parameters : transformation from world reference frame to base reference frame.
  • \(p_{w}\) is the pose of the 3d map point in the world reference frame.
  • z is the depth of the map point in camera reference frame (to transform to camera uv space).
  • u is the feature observation (in camera uv space).
\[p_{b} = e^{[\xi_{bw}]_{\times}}p_{w}\]
\[p_{c} = e^{[\xi_{cb}]_{\times}}p_{b}\]

Jacobians

Jacobians w.r.t. map points :

\[J_{p_{w}} = \frac{\partial e}{\partial p_{w}} = \frac{\partial e}{\partial p_{c}} \frac{\partial p_{c}}{\partial p_{w}}\]
\[\begin{split}e = \begin{bmatrix} x_{c}/z_{c} \\ y_{c}/z_{c} \end{bmatrix} -\begin{bmatrix} u \\ v \end{bmatrix} = \begin{bmatrix} e_{1} \\ e_{2} \end{bmatrix}\end{split}\]
\[\begin{split}\frac{\partial e}{\partial p_{c}} = \begin{bmatrix} \frac{\partial e_{1}}{\partial x_{c}} & \frac{\partial e_{1}}{\partial y_{c}} & \frac{\partial e_{1}}{\partial z_{c}} \\ \frac{\partial e_{2}}{\partial x_{c}} & \frac{\partial e_{2}}{\partial y_{c}} & \frac{\partial e_{2}}{\partial z_{c}} \end{bmatrix} = \begin{bmatrix} 1/z_{c} & 0 & -x_{c}/z_{c}^{2} \\ 0 & 1/z_{c} & - y_{c}/z_{c}^{2} \end{bmatrix}\end{split}\]
\[\frac{\partial p_{c}}{\partial p_{w}} = \frac{\partial [R_{cb}(R_{bw}p_{w} + t_{bw})+t_{cb}] } {\partial p_{w}} = R_{cb}R_{bw}\]
\[\begin{split}J_{p_{w}} = \begin{bmatrix} 1/z_{c} & 0 & -x_{c}/z_{c}^{2} \\ 0 & 1/z_{c} & - y_{c}/z_{c}^{2} \end{bmatrix} R_{cb}R_{bw}\end{split}\]

Jacobians w.r.t. extrinsic parameters :

\[J_{p_{w}} = \frac{\partial e}{\partial \xi_{cb}} = \frac{\partial e}{\partial p_{c}} \frac{\partial p_{c}}{\partial \xi_{cb}}\]
\[\frac{\partial p_{c}}{\partial \xi_{cb}} = \frac{\partial e^{[\xi_{cb}]_{\times}}p_{b}}{\partial \xi_{cb}} = \frac{\partial}{\partial \xi_{cb}}( R_{cb}p_{b} + t_{cb} ) = \frac{\partial}{\partial \xi_{cb}}( e^{[q_{cb}]_{\times}}p_{b} + t_{cb} )\]
\[\begin{split}\begin{align} \frac{\partial p_{c}}{\partial \xi_{bc}} &= \frac{\partial}{\partial \xi_{bc}}( e^{[\xi_{cb}]_{\times}}p_{b} ) \\ &= \lim_{\delta \xi_{bc}\to 0} \frac{(e^{[\xi_{bc}]_{\times}}e^{[\delta \xi_{bc}]_{\times}})^{-1}p_{b} - e^{[\xi_{cb}]_{\times}}p_{b} }{\delta \xi_{bc}} \\ &= \lim_{\delta \xi_{bc}\to 0} \frac{e^{[-\delta \xi_{bc}]_{\times}}e^{[\xi_{cb}]_{\times}}p_{b} - e^{[\xi_{cb}]_{\times}}p_{b} }{\delta \xi_{bc}} \\ &= \lim_{\delta \xi_{bc}\to 0} \frac{(1-[\delta \xi_{bc}]_{\times})e^{[\xi_{cb}]_{\times}}p_{b} - e^{[\xi_{cb}]_{\times}}p_{b}}{\delta \xi_{bc}} \\ &= \lim_{\delta \xi_{bc}\to 0} \frac{-[\delta \xi_{bc}]_{\times}e^{[\xi_{cb}]_{\times}}p_{b} }{\delta \xi_{bc}} \\ &= \lim_{\delta \xi_{bc}\to 0} \frac{-[\delta \xi_{bc}]_{\times}p_{c} }{\delta \xi_{bc}} \\ &= \lim_{\delta \xi_{bc}\to 0} -\frac{1}{\delta \xi_{bc}} \begin{bmatrix} [\delta \phi_{bc}]_{x} & \delta t_{bc} \\ 0^{T} & 0 \end{bmatrix} \begin{bmatrix} p_{c} \\1 \end{bmatrix} \end{align}\end{split}\]
\[\begin{split}\begin{align} \frac{\partial p_{c}}{\partial t_{bc}} &= \lim_{\delta \xi_{cb}\to 0} \frac{-\delta t_{bc}}{\delta t_{bc}} \\ &= - I_{3\times 3} \end{align}\end{split}\]
\[\begin{split}\begin{align} \frac{\partial p_{c}}{\partial \phi_{bc}} &= \lim_{\delta \phi_{bc}\to 0} -\frac{1}{\delta \phi_{bc}} \begin{bmatrix} [\delta \phi_{bc}]_{x} & \delta t_{bc} \\ 0^{T} & 0 \end{bmatrix} \begin{bmatrix} p_{c} \\1 \end{bmatrix} \\ &=\lim_{\delta \phi_{bc}\to 0} - \frac{[\delta \phi_{bc}]_{\times}p_{c}} {\delta \phi_{bc}} \\ &=\lim_{\delta \phi_{bc}\to 0} \frac{[p_{c}]_{\times}\delta \phi_{bc}} {\delta \phi_{bc}} \\ &=[p_{c}]_{\times} \end{align}\end{split}\]

Jacobians w.r.t. pose base:

\[\frac{\partial p_{c}}{\partial \xi_{wb}} = e^{[\xi_{cb}]_{\times}}\frac{\partial}{\partial \xi_{wb}}( e^{[\xi_{bw}]_{\times}}p_{w} )\]
\[\frac{\partial p_{c}}{\partial t_{wb}} = -R_{cb}\]
\[\frac{\partial p_{c}}{\partial \phi_{wb}} =R_{cb}[p_{b}]_{\times}\]