A lightweight and scalable visual-inertial motion capture system using fiducial markers

Abstract

Accurate localization of a moving object is important in many robotic tasks. Often an elaborate motion capture system is used to realize it. While high precision is guaranteed, such a complicated system is costly and limited to specified small size workspace. This paper describes a lightweight and scalable visual-inertial approach, which leverages paper printable, known size and unknown pose, artificial landmarks, as called fiducials, to obtain motion state estimates, including pose and velocity. Visual-inertial joint optimization using incremental smoother over factor graph and the IMU preintegration technique make our method efficient and accurate. No special hardware is required except a monocular camera and an IMU, making our system lightweight and easy to deploy. Using paper printable landmarks, as well as the efficient incremental inference algorithm, renders it nearly constant-time complexity and scalable to large-scale environment. We perform an extensive evaluation of our method on public datasets and real-world experiments. Results show our method achieves accurate state estimates and is scalable to large-scale environment and robust to fast motion and changing light condition. Besides, our method has the ability to recover from intermediate failure.

Appendix

1.1 A. Background

In the following, we wish to derive the partial derivatives of the transformation ${\varvec{\pi }}\left( {\varvec{T}}\cdot {\varvec{l}}\right)$ with respect to the pose ${\varvec{T}}$. According to Hauke (2012), this can be calculated using the smooth path $\varvec{T}\left( t\right) ={\varvec{T}}\mathrm{Exp}\left( {\delta {\varvec{\xi }}}\right)$:

$$\begin{aligned} \begin{aligned} \frac{\partial {\varvec{\pi }}\left( {\varvec{T}}\mathrm{Exp}\left( {\delta {\varvec{\xi }}}\right) \cdot {\varvec{l}}\right) }{\partial {\delta {\varvec{\xi }}}}&=\frac{\partial {\varvec{\pi }}\left( {\varvec{q}}\right) }{\partial {\varvec{q}}}{\Bigg |}_{{\varvec{q}}={\varvec{T}}\cdot {\varvec{l}}}\frac{\partial {\varvec{T}}\mathrm{Exp}\left( {\delta {\varvec{\xi }}}\right) \cdot {\varvec{l}}}{\partial {\delta {\varvec{\xi }}}}{\Bigg |}_{{\delta {\varvec{\xi }}}={\varvec{0}}}\\&={\varvec{J}}_r{\varvec{T}}\left[ \begin{array}{cc} {\varvec{I}}_{3\times 3} &{} -{\varvec{l}}_{1:3}^\wedge \\ {\varvec{0}}_{3\times 3} &{} {\varvec{0}}_{3\times 3} \end{array}\right] \end{aligned} \end{aligned}$$

(30)

where ${\varvec{q}={\varvec{T}}\mathrm{Exp}\left( {\delta {\varvec{\xi }}}\right) \cdot {\varvec{l}}}$, ${\delta {\varvec{\xi }}}= \left[ \begin{array}{cc} {\delta {\varvec{\rho }}}&{\delta {\varvec{\phi }}} \end{array}\right] ^T$, ${\delta {\varvec{\xi }}}\in \mathfrak {se}$(3), ${\delta {\varvec{\phi }}\in \mathfrak {so}}$(3) and ${\delta {\varvec{\rho }}} \in \mathbb {R}^3$. ${\varvec{J}}_r$ denotes the Jacobian matrix of the pinhole camera model with respect to the 3-dimension landmark point coordinates expressed in the camera frame. We will also use the exponential map property:

$$\begin{aligned} \mathrm{Exp}\left( -\delta {\varvec{\phi }}\right) ^T=\mathrm{Exp}\left( \delta {\varvec{\phi }}\right) \end{aligned}$$

(31)

1.2 B. Jacobians

This section provides the Jacobians of the tag reprojection error with respect to the moving object pose $\varvec{T}_{WB}$ and the tag pose $\varvec{T}_{WA}$. The tag reprojection error of the $n\mathrm{th}$ corner in the $j\mathrm{th}$ tag at image time $t_i$ is

$$\begin{aligned} \varvec{e}^{i,j,n}_{re} = \varvec{z}^{i,j,n}-{\varvec{\pi }} \left( \varvec{T}_{CB} \left( \varvec{T}_{WB}^i\right) ^{-1} \varvec{T}_{WA_j}\cdot {\varvec{l}}_j^n \right) \end{aligned}$$

(32)

1. Jacobian of the tag reprojection error with respect to the moving object pose $\varvec{T}_{WB}$:

The reprojection error with respect to the rotational increment is:

$$\begin{aligned} \begin{aligned}&\varvec{e}^{i,j,n}_{re}\left( \varvec{R}_{WB}^i\mathrm{Exp}\left( \delta {\varvec{\phi }}_R^i\right) \right) \\&\quad = \varvec{z}^{i,j,n}-\pi \left( \varvec{T}^i_{CB}\left[ \begin{array}{cc} \left( {\varvec{R}}^i_{WB}\mathrm{Exp}\left( \delta {\varvec{\phi }}_R^i\right) \right) ^T &{} -\left( {\varvec{R}}^i_{WB}\mathrm{Exp}\left( \delta {\varvec{\phi }}_R^i\right) \right) ^T_W{\varvec{p}}^i_{WB}\\ {\varvec{0}}_{3\times 3} &{} 1 \end{array} \right] \varvec{T}_{WA_j}\cdot \varvec{l}_j^n \right) \\&\quad = \varvec{z}^{i,j,n}-\pi \left( \varvec{T}^i_{CB}\left[ \begin{array}{c} \mathrm{Exp}\left( -\delta {\varvec{\phi }}_R^i\right) {\varvec{R}}^{i\;T}_{WB}\left( _W {\varvec{l}}_{j\;1:3}^n-_W{\varvec{p}}^i_{WB} \right) \\ 1 \end{array}\right] \right) \end{aligned} \end{aligned}$$

(33)

where $_W{\varvec{l}}^n=\left[ \begin{array}{cc} _W{\varvec{l}}^n_{1:3}&1 \end{array}\right] ^T={\varvec{T}}_{WA_j}\cdot { \varvec{l}_j^n}$. We can get the Jacobian of the tag reprojection error with respect to the moving object orientation using Eqs. (30) and (31):

$$\begin{aligned} \begin{aligned} \frac{\partial \varvec{e}^{i,j,n}_{re}}{\partial {\delta {\varvec{\phi }}_R^i}}&=-{\varvec{J}}_{r\;j,n}{\varvec{T}}_{CB}^i\left[ \begin{array}{c} \left( {\varvec{R}}^{i\;T}_{WB}\left( _W {\varvec{l}}_{j\;1:3}^n-_W{\varvec{p}}^i_{WB} \right) \right) ^\wedge \\ {\varvec{0}}_{1\times 3} \end{array}\right] \end{aligned} \end{aligned}$$

(34)

The reprojection error with respect to the translational increment is:

$$\begin{aligned} \begin{aligned}&\varvec{e}^{i,j,n}_{re}\left( _W{\varvec{p}}^i_{WB}+\delta _W{\varvec{p}}^i_{WB}\right) \\&\quad = \varvec{z}^{i,j,n}-\pi \left( \varvec{T}^i_{CB}\left[ \begin{array}{c} {\varvec{R}}^{i\;T}_{WB}\left( _W {\varvec{l}}_{j\;1:3}^n-_W{\varvec{p}}^i_{WB}-\delta _W{\varvec{p}}^i_{WB} \right) \\ 1 \end{array}\right] \right) \end{aligned} \end{aligned}$$

(35)

and the Jacobian of the tag reprojection error with respect to the moving object translation is:

$$\begin{aligned} \frac{\partial \varvec{e}^{i,j,n}_{re}}{\partial \delta _W{\varvec{p}}^i_{WB}}={\varvec{J}}_{r\;j,n}{\varvec{T}}_{CB}^i\left[ \begin{array}{c} {\varvec{R}}^{i\;T}_{WB}\\ {\varvec{0}}_{1\times 3} \end{array}\right] \end{aligned}$$

(36)

2. Jacobian of the tag reprojection error with respect to the tag pose $\varvec{T}_{WA}$: The reprojection error with respect to the SE(3) increment is:

$$\begin{aligned} \begin{aligned}&\varvec{e}^{i,j,n}_{re}\left( \varvec{T}_{WA_j}\mathrm{Exp}\left( \delta {\varvec{\xi }}_F^j\right) \right) \\&\quad = {\varvec{z}}^{i,j,n}-\pi \left( \varvec{T}_{CB} \left( \varvec{T}_{WB}^i\right) ^{-1} {\varvec{T}}_{WA_j}\mathrm{Exp}\left( \delta {\varvec{\xi }}_F^j\right) \cdot \varvec{l}_j^n \right) \end{aligned} \end{aligned}$$

(37)

so we can get the Jacobian of the tag reprojection error with respect to the tag pose using Eq. (30):

$$\begin{aligned} \begin{aligned} \frac{\partial \varvec{e}^{i,j,n}_{re}}{\partial {\delta {\varvec{\xi }}}}&=-{\varvec{J}}_{r\;j,n}{\varvec{T}}_{CB}^i\left( {\varvec{T}}_{WB}^i\right) ^{-1}{\varvec{T}}_{WA}^j\left[ \begin{array}{cc} {\varvec{I}}_{3\times 3} &{} -{\varvec{l}}_{j\;1:3}^\wedge \\ {\varvec{0}}_{1\times 3} &{} {\varvec{0}}_{1\times 3} \end{array}\right] \end{aligned} \end{aligned}$$

(38)