Lie Group Modelling for an EKF-Based Monocular SLAM Algorithm

Abstract

This paper addresses the problem of monocular Simultaneous Localization And Mapping on Lie groups using fiducial patterns. For that purpose, we propose a reformulation of the classical camera model as a model on matrix Lie groups. Thus, we define an original-state vector containing the camera pose and the set of transformations from the world frame to each pattern, which constitutes the map’s state. Each element of the map’s state, as well as the camera pose, are intrinsically constrained to evolve on the matrix Lie group $SE\left(3\right)$. Filtering is then performed by an extended Kalman filter dedicated to matrix Lie groups to solve the visual SLAM process (LG-EKF-VSLAM). This algorithm has been evaluated in different scenarios based on simulated data as well as real data. The results show that the LG-EKF-VSLAM can improve the absolute position and orientation accuracy, compared to a classical EKF visual SLAM (EKF-VSLAM).

1. Introduction

The problem of simultaneously localizing a mobile robot and constructing a map from sensor measurements has arisen in many research works during the last decades [1,2]. Simultaneous Localization And Mapping (SLAM) can be seen as an estimation problem, where the state includes the pose of the robot and the position of the detected landmarks. This estimation problem is usually solved by one of the following two methods:

• the bundle adjustment approach: this formulates the SLAM as an optimization problem, where the set of all measurements (or a part of) is used to estimate the desired system state [3,4];
• the filtering approach: this is classically based on a Kalman filter and its variants (such as the extended Kalman filter, i.e., EKF). It provides an estimate of the state and its associated covariance matrix at each epoch.

When a visual sensor is used for SLAM, the approach is usually referred to as visual-SLAM (VSLAM) [5]. In this context, the classical landmarks are provided by well-known corner-detection algorithms, such as Harris [6], FAST [7], SIFT, BRIEF, SURF, or ORB. The use of such 2D features admit two main disadvantages:

(1)

the number of detected landmarks in each image can be high (≥500), making the map huge for big environments. This impacts directly the size of the state vector, which increases dramatically, making a filter-based method with 2D features not well-suited for long-term navigation;

(2)

each measurement must be associated to each detection with a data association algorithm [8]. This step remains a challenge and false matching can cause the filter to diverge. Moreover, it can be difficult to treat it from a computational point of view.

To overcome these difficulties, we propose in this study the use of fiducial or coded markers as landmarks [9,10,11]. The main advantage of such patterns is that they reduce the number of landmarks while solving the association step, thanks to labelled detections. Moreover, even if it is required to instrument the environment with coded patterns, a priori knowledge of the absolute position of one of the artificial landmarks would allow precise absolute localization.

For this study, the chosen patterns are constituted of a constellation of circular shapes (see Figure 1) but can be changed for other rigid patterns such as ARTag [12] without loss of generality. By assuming that the patterns have a known given size L, the pose of a pattern in the global frame can be represented as a 3D affine transformation. The latter is composed of a rotation and a translation and can be associated to the set of matrices defined in the special Euclidean group $SE\left(3\right)$, which has the structure of a Lie group (LG). The aim of this article is to rewrite the camera model in order to express the camera pose, as well as the pose of each landmark, in $SE\left(3\right)$ to reformulate the VSLAM problem as a recursive estimation problem on LGs.

1.1. Formulating VSLAM on Lie Groups

The problem of filtering on LGs has recieved a lot of attention in recent years, particularly in applications such as visual navigation and robotics to estimate the rotation and transformation matrices of a rigid body [13,14]. The interest of the LG framework is to overcome a complex Euclidean parametrization (as Euler angles or quaternions) by using a unified and elegant formalism, taking into account the geometric properties of these matrices. Consequently, the LG structure of the state vector admits advantages, compared to the classical VSLAM extended Kalman filter (EKF-VSLAM):

• We can rewrite a compact camera model, allowing us to overcome the high non-linearities created by rotation matrices parametrization;
• The analytical development of quantities of interest, such as Jacobian matrices, are intrinsic to LGs and are consequently less difficult to compute and to implement.

To apply filtering on LGs, several dedicated analytical algorithms have been developed in the literature, such as the Lie Group-Extended Kalman Filter (LG-EKF) [15], the Invariant Kalman filter [16,17], the Unscented Kalman filter (LG-UKF) [18], the information Kalman filter (LG-IKF) [19], but also Monte-Carlo filtering, such as particle filters [20]. In the context of SLAM, several works have been made. In [21], a VSLAM on LG is proposed and based on a LG-UKF. The unknown state is assumed to be lying on the LG $S{E}_{2+p}\left(3\right)$. In [22], a SLAM on LGs is developed that combines LG-EKF and LG-IKF. In [23], a robust hybrid VSLAM is proposed to detect the loop closures: the map and the pose are constructed in a Euclidean framework, but the loop closures are determined by estimating 3D map similarities with a LG modelling. All of these techniques have in common the assumption that the map is constructed by estimating Euclidean parameters (3D points).

In this study, we propose a LG-EKF-VSLAM which estimates both the state and the map on LGs. The advantages of this method are that it intrinsically takes into account the geometry of the pose, and it copes with the high non-linearities introduced by the camera model. Consequently, the proposed approach should theoretically improve the pose-estimation performance.

1.2. Contribution

Our contribution is two-fold:

• Theoretical: we define a new matrix state containing the camera pose and the pattern transformations on $SE\left(3\right)$, which constitutes the map. Consequently, the associated state space is ${\left(SE\left(3\right)\right)}^{1+K}$ ($1+K$ direct products of $SE\left(3\right)$), where K is the number of current patterns;
• Algorithmic: the newly detected patterns are initialized by a $SE\left(3\right)$ transformation, obtained by minimizing a criterion through a Gauss–Newton algorithm on LGs.

1.3. Organization

This paper is organized as follows: the EKF-VSLAM associated with the camera observation model in the context of coded patterns is detailed in Section 2. In Section 3, a background on LGs is introduced, and the considered camera model is detailed and rewritten on LG $SE\left(3\right)$. The proposed LG-VSLAM, based on LG-EKF, is then explained in Section 4. Section 5 is dedicated to the performance evaluation. The proposed algorithm is implemented and assessed with simulated and real data and compared to a classical Euclidean EKF-SLAM. Finally, conclusions and remarks are drawn in Section 6.

2. The Euclidean Approach

In this section, we present the classical Euclidean pinhole camera model. Particularly, we provide the different Euclidean parametrizations of unknown variables and the way the EKF-VSLAM is applied.

2.1. Coded Patterns

Consider a mobile robot equipped with a monocular camera which tries to localize itself by using coded patterns spread out in its close environment. In our case, each coded pattern is a 2D square surface, composed of four circles of the same size (see Figure 1). We define the center of the bottom left ellipse as the origin of the pattern’s local frame. In this frame, the $3D$ coordinates ${\left\{{\mathbf{q}}_i\right\}}_{i=1}^4$ of the centers of each ellipse can be computed, knowing the inter-circle distance L. Consequently, every pattern j can be fully characterized by:

• a rotation matrix between the local frame and a world frame, ${\mathbf{R}}_{WP}^{\left(j\right)}$;
• the position of the origin of the local frame expressed in this world frame, ${\mathbf{p}}_{WP}^{\left(j\right)}$;
• the distance L between the center of two ellipses;
• an ID coded by the circular bar-codes.

2.2. Camera Observation Model

At each instant k, a fiducial pattern detector provides the 2D coordinates of the center of each ellipse, ${\mathbf{u}}_{k,i}^{\left(j\right)}$, associated to the 3D point ${\mathbf{q}}_i$ of pattern j in the world frame. According to the pinhole camera model, the reprojection function provides the mathematical relation linking ${\left\{{\mathbf{u}}_{k,i}^{\left(j\right)}\right\}}_{i=1}^4$ and ${\left\{{\mathbf{q}}_{i,C}^{\left(j\right)}\right\}}_{i=1}^4$, such as:

$${\mathbf{u}}_{k,i}^{\left(j\right)}={\displaystyle \frac{f}{{\left({\mathbf{q}}_{i,C}^{\left(j\right)}\right)}_3}}\left[{\nu }_x{\left({\mathbf{q}}_{i,C}^{\left(j\right)}\right)}_1+x_0,{\nu }_y{\left({\mathbf{q}}_{i,C}^{\left(j\right)}\right)}_2+y_0\right]+{\mathbf{n}}_{k,i}^{\left(j\right)},$$

(1)

with f being the focal distance of the camera, $(x_0,y_0)$ being the principal point of the image and parameters ${\nu }_x$ and ${\nu }_y$, the pixel densities on the horizontal and the vertical axes, respectively. ${\left(\mathbf{v}\right)}_l$ denotes the $l^{th}$ component of vector $\mathbf{v}$. The random process, ${\mathbf{n}}_{k,i}^{\left(j\right)}$, is considered as white Gaussian noise on ${\mathbb{R}}^2$ with covariance $\mathbf{N}$, ${\mathbf{n}}_{k,i}^{\left(j\right)}\sim {\mathcal{N}}_{{\mathbb{R}}^2}(\mathbf{0},\mathbf{N})$. As illustrated in Figure 2, ${\mathbf{q}}_{i,C}^{\left(j\right)}$ explicitly depends on the rotation matrix ${\mathbf{R}}_{WC}^{\left(k\right)}$ and the rotation between the frame of the coded pattern and the world frame ${\mathbf{R}}_{WP}^{\left(j\right)}$. In a classical Euclidean framework, they can be parametrized by:

• Euler angles: the unknown variables are six angles $\{{\theta }^{\left(k\right)},{\varphi }^{\left(k\right)},{\psi }^{\left(k\right)},{\theta }^{\left(j\right)}{\varphi }^{\left(j\right)},{\psi }^{\left(j\right)}\}$, such as:

  $$\begin{array}{cc}\hfill {\mathbf{R}}_{WC}^{\left(k\right)}& ={\mathbf{R}}_x\left({\theta }^{\left(k\right)}\right){\mathbf{R}}_y\left({\varphi }^{\left(k\right)}\right){\mathbf{R}}_z\left({\psi }^{\left(k\right)}\right)\hfill \end{array}$$

  (2)

  $$\begin{array}{cc}\hfill {\mathbf{R}}_{WP}^{\left(j\right)}& ={\mathbf{R}}_x\left({\theta }^{\left(j\right)}\right){\mathbf{R}}_y\left({\varphi }^{\left(j\right)}\right){\mathbf{R}}_z\left({\psi }^{\left(j\right)}\right),\hfill \end{array}$$

  (3)

  where ${\mathbf{R}}_u$ denotes an elementary 3D rotation according to the axis u;
• Quaternion: In this case ${\mathbf{R}}_{WC}^{\left(k\right)}$ and ${\mathbf{R}}_{WP}^{\left(j\right)}$ can be written as a complex combination of quaternion coefficients with the form $\mathbf{z}=a+b\mathbf{i}+c\mathbf{j}+d\mathbf{k}$.

Through this observation model and a given dynamic model, it is possible to estimate the global state ${\mathbf{x}}_k$ containing:

• the pose of the camera $\left[{\theta }^{\left(k\right)},{\varphi }^{\left(k\right)},{\psi }^{\left(k\right)},{\mathbf{p}}_{WC}^{\left(k\right)}\right]\in {\mathbb{R}}^6$;
• the set of parameters of the patterns ${\{{\theta }^{\left(j\right)},{\varphi }^{\left(j\right)},{\psi }^{\left(j\right)},{\mathbf{p}}_{WP}^{\left(j\right)}\}}_{j=1}^K\in {\mathbb{R}}^{6K}$, where K is the number of detected patterns,

by performing a classical EKF-VSLAM [24].

2.3. Why EKF-SLAM on Lie Groups?

Using the EKF-VSLAM with coded patterns and classical camera model has some drawbacks due to the high non-linearity of the observation model. Indeed, we observe that the unknown variables are linked to the observations by the composition of multiple inverse and trigonometric functions for the Euler parametrization. For the quaternion parametrization, the non-linearity is introduced by the composition of inverse and multiple square functions. Thus, the linearization in the update step of the EKF introduces errors. This can induce some numerical problems in the Jacobian calculation and degrade the performance of the estimation.

In the following, we propose to resolve the SLAM problem by performing the filtering on LGs. Indeed, this formalism enables us to overcome these problems. Particularly:

•

the camera observation model can be written in a compact way and depends “linearly” on the rotation matrix. Consequently, the approximation at order 1 is more valid;

•

The Jacobian of the observation model can be computed analytically by considering the geometrical structure of the rotation matrices and does not depend on any parametrization.

3. Background on Lie Groups

3.1. Elementary Notions

A LG G is a set which has the properties of a group and a smooth manifold [25,26]. In this work, we deal with matrix LGs; thus, every element of G is a matrix belonging to ${\mathbb{R}}^{n\times n}$.

→

Due to its group structure, G has an internal composition law which is associative. Furthermore, there is an identity element defined, so that each element of the group has an inverse;

→

Due to its manifold structure, it is possible to compute the derivative and the integral of two elements, or the inverse of one element.

Furthermore, the properties of Lie Groups allow us to define a tangent vector space $T_{\mathbf{M}}G$ at every point $\mathbf{M}\in G$. It is then possible to associate any point of $T_{\mathbf{I}}G$ to any point of $T_{\mathbf{X}}G$, thanks to the operator called left tangent application (see Figure 3). The latter is defined, between two tangent spaces, by:

$$\begin{array}{ccccc}{L}_{\mathbf{X}\mathbf{Y}}^L& :& T_{\mathbf{X}}G& \to & T_{\mathbf{Y}}G\\ & & \mathbf{V}& \mapsto & \mathbf{Y}{\mathbf{X}}^{-1}\mathbf{V}\end{array}$$

(4)

Consequently, we can work with the tangent space to the identity ${\mathbf{I}}_{n\times n}$, also called the Lie algebra, denoted as $\mathfrak{g}$. This space is a vector space with a dimension of m and defines the dimension of G as manifold.

We can establish a relation between G and $\mathfrak{g}$ through the exponential mapping, ${\mathrm{Exp}}_G:\mathfrak{g}\to G$, and its reciprocal, the logarithm mapping, ${\mathrm{Log}}_G:G\to \mathfrak{g}$. They are locally bijective. The mathematical expressions of these operators can be found in [25,27,28].

On the other hand, $\mathfrak{g}$ can be directly linked to the Euclidean space ${\mathbb{R}}^m$ through an isomorphism ${\left[.\right]}_G^{\wedge }$ and its inverse ${\left[.\right]}_G^{\vee }$, such as $\mathfrak{a}={\left[\mathbf{a}\right]}_G^{\wedge }$ and $\mathbf{a}={\left[\mathfrak{a}\right]}_G^{\vee }$, $\forall \mathbf{a}\in {\mathbb{R}}^m,\forall \mathfrak{a}\in \mathfrak{g}$. To condense these notations, we define $\mathbf{a}={\mathrm{Log}}_G^{\vee }\left(\mathbf{X}\right)={\left[{\mathrm{Log}}_G\left(\mathbf{X}\right)\right]}^{\vee }$ and $\mathbf{X}={\mathrm{Exp}}_G^{\wedge }\left(\mathbf{a}\right)={\mathrm{Exp}}_G\left({\left[\mathbf{a}\right]}_G^{\wedge }\right)$, with $\mathbf{X}\in G$ and $\mathbf{a}\in {\mathbb{R}}^m$. Figure 4 shows, geometrically, the relationship between a LG G, its Lie algebra $\mathfrak{g}$, and ${\mathbb{R}}^m$.

3.2. Non-Commutativity and Jacobian

3.2.1. Non-Commutativity

In general, a LG is non-commutative and $\forall (\mathbf{a},\mathbf{b})\in {\left({\mathbb{R}}^m\right)}^2$:

$${\mathrm{Log}}_G^{\vee }\left({\mathrm{Exp}}_G^{\wedge }\left(\mathbf{a}\right){\mathrm{Exp}}_G^{\wedge }\left(\mathbf{b}\right)\right)\ne \mathbf{a}+\mathbf{b}.$$

(5)

The adjoint representation allows one to take into account the non-commutativity of LGs. It is an application $A{d}_G(.):G\to {\mathbb{R}}^{m\times m}$, such that $\forall \mathbf{X}\in G,\forall \mathbf{a}\in {\mathbb{R}}^m,\mathbf{X}{\mathrm{Exp}}_G^{\wedge }\left(\mathbf{a}\right)={\mathrm{Exp}}_G^{\wedge }\left(A{d}_G\left(\mathbf{X}\right)\mathbf{a}\right)\mathbf{X}$.

The non-commutativity of LGs can be also explicited through the Baker–Campbell–Hausdorff formula (BCH):

$${\mathrm{Log}}_G^{\vee }\left({\mathrm{Exp}}_G^{\wedge }(-\mathbf{a}){\mathrm{Exp}}_G^{\wedge }(\mathbf{a}+\mathbf{b})\right)={\mathit{\varphi }}_G(-\mathbf{a})\mathbf{b}+{O(\parallel \mathbf{b}\parallel }^2).,$$

(6)

and depends of the left Jacobian matrix, which is written:

$${\mathit{\varphi }}_G\left(\mathbf{a}\right)=\sum _{n=0}^{+\infty }{\displaystyle \frac{a{d}_G{\left(\mathbf{a}\right)}^n}{(n+1)!}},$$

(7)

where $a{d}_G:{\mathbb{R}}^m\to {\mathbb{R}}^{m\times m}$ is an adjoint operator characterizing the non-commutativity directly on the Lie algebra.

3.2.2. Jacobian on LGs

We can define the notion of Jacobian according to a function. Let us consider $f:G\to \mathbb{R}$ an integrable function. We define LG Jacobian of f by the following expression:

$$D_{\mathbf{X}}f={\left.{\displaystyle \frac{\partial f\left(\mathbf{X}{\mathrm{Exp}}_G^{\wedge }\left(\mathit{\delta }\right)\right)}{\partial \mathit{\delta }}}\right|}_{\mathit{\delta }=\mathbf{0}},$$

(8)

which can be seen as a generalization of the classical directional derivative.

3.3. Lie Groups of Interest

Two useful LGs in our application are introduced in this part.

3.3.1. The Special Orthogonal LG $SO\left(n\right)$

The LG $SO\left(n\right)$ is the group of rotation matrices of dimension n. Every element $\mathbf{R}$ of $SO\left(n\right)$ belongs to the following set:

$$SO\left(n\right)=\{\mathbf{R}\in {\mathbb{R}}^{n\times n}|\mathbf{R}{\mathbf{R}}^{\top }=\mathbf{I},\left|\mathbf{R}\right|=1\},$$

(9)

where $\left|\mathbf{R}\right|$ is the determinant of matrix $\mathbf{R}$. Its Lie algebra is the set of skew-symmetric matrices:

$$\mathfrak{so}\left(n\right)=\left\{{\left[\mathbf{w}\right]}_{\times }\right|\mathbf{w}\in {\mathbb{R}}^m\},$$

(10)

where $\mathbf{w}$ is the vector of ${\mathbb{R}}^m$ isomorph to the element of ${\left[\mathbf{w}\right]}_{\times }$. The notation ${[.]}_{\times }$ refers to the skew-symmetric matrix operator. In the case of $n=3$, every element of $SO\left(3\right)$ is a 3D rotation matrix, which can be classically parametrized in different ways (quaternions or Euler angles), as seen previously. With the LG formalism, we may parametrize it with an angle vector $\theta \in {\mathbb{R}}^m$$(m=3)$ . It is written, according to the Rodrigues formula [29], as:

$$\mathbf{R}={\mathbf{I}}_{3\times 3}+{\displaystyle \frac{{\left[\mathit{\theta }\right]}_{\times }}{\parallel \mathit{\theta }\parallel }}\sin (\parallel \mathit{\theta }\parallel )+{\displaystyle \frac{{\left[\mathit{\theta }\right]}_{\times }^2}{{\parallel \mathit{\theta }\parallel }^2}}\left(1-\cos (\parallel \mathit{\theta }\parallel )\right).$$

(11)

Every element of its Lie algebra has the following structure:

$${\left[\mathit{\theta }\right]}_{\times }=\left[\begin{array}{ccc}0& -{\left(\mathit{\theta }\right)}_3& {\left(\mathit{\theta }\right)}_2\\ {\left(\mathit{\theta }\right)}_3& 0& -{\left(\mathit{\theta }\right)}_1\\ -{\left(\mathit{\theta }\right)}_2& {\left(\mathit{\theta }\right)}_1& 0\end{array}\right].$$

(12)

3.3.2. The Special Euclidean LG $SE\left(n\right)$

The LG $SE\left(n\right)$ corresponds to the semi-direct product between $SO\left(n\right)$ and ${\mathbb{R}}^n$:

$$SE\left(n\right)=\left\{\mathbf{A}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{x}\\ {\mathbf{0}}_{1\times n}& 1\end{array}\right]|\mathbf{R}\in SO\left(n\right),\mathbf{x}\in {\mathbb{R}}^n\right\}.$$

(13)

Its Lie algebra is:

$$\mathfrak{se}\left(n\right)=\left\{\left[\begin{array}{cc}{\left[\mathbf{w}\right]}_{\times }& \mathbf{u}\\ {\mathbf{0}}_{1\times n}& 0\end{array}\right]|\mathbf{w}\in {\mathbb{R}}^{\frac{n(n-1)}{2}},\mathbf{u}\in {\mathbb{R}}^n\right\}.$$

(14)

3.4. Camera Observation Model on LGs

The observation camera model (1) can be written in a compact way on the LG $SE\left(3\right)$. Indeed, the camera rotation ${\mathbf{R}}_{WC}^{\left(k\right)}$ and the associated translation ${\mathbf{p}}_{WC}^{\left(k\right)}$ constitute the rigid-body transformation of the camera and has the following form:

$${\mathbf{X}}_{WC}^{\left(k\right)}=\left[\begin{array}{cc}{\mathbf{R}}_{WC}^{\left(k\right)}& {\mathbf{p}}_{WC}^{\left(k\right)}\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]\in SE\left(3\right).$$

(15)

In a similar way for the landmarks, the rotation matrix ${\mathbf{R}}_{WP}^{\left(j\right)}$ and the associated translation ${\mathbf{p}}_{WP}^{\left(j\right)}$ are written as:

$${\mathbf{X}}_{WP}^{\left(j\right)}=\left[\begin{array}{cc}{\mathbf{R}}_{WP}^{\left(j\right)}& {\mathbf{p}}_{WP}^{\left(j\right)}\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]\in SE\left(3\right).$$

(16)

According to Figure 2, we can express the successive $SE\left(3\right)$ transformations between the camera frame C and the pattern frame P in the following way:

$$\begin{array}{cc}\hfill \left[{\left({\mathbf{q}}_{i,W}^{\left(j\right)}\right)}^{\top },1\right]& ={\mathbf{X}}_{WP}^{\left(j\right)}\left[{\mathbf{q}}_i^{\top },1\right]\hfill \end{array}$$

(17)

$$\begin{array}{cc}\hfill {\left[{\left({\mathbf{q}}_{i,C}^{\left(j\right)}\right)}^{\top },1\right]}^{\top }& ={\left({\mathbf{X}}_{WC}^{\left(k\right)}\right)}^{-1}{\left[{\left({\mathbf{q}}_{i,W}^{\left(j\right)}\right)}^{\top },1\right]}^{\top }.\hfill \end{array}$$

(18)

Consequently, the pinhole model (1) can be written as:

$$\begin{array}{c}\hfill {\left[{\left({\mathbf{u}}_{k,i}^{\left(j\right)}\right)}^{\top },1\right]}^{\top }=\mathbf{h}({\mathbf{X}}_{WC}^{\left(k\right)},{\mathbf{X}}_{WP}^{\left(j\right)})+{\left[{\left({\mathbf{n}}_{k,i}^{\left(j\right)}\right)}^{\top },0\right]}^{\top },\end{array}$$

(19)

where:

$$\mathbf{h}({\mathbf{X}}_{WC}^{\left(k\right)},{\mathbf{X}}_{WP}^{\left(j\right)})={\displaystyle \frac{\mathbf{K}{\mathbf{I}}_{3\times 4}{\left({\mathbf{X}}_{WC}^{\left(k\right)}\right)}^{-1}{\mathbf{X}}_{WP}^{\left(j\right)}{\left[{\mathbf{q}}_i^{\top },1\right]}^{\top }}{{\left({\mathbf{I}}_{3\times 4}{\left({\mathbf{X}}_{WC}^{\left(k\right)}\right)}^{-1}{\mathbf{X}}_{WP}^{\left(j\right)}{\left[{\mathbf{q}}_i^{\top },1\right]}^{\top }\right)}_3}},$$

(20)

and $\mathbf{K}$ corresponds to the camera’s calibration matrix:

$$\mathbf{K}=\left[\begin{array}{ccc}{\nu }_{x}f& 0& x_0\\ 0& {\nu }_{y}f& y_0\\ 0& 0& 1\end{array}\right].$$

(21)

It is also interesting to note that the model can be written by substituting L:

$${\left[{\left({\mathbf{u}}_{k,i}^{\left(j\right)}\right)}^{\top },1\right]}^{\top }={\mathbf{h}}^L({\mathbf{X}}_{WC}^{\left(k\right)},{\mathbf{X}}_{WP}^{\left(j\right)};L)+{\left[{\left({\mathbf{n}}_{k,i}^{\left(j\right)}\right)}^{\top },0\right]}^{\top },$$

(22)

where:

$${\mathbf{h}}^L({\mathbf{X}}_{WC}^{\left(k\right)},{\mathbf{X}}_{WP}^{\left(j\right)},L)={\displaystyle \frac{\mathbf{K}{\mathbf{I}}_{3\times 4}{\left({\mathbf{X}}_{WC}^{\left(k\right)}\right)}^{-1}{\mathbf{X}}_{WP}^{\left(j\right)}{\left[L{\mathbf{e}}_i,1\right]}^{\top }}{{\left({\mathbf{I}}_{3\times 4}{\left({\mathbf{X}}_{WC}^{\left(k\right)}\right)}^{-1}{\mathbf{X}}_{WP}^{\left(j\right)}{\left[L{\mathbf{e}}_i,1\right]}^{\top }\right)}_3}}$$

(23)

$$\left\{\begin{array}{l}{\mathbf{e}}_1=\left[0,0,0\right]\left(24\right)\\ {\mathbf{e}}_2=\left[1,0,0\right]\left(25\right)\\ {\mathbf{e}}_3=\left[0,1,0\right]\left(26\right)\\ {\mathbf{e}}_4=\left[1,1,0\right].\left(27\right)\end{array}\right.$$

This reformulation allows us to define an inverse problem for L. Thus, when the latter is unknown, it is also possible to estimate it conjointly with the camera pose.

4. SLAM Filtering Problem on Lie Groups

In the state-of-the-art VSLAM approaches (Euclidean or Lie groups), the aim is to obtain a recursive estimation of both the pose of the camera and the pose of each pattern in the world frame. The unknown state contains, at each instant, the pose parameters (on $SE\left(3\right)$ or on ${\mathbb{R}}^6$) and the set of 3D points on ${\mathbb{R}}^3$. Contrary to the Euclidean VSLAM approach, we propose, in the same way as [21], to define the pose parameters as a $SE\left(3\right)$ matrix, but instead of estimating the positions of the 3D points that compose each pattern, we take advantage of the coded patterns approach in order to estimate the $SE\left(3\right)$ transformation ${\mathbf{X}}_{WP}^{\left(j\right)}$. Consequently, if the pattern size is known, the estimation of this transformation provides directly the estimation of the 3D position of the center of each ellipse in that pattern. In this way, the global state belongs to several direct products of $SE\left(3\right)$. To achieve this estimation, we perform SLAM-extended Kalman filtering on LGs (LG-EKF-VSLAM) based on the LG-EKF proposed in [23]. Classically, on a Euclidean space, an extended Kalman filter is a recursive filtering algorithm which enables the approximation of the posterior distribution of parameters of interest by a Gaussian distribution. The LG-EKF can be seen as a generalization of the latter in the case where the parameters are lying on LG.

The main difference with the algorithm proposed in [23] is that we express the landmark initialization phase (mean and covariance) using a concentrated Gaussian on LG.

4.1. Gaussian Distribution on LGs

To perform filtering on LGs, probability distributions on these manifolds have to be defined. Some have already been introduced in the literature, in particular for smooth manifolds [30,31]. We focus, in this paper, on the Gaussian distributions (CGD) on LGs, introduced in [32], which generalizes the concept of Euclidean Gaussian multivariate probability distribution functions (pdf). Indeed, they have similar properties, such as minimizing the entropy under some constraints.

It is possible to obtain a realization from a CGD thanks to a Euclidean Gaussian sample. Indeed, if $\mathit{ϵ}$ is a Gaussian vector with covariance matrix $\mathbf{P}$, and $\mathit{\mu }$ is a matrix defined on a LG G of intrinsic dimension m, then $\mathbf{X}=\mathit{\mu }{\mathrm{Exp}}_G^{\wedge }\left(\mathit{ϵ}\right)$ is distributed according to a left-concentrated Gaussian pdf (CGD) (in a similar way, we can define a right CGD where sample X can be written $\mathbf{X}={\mathrm{Exp}}_G^{\wedge }\left(\mathit{ϵ}\right)\mathit{\mu }$) on G, with parameters $\mathit{\mu }\in G$ and $\mathbf{P}\in {\mathbb{R}}^{m\times m}$. We note that $\mathbf{X}\sim {\mathcal{N}}_G^L(\mathbf{X};\mathit{\mu },\mathbf{P})$. If $\mathit{\mu }$ is close to $\mathbf{I}$, the expression of this pdf can be approximated by:

$$p\left(\mathbf{X}\right)\simeq {\displaystyle \frac{1}{\sqrt{{\left(2\pi \right)}^m\left|\mathbf{P}\right|}}}{exp}^{-\frac{1}{2}\left|\right|{\mathrm{Log}}_G^{\vee }\left({\mu }^{-1}\mathbf{X}\right){\left|\right|}_{\mathbf{P}}^2},$$

(28)

where $\left|\mathbf{P}\right|$ is the determinant of the matrix $\mathbf{P}$ and ${\parallel .\parallel }_{\mathbf{P}}^2$ is the Mahalanobis distance.

4.2. Unknown State and Evolution Model

The unknown state is constituted of the camera pose ${\mathbf{X}}_{WC}^{\left(k\right)}\in SE\left(3\right)$ and the transformation ${\mathbf{X}}_{WP}^{\left(j\right)}$ of every pattern $j,\forall j\in 〚1,m_k〛$, where $m_k$ is the number of already-detected patterns from an initial instant to instant k.

Due to the dynamics of the robot, some other Euclidean parameters (for instance, the vehicle velocity or the ellipse size) may have to be integrated in the state. Thus, to keep the LG compact in form, such additional parameters can be added into a vector ${\mathbf{g}}^{\left(k\right)}\in {\mathbb{R}}^p$. Thus, the global state (without landmarks) of our VSLAM problem can be written as:

$${\mathbf{X}}_a^{\left(k\right)}=\left[\begin{array}{cc}{\mathbf{X}}_{WC}^{\left(k\right)}& {\mathbf{0}}_{4\times \left(p+1\right)}\\ {\mathbf{0}}_{\left(p+1\right)\times \left(p+1\right)}& \begin{array}{cc}{\mathbf{I}}_{p\times p}& {\mathbf{g}}^{\left(k\right)}\\ {\mathbf{0}}_{1\times p}& 1\end{array}\end{array}\right]\in SE\left(3\right)\times {\mathbb{R}}^p.$$

As a consequence, the LG unknown global state ${\mathbf{X}}^{\left(k\right)}$, containing ${\mathbf{X}}_a^{\left(k\right)}$ and the set of pattern transformations, can be written as:

$${\mathbf{X}}^{\left(k\right)}=\left[\begin{array}{cccc}{\mathbf{X}}_a^{\left(k\right)}& {\mathbf{0}}_{(5+p)\times 4}& \dots & {\mathbf{0}}_{(5+p)\times 4}\\ {\mathbf{0}}_{4\times (5+p)}& {\mathbf{X}}_{WP}^{\left(1\right)}& & {\mathbf{0}}_{4\times 4}\\ ⋮& & \ddots & ⋮\\ {\mathbf{0}}_{4\times (5+p)}& {\mathbf{0}}_{4\times 4}& \dots & {\mathbf{X}}_{WP}^{\left(m_k\right)}\end{array}\right],$$

(29)

with ${\mathbf{X}}^{\left(k\right)}\in G=SE\left(3\right)\times {\mathbb{R}}^p\times SE{\left(3\right)}^{m_k}$ being a square matrix with size $d_k^X=5+p+4m_k$.

To perform the filtering, we have to define the evolution model of this state. By assuming that the patterns are static, the global evolution model can be written as:

$$\mathbf{\Pi }\left({\mathbf{X}}^{\left(k\right)}\right)=\mathbf{\Pi }\left({\mathbf{X}}^{(k-1)}\right){\mathrm{Exp}}_{G^{\prime }}^{\wedge }(\mathbf{\Omega }({\mathbf{X}}^{(k-1)},{\mathbf{w}}^{(k-1)})+{\mathbf{n}}^{\left(k\right)}),$$

(30)

where $\mathbf{\Pi }$ is a function which expresses the state on the LG $G^{\prime }=SO\left(3\right)\times {\mathbb{R}}^3\times {\mathbb{R}}^p\times SE{\left(3\right)}^{m_k}$ and defines two separated dynamic models on the rotation ${\mathbf{R}}_{WC}^{\left(k\right)}$ and the position ${\mathbf{p}}_{WC}^{\left(k\right)}$.

•

$\mathbf{\Omega }:G^{\prime }\to {\mathbb{R}}^{\dim \left(G^{\prime }\right)}={\left[{\mathbf{\Omega }}_{\mathbf{X}}{\left({\mathbf{X}}_a^{\left(k\right)}\right)}^{\top },{\mathbf{0}}_{1\times 6m_k}\right]}^{\top }$ is a dynamic evolution function (where $\dim \left(G^{\prime }\right)$ corresponds to the dimension of $G^{\prime }$);

•

${\mathbf{w}}^{(k-1)}$ is a control input;

•

${\mathbf{n}}^{\left(k\right)}={\left[{\left({\mathbf{n}}_a^{\left(k\right)}\right)}^{\top },{\mathbf{0}}_{1\times 6m_k}\right]}^{\top }$, where ${\mathbf{n}}_a^{\left(k\right)}$ is a white Gaussian noise on ${\mathbb{R}}^{\dim \left(G^{\prime }\right)}$, with covariance matrix ${\mathbf{Q}}_a^{\left(k\right)}$.

4.3. Principle of LG-EKF

The aim of the LG-EKF is to recursively approximate the posterior distribution of the unknown parameters defined on LG by a left CGD (note that it also possible to define the LG-EKF-SLAM with a right CGD). Precisely, in our case, we want to approach $p\left({\mathbf{X}}^{\left(k\right)}\right|{\mathbf{u}}_{(1:k)})$ by:

$$p\left({\mathbf{X}}^{\left(k\right)}\right|{\mathbf{u}}_{(1:k)})\simeq {\mathcal{N}}_G^L({\mathbf{X}}^{\left(k\right)};{\widehat{\mathbf{X}}}^{\left(k\right|k)},{\mathbf{P}}^{\left(k\right|k)}),$$

(31)

where ${\mathbf{u}}_{(1:k)}$ is the set of all collected measurements from instant 1 to k. In the same way as the classical EKF, the estimation is done in two steps: prediction and update.

4.3.1. The Prediction Step

The objective of this step is to estimate the distribution $p\left({\mathbf{X}}^{\left(k\right)}\right|{\mathbf{u}}_{(1:k-1)})$ (i.e., estimating the values $\{{\widehat{\mathbf{X}}}^{\left(k\right|k-1)},{\mathbf{P}}^{\left(k\right|k-1)}\}$), given a prior on the distribution $p\left({\mathbf{X}}^{(k-1)}\right|{\mathbf{u}}_{(1:k-1)})$ (defined by $\{{\widehat{\mathbf{X}}}^{(k-1|k-1)},{\mathbf{P}}^{(k-1|k-1)}\}$). As ${\mathbf{n}}_a^{\left(k\right)}$ is zero-mean, the prediction of ${\widehat{\mathbf{X}}}^{(k-1|k-1)}$ is given, thanks to the noise-free dynamic model (30) on LG, by:

$$\begin{array}{cc}\hfill & \mathbf{\Pi }\left({\widehat{\mathbf{X}}}^{\left(k\right|k-1)}\right)=\mathbf{\Pi }\left({\widehat{\mathbf{X}}}^{(k-1|k-1)}\right){\mathrm{Exp}}_{G^{\prime }}^{\wedge }\left(\mathbf{\Omega }\left({\widehat{\mathbf{X}}}^{(k-1|k-1)}\right)\right)\hfill \end{array}$$

(32)

and ${\mathbf{P}}^{\left(k\right|k-1)}$ is given by:

$$\begin{array}{c}{\mathbf{P}}^{\left(k\right|k-1)}={\mathbf{F}}^{\left(k\right)}{\mathbf{P}}^{(k-1|k-1)}{\left({\mathbf{F}}^{\left(k\right)}\right)}^{\top }+\hfill & {\mathit{\varphi }}_G(-\mathbf{\Omega }\left({\widehat{\mathbf{X}}}^{(k-1|k-1)}\right)){\mathbf{Q}}^{\left(k\right)}{\mathit{\varphi }}_G{(-\mathbf{\Omega }\left({\widehat{\mathbf{X}}}^{(k-1|k-1)}\right))}^{\top },\hfill \end{array}$$

(33)

where:

$$\begin{array}{cc}\hfill {\mathbf{F}}^{\left(k\right)}& =A{d}_G({\mathrm{Exp}}_G^{\wedge }\left(\mathbf{\Omega }(-{\widehat{\mathbf{X}}}^{(k-1|k-1)})\right)+{\mathit{\varphi }}_G\left(\mathbf{\Omega }(-{\widehat{\mathbf{X}}}^{(k-1|k-1)})\right){\mathbf{C}}^{(k-1)}\hfill \end{array}$$

(34)

$$\begin{array}{cc}\hfill {\mathbf{C}}^{(k-1)}& =D_{{\widehat{\mathbf{X}}}^{(k-1|k-1)}}\mathbf{\Omega }\hfill \end{array}$$

(35)

$$\begin{array}{cc}\hfill {\mathbf{Q}}^{\left(k\right)}& =\left[\begin{array}{cc}{\mathbf{Q}}_a^{\left(k\right)}& {\mathbf{0}}_{9\times 6m_k}\\ {\mathbf{0}}_{6m_k\times 9}& {\mathbf{0}}_{6m_k\times 6m_k}\end{array}\right].\hfill \end{array}$$

(36)

4.3.2. The Update Step

The update step takes advantage of the camera observation ${\mathbf{u}}_{k,i}^{\left(j\right)}$ provided at instant k of the $j\mathrm{th}$ already-known pattern, in order to approximate:

$$p\left({\mathbf{X}}^{\left(k\right)}\right|{\mathbf{u}}_{(1:k)})\simeq {\mathcal{N}}_G^L({\mathbf{X}}^{\left(k\right)};{\widehat{\mathbf{X}}}^{\left(k\right|k)},{\mathbf{P}}^{\left(k\right|k)}).$$

(37)

The parameters $\{{\widehat{\mathbf{X}}}^{\left(k\right|k)},{\mathbf{P}}^{\left(k\right|k)}\}$ are written as:

$$\begin{array}{cc}\hfill & {\widehat{\mathbf{X}}}^{\left(k\right|k)}={\widehat{\mathbf{X}}}^{\left(k\right|k-1)}{\mathrm{Exp}}_G^{\wedge }\left({\mathbf{K}}^{\left(k\right)}{\mathbf{m}}^{\left(k\right)}\right),\hfill \end{array}$$

(38)

$$\begin{array}{cc}\hfill & {\mathbf{P}}^{\left(k\right|k)}={\mathit{\varphi }}_G(-{\mathbf{K}}^{\left(k\right)}{\mathbf{m}}^{\left(k\right)}){\mathbf{P}}^{\left(k\right|k,-)}{\mathit{\varphi }}_G{(-{\mathbf{K}}^{\left(k\right)}{\mathbf{m}}^{\left(k\right)})}^{\top },\hfill \end{array}$$

(39)

where:

$$\begin{array}{cc}\hfill \phantom{(}{\mathbf{m}}^{\left(k\right)}& =\stackrel{\mathrm{Innovation}\mathrm{vector}}{\overbrace{\left({\tilde{\mathbf{u}}}_k^{\left(j\right)}-\tilde{\mathbf{h}}({\widehat{\mathbf{X}}}_a^{\left(k\right|k-1)},{\widehat{\mathbf{X}}}_{WP}^{\left(j\right)})\right)}}\hfill \\ \hfill {\mathbf{K}}^{\left(k\right)}& ={\mathbf{P}}^{\left(k\right|k-1)}{\left({\mathbf{J}}^{\left(k\right)}\right)}^{\top }\times \hfill \end{array}$$

(40)

$$\begin{array}{cc}\hfill & {\left({\mathbf{J}}^{\left(k\right)}{\mathbf{P}}^{\left(k\right|k-1)}{\left({\mathbf{J}}^{\left(k\right)}\right)}^{\top }+\tilde{\mathbf{N}}\right)}^{-1}\hfill \end{array}$$

(41)

$$\begin{array}{cc}\hfill {\mathbf{P}}^{\left(k\right|k,-)}& =(\mathbf{I}-{\mathbf{K}}^{\left(k\right)}{\mathbf{J}}^{\left(k\right)}){\mathbf{P}}^{\left(k\right|k-1)},\hfill \end{array}$$

(42)

and:

$$\begin{array}{cc}\hfill & •\tilde{\mathbf{N}}=\left[\begin{array}{cc}\mathbf{N}& 0\\ 0& 0\end{array}\right]\hfill \end{array}$$

(43)

$$\begin{array}{cc}\hfill & •{\tilde{\mathbf{u}}}_k^{\left(j\right)}={\left[{{\tilde{\mathbf{u}}}_{k,1}^{\left(j\right)}}^{\top },\dots ,{{\tilde{\mathbf{u}}}_{k,4}^{\left(j\right)}}^{\top }\right]}^{\top }\hfill \end{array}$$

(44)

$$\begin{array}{cc}& \mathrm{with}{\tilde{\mathbf{u}}}_{k,i}^{\left(j\right)}={\left[{\left({\mathbf{u}}_{k,i}^{\left(j\right)}\right)}^{\top },1\right]}^{\top }\hfill \\ \hfill & •\tilde{\mathbf{h}}({\widehat{\mathbf{X}}}_a^{\left(k\right|k-1)},{\widehat{\mathbf{X}}}_{WP}^{\left(j\right)})=\hfill \end{array}$$

(45)

$$\underset{\mathrm{repeated} 4 \mathrm{times} \mathrm{for} \mathrm{the} 4 \mathrm{measurements}}{\underbrace{{\left[\mathbf{h}{\left({\widehat{\mathbf{X}}}_a^{\left(k|k-1\right)},{\widehat{\mathbf{X}}}_{WP}^{\left(j\right)}\right)}^{\top },\mathrm{\dots },\mathbf{h}{\left({\widehat{\mathbf{X}}}_a^{\left(k|k-1\right)},{\widehat{\mathbf{X}}}_{WP}^{\left(j\right)}\right)}^{\top }\right]}^{\top }}}$$

(46)

$$•J\left(k\right)\left[\begin{array}{cccc}{\mathbf{J}}_c^{\left(k\right)}& {\mathbf{0}}_{2\times 6(j-1)}& {\mathbf{J}}_p^{\left(j\right)}& {\mathbf{0}}_{2\times 6(m_k-j)}\end{array}\right]\odot {\mathbf{I}}_{4\times 1}$$

(47)

is the Jacobian matrix of $\tilde{\mathbf{h}},\mathrm{according}\mathrm{to}{\widehat{\mathbf{X}}}^{\left(k\right|k-1)},\mathrm{with}$:

$$\begin{array}{cc}\hfill & {\mathbf{J}}_c^{\left(k\right)}=D_{{\widehat{\mathbf{X}}}_a^{\left(k\right|k-1)}}\mathbf{h}\hfill \end{array}$$

(48)

$$\begin{array}{cc}\hfill & {\mathbf{J}}_p^{\left(j\right)}=D_{{\widehat{\mathbf{X}}}_{WP}^{\left(j\right)}}\mathbf{h},\hfill \end{array}$$

(49)

and $\odot \mathrm{defines}\mathrm{the}\mathrm{Kronecker}\mathrm{product}.$

Remark 1.

It is important to note that if several patterns are detected at the same instant, the update step is recursively realized for each of them.

4.4. Initialization of Pattern on $SE\left(3\right)$

If a new pattern is detected by the camera, the step is called the mapping step. As a new object ${\mathbf{X}}_{WP}^{\left(j\right)}$ is detected, the current state ${\mathbf{X}}^{\left(k\right)}$ is augmented as follows:

$${\mathbf{X}}_p^{\left(k\right)}=\left[\begin{array}{cc}{\mathbf{X}}^{\left(k\right)}& 0_{d_{k-1}^X\times 4}\\ 0_{4\times d_{k-1}^X}& {\mathbf{X}}_{WP}^{\left(j\right)}\end{array}\right].$$

(50)

Consequently, the associated covariance to this state can be written as:

$${\mathbf{P}}_a^{\left(k\right|k)}=\left[\begin{array}{cc}{\mathbf{P}}^{\left(k\right|k)}& {\mathbf{P}}_{CP}^{\left(k\right|k)}\\ {\left({\mathbf{P}}_{CP}^{\left(k\right|k)}\right)}^{\top }& {\mathbf{P}}_{WP}^{\left(k\right|k)}\end{array}\right],$$

(51)

where:

• ${\mathbf{P}}_{CP}^{\left(k\right|k)}$ corresponds to the correlation between the new pattern and the previous state;
• ${\mathbf{P}}_{WP}^{\left(k\right|k)}$ is the covariance associated to the new pattern j.

At this step, we need to have an initial estimation of the pose of the pattern and of its associated covariance matrix. Classically, in a Euclidean framework, the initialization of a pattern is determined by finding the solution of the inverse of the observation model, and the covariance matrix is approximated by computing correlations using associated Jacobians [33].

In the case of the camera model, this approach is not feasible due to its analytical complexity, and numeric approaches must be considered. For instance, it is possible to find the initial state by implementing a local bundle adjustment, which optimizes a criterion built from the observation model. The covariance matrix of pattern j can be obtained by a Gauss–Laplace approximation [34], and then the global covariance matrix of the new state is updated according to Equation (39). In this work, we propose an adaptation of this method, using the LG formalism.

The initialization of ${\mathbf{X}}_{WP}^{\left(j\right)}$ can be obtained by minimizing the anti-logarithm (opposite of the logarithm) of the posterior distribution $p\left({\mathbf{X}}_{WP}^{\left(j\right)}\right|{\left\{{\mathbf{u}}_{k,i}^{\left(j\right)}\right\}}_{i=1}^4,{\mathbf{u}}_{(1:k-1)},{\widehat{\mathbf{X}}}_{WC}^{\left(k\right|k-1)})$, associated to the measurement j. Due to its non-tractability, we propose approaching it by a left CGD with parameters $\{{\widehat{\mathbf{X}}}_{WP},{\mathbf{P}}_{WP}\}$:

$$\begin{array}{cc}\hfill & p\left({\mathbf{X}}_{WP}^{\left(j\right)}\right|{\left\{{\mathbf{u}}_{k,i}^{\left(j\right)}\right\}}_{i=1}^4,{\mathbf{u}}_{(1:k-1)},{\widehat{\mathbf{X}}}_{WC}^{\left(k\right|k-1)})\hfill \end{array}$$

$$\begin{array}{c}\hfill \simeq {\mathcal{N}}_{SE\left(3\right)}^L({\mathbf{X}}_{WP}^{\left(j\right)};{\widehat{\mathbf{X}}}_{WP},{\mathbf{P}}_{WP}),\end{array}$$

(52)

where ${\widehat{\mathbf{X}}}_{WP}$ is the estimated pose of the new pattern and ${\mathbf{P}}_{WP}$ its associated covariance.

4.4.1. Computation of ${\widehat{\mathbf{X}}}_{WP}$

${\widehat{\mathbf{X}}}_{WP}$ can be determined by maximizing the posterior distribution (52). Due to the Bayes rule, we show that:

$$\begin{array}{cc}\hfill & p\left({\mathbf{X}}_{WP}^{\left(j\right)}\right|{\left\{{\mathbf{u}}_{k,i}^{\left(j\right)}\right\}}_{i=1}^4,{\mathbf{u}}_{(1:k-1)},{\widehat{\mathbf{X}}}_{WC}^{\left(k\right|k-1)})\hfill \end{array}$$

$$\begin{array}{c}\hfill \propto \prod _{i=1}^{4}p\left({\mathbf{u}}_{k,i}^{\left(j\right)}\right|{\widehat{\mathbf{X}}}_{WC}^{\left(k\right|k-1)},{\mathbf{u}}_{(1:k-1)},{\mathbf{X}}_{WP}^{\left(j\right)})\end{array}$$

(53)

$$\begin{array}{cc}\hfill & \simeq \prod _{i=1}^4{\mathcal{N}}_{{\mathbb{R}}^2}\left({\mathbf{u}}_{k,i}^{\left(j\right)};{\mathbf{h}}_p({\widehat{\mathbf{X}}}_{WC}^{\left(k\right|k-1)},{\mathbf{X}}_{WP}^{\left(j\right)}),\mathbf{N}\right),\hfill \end{array}$$

(54)

where ${\mathbf{h}}_p$ is a function conserving the two first components of $\mathbf{h}$. Consequently, we seek to find the minima of the following criterion corresponding to the anti-logarithm of (54):

$${\widehat{\mathbf{X}}}_{WP}=\underset{{\mathbf{X}}_{WP}\in SE\left(3\right)}{argmin}{\displaystyle \frac{1}{2}}{\parallel \mathit{\varphi }\left({\mathbf{X}}_{WP}\right)\parallel }_{\mathbf{\Sigma }}^2,$$

(55)

where:

$$\left\{\begin{array}{l}{\left[\mathit{\varphi }\left(X_{WP}\right)\right]}_i={\mathbf{u}}_{k,i}^{\left(j\right)}-{\mathbf{h}}_p\left({\widehat{\mathbf{X}}}_{WC}^{\left(k|k-1\right)},{\mathbf{X}}_{WP}\right)\forall i\in \left\{1,\mathrm{\dots },4\right\}(56)\\ \sum =\mathbf{N}\odot {\mathbf{I}}_{4\times 4}(57)\end{array}\right.$$

and defines an optimization problem on $SE\left(3\right)$. A local minima is obtained thanks to a Gauss–Newton algorithm on LG [35]. At each iteration l, the updated recursive formula is written as:

$${\mathbf{X}}_{WP}^{\left(l\right)}={\mathbf{X}}_{WP}^{(l-1)}{\mathrm{Exp}}_{SE\left(3\right)}^{\wedge }\left({\mathit{\delta }}^{\left(l\right)}\right),$$

(58)

where ${\mathit{\delta }}^{\left(l\right)}$ is a descent direction, computed in the following way:

$${\mathit{\delta }}^{\left(l\right)}=-{\left({\left({\mathbf{J}}_{\mathit{\varphi }}^{(l-1)}\right)}^{\top }{\mathbf{\Sigma }}^{-1}{\mathbf{J}}_{\mathit{\varphi }}^{(l-1)}\right)}^{-1}{\left({\mathbf{J}}_{\mathit{\varphi }}^{(l-1)}\right)}^{\top }{\mathbf{\Sigma }}^{-1}{\mathit{\varphi }}^{(l-1)},$$

(59)

with:

$$\begin{array}{cc}\hfill {\mathbf{J}}_{\mathit{\varphi }}^{(l-1)}& =D_{{\mathbf{X}}_{WP}^{(l-1)}}\mathit{\varphi }\hfill \end{array}$$

(60)

$$\begin{array}{cc}\hfill {\mathit{\varphi }}^{(l-1)}& =\mathit{\varphi }\left({\mathbf{X}}_{WP}^{(l-1)}\right).\hfill \end{array}$$

(61)

4.4.2. Computation of ${\mathbf{P}}_{WP}$

The initial covariance of ${\mathbf{X}}_{WP}$ is obtained by using a Gauss–Laplace approximation on LG. It consists to approximate $\mathit{\varphi }$ around ${\widehat{\mathbf{X}}}_{WP}$ under the form:

$$\mathit{\varphi }\left({\mathbf{X}}_{WP}\right)\simeq \mathit{\varphi }\left({\widehat{\mathbf{X}}}_{WP}\right)+{\mathbf{J}}_{\mathit{\varphi }}{\mathrm{Log}}_{SE\left(3\right)}^{\vee }\left({\mathbf{X}}_{WP}^{-1}{\widehat{\mathbf{X}}}_{WP}\right).$$

(62)

As ${\widehat{\mathbf{X}}}_{WP}$ is a critical point of ${\parallel \mathit{\varphi }\left(\mathbf{X}\right)\parallel }_{\mathbf{\Sigma }}^2$, we obtain:

$$\begin{array}{cc}\hfill & p\left({\mathbf{X}}_{WP}^{\left(j\right)}\right|{\mathbf{u}}_{(1:k-1)},{\widehat{\mathbf{X}}}_{WC}^{\left(k\right|k-1)})\simeq \hfill \end{array}$$

(63)

$$\begin{array}{cc}& {exp}^{-{\displaystyle \frac{1}{2}}{\mathrm{Log}}_{SE\left(3\right)}^{\vee }\left({\mathbf{X}}_{WP}^{-1}{\widehat{\mathbf{X}}}_{WP}\right){}^{\top }{\mathbf{J}}_{\mathit{\varphi }}^{\top }{\mathbf{\Sigma }}^{-1}{\mathbf{J}}_{\mathit{\varphi }}{\mathrm{Log}}_{SE\left(3\right)}^{\vee }\left({\mathbf{X}}_{WP}^{-1}{\widehat{\mathbf{X}}}_{WP}\right)}.\hfill \end{array}$$

(64)

By identification, with the expression of the CGD, it implies that ${\mathbf{P}}_{WP}$ is approximated by:

$${\mathbf{P}}_{WP}={\mathbf{J}}_{\mathit{\varphi }}^{\top }{\mathbf{\Sigma }}^{-1}{\mathbf{J}}_{\mathit{\varphi }}.$$

(65)

To obtain the cross-correlation matrix ${\mathbf{P}}_{CP}^{\left(k\right|k)}$, as in (51), we perform an update step of the increased state (50) with the measurement ${\mathbf{u}}_{k,i}^{\left(j\right)}$.

5. Experiments

In this section, we compare our proposed approach, the LG-EKF-VSLAM, to the standard EKF-VSLAM algorithm while using coded patterns. We first carried out two Monte-Carlo simulations in order to evaluate the usefulness of performing LG modelling for monocular visual SLAM. In the first scenario, we considered that the distance between the center of two ellipse on the coded patterns, L, is perfectly known. For the second simulation, L is added to the state vector as a parameter to be estimated.

We then evaluated the LG-EKF-VSLAM with real data obtained within our laboratory, since, to the best of our knowledge, there are no available public benchmarks with coded patterns.

5.1. Experiments with Simulated Data

In the simulated scenarios, we considered a wheeled robot following a trajectory generated according to a constant velocity model. The control input is given by a simulated angular velocity ${\mathbf{w}}^{\left(k\right)}$. The unknown state is constituted of the camera pose ${\mathbf{X}}_{WC}^{\left(k\right)}$ and the linear velocity ${\mathbf{v}}^{\left(k\right)}$:

$${\mathbf{X}}_a^{\left(k\right)}=\left[\begin{array}{cc}{\mathbf{X}}_{WC}^{\left(k\right)}& {\mathbf{0}}_{4\times 4}\\ {\mathbf{0}}_{4\times 4}& \begin{array}{cc}{\mathbf{I}}_{3\times 3}& {\mathbf{v}}^{\left(k\right)}\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\end{array}\right]\in SE\left(3\right)\times {\mathbb{R}}^3.$$

The discrete dynamic model is written as:

$$\begin{array}{cc}\hfill {\mathbf{v}}^{\left(k\right)}& ={\mathbf{v}}^{(k-1)}+{\mathbf{n}}_v{\delta }_t\hfill \end{array}$$

(66)

$$\begin{array}{cc}\hfill {\mathbf{p}}_{WC}^{\left(k\right)}& ={\mathbf{p}}_{WC}^{(k-1)}+{\mathbf{v}}^{(k-1)}\delta t+{\mathbf{n}}_p{\delta }_t\hfill \end{array}$$

(67)

$$\begin{array}{cc}\hfill {\mathbf{R}}_{WC}^{\left(k\right)}& ={\mathbf{R}}_{WC}^{(k-1)}{\mathrm{Exp}}_{SO\left(3\right)}^{\wedge }({\mathbf{w}}^{(k-1)}\delta t+{\mathbf{n}}_r{\delta }_t),\hfill \end{array}$$

(68)

where $\delta t$ is the sampling rate. ${\mathbf{n}}_v,{\mathbf{n}}_w$, ${\mathbf{n}}_p$, and ${\mathbf{n}}_r$ are four white Gaussian noises on linear and angular velocity, position, and orientation, respectively. These equations can be written on G with the form (30), where:

$$\begin{array}{cc}\hfill {\mathbf{\Omega }}_{\mathbf{X}}\left({\mathbf{X}}_a^{(k-1)}\right)& ={\left[{\left({\mathbf{w}}^{(k-1)}\right)}^{\top }\delta t,{\left({\mathbf{v}}^{(k-1)}\right)}^{\top }\delta t,{\mathbf{0}}_{1\times 6m_k}\right]}^{\top }\hfill \end{array}$$

(69)

$$\begin{array}{cc}\hfill {\mathbf{n}}_a^{\left(k\right)}& ={\left[{\mathbf{n}}_r^{\top }\delta t,{\mathbf{n}}_p{\delta t}^{\top },{\mathbf{n}}_v{\delta t}^{\top }\right]}^{\top }.\hfill \end{array}$$

(70)

The obtained trajectory is generated during $T=885s$ with $\delta t=1s$, and is shown in Figure 5. The positions of each coded pattern are defined with a null z-axis and are randomly distributed all along the ground-truth trajectory. Their orientations are obtained by sampling from a CGD on $SO\left(3\right)$, with mean ${\mathbf{I}}_3$ and covariance $0.2^2{\mathbf{I}}_3$.

The camera measurements are simulated according to the model (22), with:

$$\begin{array}{cc}\hfill \mathbf{K}=\left[\begin{array}{ccc}200& 0& 240\\ 0& 200& 320\\ 0& 0& 1\end{array}\right],& \mathbf{N}={\sigma }_n^2{\mathbf{I}}_2\\ & \\ \hfill {\sigma }_n={10}^{-1}\mathrm{pixel},& L=5\mathrm{m}.\end{array}$$

The Jacobian matrices ${\mathbf{C}}^{(k-1)}$, ${\mathbf{J}}^{\left(k\right)}$, and ${\mathit{\varphi }}_G(.)$ of the LG-EKF-VSLAM (from Equations (7), (35), and (47), respectively) have been implemented according to the formulation given in Appendix A.1, Appendix B, and Appendix C.

The performance of the method must be evaluated on a state belonging to $SE\left(3\right)$. Consequently, we studied it by separating the estimator associated with the rotation matrix ${\widehat{\mathbf{R}}}_{WC}^{\left(k\right|k)}$ and the estimator associated with the position ${\widehat{\mathbf{p}}}_{WC}^{\left(k\right|k)}$.

(1)

The quality of ${\widehat{\mathbf{R}}}_{WC}^{\left(k\right|k)}$ can be evaluated by using two intrinsic metrics on LGs, which enables us to compute the rotation error directly on $SO\left(3\right)$:

•

a RMSE (Root Mean Square Error) metric, which can be seen as a generalization of the classical Euclidean RMSE. The classical distance is substituted by the geodesic distance on $SO\left(3\right)$ [36]:

$$\begin{array}{c}RMS{E}_R=\hfill & \sqrt{{\displaystyle \frac{1}{N_{r}T}}\sum _{k=1}^T\sum _{i=1}^{Nr}{\parallel {\mathrm{Log}}_{SO\left(3\right)}^{\vee }\left({\left({\mathbf{R}}_{WC}^{\left(k\right)}\right)}^{-1}{\widehat{\mathbf{R}}}_{WC}^{\left(k\right|k),i}\right)\parallel }^2},\hfill \end{array}$$

(71)

where ${\widehat{\mathbf{R}}}^{\left(k\right|k),i}$ corresponds to the Monte-Carlo realization i of the rotation matrix estimator at epoch k. $N_r$ is the number of realizations;

•

a mean RPE (Relative Pose Error) metric to evaluate the relative pose error between two consecutive epochs for several realizations of the algorithm.

$$\begin{array}{cc}\hfill & RP{E}_R=\hfill \\ & {\displaystyle \frac{1}{N_{r}T}}\sum _{k=1}^T\sum _{i=1}^{N_r}\left|\parallel {\mathrm{Log}}_{SO\left(3\right)}^{\vee }\left({\left({\mathbf{R}}_{WC}^{(k-1)}\right)}^{-1}{\mathbf{R}}_{WC}^{\left(k\right)}\right)\parallel -\right.\hfill \end{array}$$

(72)

$$\begin{array}{cc}& \left.\parallel {\mathrm{Log}}_{SO\left(3\right)}^{\vee }\left({\left({\widehat{\mathbf{R}}}_{WC}^{(k-1|k-1),i}\right)}^{-1}{\widehat{\mathbf{R}}}_{WC}^{\left(k\right|k),i}\right)\parallel \right|;\hfill \end{array}$$

(73)

(2)

As the position parameter ${\mathbf{p}}_{WC}^{\left(k\right)}$ is Euclidean, the quality of its estimator is determined with the classical Euclidean RPE and RMSE.

These performances are compared to a Euclidean EKF-VSLAM. In the latter, the rotation matrix is parametrized by Euler angles ${\left[{\theta }^{\left(k\right)},{\psi }^{\left(k\right)},{\varphi }^{\left(k\right)}\right]}^{\top }$. Consequently, the unknown state is a vector which belongs to ${\mathbb{R}}^{6+p+6m_k}$. The estimation error on rotation and position are directly evaluated by a classical RPE and RMSE between the vector ${\left[{\theta }^{\left(k\right)},{\psi }^{\left(k\right)},{\varphi }^{\left(k\right)},{\left({\mathbf{p}}_{WC}^{\left(k\right)}\right)}^{\top }\right]}^{\top }$ and the estimated vector ${\left[{\widehat{\theta }}^{\left(k\right)},{\widehat{\psi }}^{\left(k\right)},{\widehat{\varphi }}^{\left(k\right)},{\left({\widehat{\mathbf{p}}}_{WC}^{\left(k\right)}\right)}^{\top }\right]}^{\top }$.

5.1.1. Case 1: L Perfectly Known

According to

Table 1. Simulation results with known L. Obtained performance estimation for $N_r=500$ realizations.

	LG-EKF-VSLAM	EKF-VSLAM
RMSE position (m)	0.298	0.427
RMSE orientation (rad)	0.0048	0.0055
RPE position (m)	0.0172	0.0205
RPE orientation (rad)	$5.12\times {10}^{-4}$	$6\times {10}^{-4}$

, we observe that the proposed method slightly outperforms the classical EKF-VSLAM. Two arguments can explain these results:

• Firstly, the LG modelling takes into account the intrinsic properties of the unknown state belonging to $SE\left(3\right)$. Consequently, the indeterminacy of the Euler angle estimates is deleted;
• Secondly, through this modelling, the observation model is freed from the non-linearity introduced by the cosine and sine functions of the rotation matrix in the Euclidean modelling. In this way, the approximation of the covariance estimation error is more precise.

5.1.2. Case 2: L Is Misspecified

In order to challenge the proposed algorithm, we assume that the distance between two ellipses of a pattern is incorrectly specified. Consequently, L is initialized with an initial error on the true value (5 m) equal to 1 m, and added to the state:

$$\left[\begin{array}{cc}{\mathbf{X}}_a^{\left(k\right)}& 0\\ 0& L\end{array}\right]\in SE\left(3\right)\times {\mathbb{R}}^4.$$

(74)

In order to estimate L with the LG-EKF-VSLAM, the observation model that explicitly involves L is considered. From a numerical point of view, we also have to compute the Jacobian of the observation model according to L.

From the obtained results given in

Table 2. Simulation results with misspecified L. Obtained performance estimation for $N_r=500$ realizations.

	LG-EKF-VSLAM	EKF-VSLAM
RMSE position (m)	0.758	1.257
RMSE orientation (rad)	0.0158	0.065
RPE position (m)	0.0398	0.0924
RPE orientation (rad)	$7.2\times {10}^{-4}$	$7.7\times {10}^{-4}$

and Figure 6, we can observe that the LG-EKF-VSLAM achieves the best results. Indeed, compared to the EKF-VSLAM, it is able to asymptotically converge to the true value of L, in spite of high initialization errors (here, $1\mathrm{m})$. Indeed, the estimation error becomes inferior to $0.1\mathrm{m}$. Moreover, the precision of the estimator, after convergence, is better than for the Euclidean EKF-VSLAM (see Table 2).

5.2. Experiment with Real Data

5.2.1. Experiment Setup

To validate the methodology presented in the previous section (Section 4), we performed an experiment with a monocular camera (PointGrey Blackfly from Flir @7fps), with a 3.8–13-mm Varifocal lens, embedded on a mobile cart. Additionally, an Optitrack motion-capture system provided the ground truth for the pose of the camera and the coded patterns. The camera’s reference trajectory and the true positions of the coded patterns (i.e., the center of the bottom-left ellipse) are represented in Figure 8. The camera’s calibration matrix is given by:

$$\mathbf{K}=\left[\begin{array}{ccc}1250.94& 0& 667.14\\ 0& 1251.77& 532.89\\ 0& 0& 1\end{array}\right].$$

(75)

In this experiment, 12 coded patterns, each printed on a $A3$ paper, with $L=0.1415\mathrm{m}$, were placed in different places of the room, as illustrated in Figure 7 and Figure 8. The camera moved in a circular motion and the fiducial pattern-detection algorithm provided observations for each frame. The state ${\mathbf{X}}_a^{\left(k\right)}$ contains the pose, but also the linear and angular velocity ${\mathbf{w}}^{\left(k\right)}$. Indeed, in our experiment, no angular sensor is available; thus, we used a constant velocity model. Consequently, we have:

$${\mathbf{X}}_a^{\left(k\right)}=\left[\begin{array}{ccc}{\mathbf{X}}_{WC}^{\left(k\right)}& {\mathbf{0}}_{4\times 4}& {\mathbf{0}}_{4\times 4}\\ {\mathbf{0}}_{4\times 4}& \begin{array}{cc}{\mathbf{I}}_{3\times 3}& {\mathbf{v}}^{\left(k\right)}\\ {\mathbf{0}}_{1\times 3}& 1\end{array}& {\mathbf{0}}_{4\times 4}\\ {\mathbf{0}}_{4\times 4}& {\mathbf{0}}_{4\times 4}& \begin{array}{cc}{\mathbf{I}}_{3\times 3}& {\mathbf{w}}^{\left(k\right)}\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\end{array}\right]\in SE\left(3\right)\times {\mathbb{R}}^6.$$

(76)

To take into account the circular motion of the mobile in the filter, we propose leveraging a position-prediction model correlating its position and orientation. Indeed, the correlations between these two variables are especially high in the turns.

The proposed model has the following structure:

$${\mathbf{p}}_{WC}^{\left(k\right)}={\mathbf{p}}_{WC}^{(k-1)}+{\mathbf{R}}_{WC}^{(k-1)}{\left[{\left({\mathbf{v}}^{(k-1)}\right)}_1,0,0\right]}^{\top }\delta t+{\mathbf{n}}_p{\delta }_t.$$

(77)

The translation velocity and rotation matrix are assumed to follow the evolution models (66) and (68). Moreover, we suppose that the angular velocity follows a random walk process with a Gaussian white noise ${\mathbf{n}}_w$. Consequently, the function ${\mathbf{\Omega }}_{\mathbf{X}}\left({\mathbf{X}}_a^{(k-1)}\right)$ is written as:

$$\begin{array}{cc}\hfill & {\mathbf{\Omega }}_{\mathbf{X}}\left({\mathbf{X}}_a^{(k-1)}\right)=\hfill \end{array}$$

$$\begin{array}{c}\hfill {\left[{\left({\mathbf{w}}^{(k-1)}\right)}^{\top }\delta t,{\left({\mathbf{R}}_{WC}^{(k-1)}{\left[{\left({\mathbf{v}}^{(k-1)}\right)}_1,0,0\right]}^{\top }\right)}^{\top }\delta t,{\mathbf{0}}_{1\times 6}\right]}^{\top }.\end{array}$$

(78)

The computation of the Jacobian of ${\mathbf{\Omega }}_{\mathbf{X}}$ is provided in Appendix A.2.

5.2.2. Obtained Results

In order to use the method with real data, we first have to define the noise parameters. Concerning the observation model, the covariance noise is taken to be sufficiently high in order to take into account the uncertainties introduced by non-analytical phenomenons. Consequently, we take $\mathbf{N}={\sigma }_n^2{\mathbf{I}}_3$ with ${\sigma }_n=2$ pixels. Concerning the dynamic model, the standard deviation of noises ${\mathbf{n}}_r$, ${\mathbf{n}}_p$, ${\mathbf{n}}_v$, and ${\mathbf{n}}_w$ are, respectively, fixed at ${\sigma }_p=\frac{{10}^{-4}}{{\delta }_t}\mathrm{m}.{\mathrm{s}}^{-1}$${\sigma }_r=\frac{{10}^{-2}}{{\delta }_t}\mathrm{rad}.{\mathrm{s}}^{-2}$, ${\sigma }_v=\frac{{10}^{-6}}{{\delta }_t}\mathrm{m}.{\mathrm{s}}^{-2}$, and ${\sigma }_w=\frac{{10}^{-4}}{{\delta }_t}\mathrm{rad}.{\mathrm{s}}^{-2}$.

In order to quantify the estimation error of the trajectory, we use the Optitrack reference system and compare it with the estimated trajectory given by each of the two methods. This is done by computing the absolute error on the position at each instant of the trajectory, as represented in Figure 9.

In the two approaches, we first observe, through the Figure 10 and Figure 11, that the estimated position is particularly degraded in some parts of the trajectory, especially between the coded patterns 9 and 11 and 6 and 7. This is due to the pattern detector, which does not provide any detection because of blur and specular reflection on the objects. Consequently, during these instants, the estimation is only realized by dead-reckoning navigation, and the non-linearities of the models increases the pose-estimation error. In Figure 9, we observe that the performance of the two approaches are quite similar in terms of absolute error estimation, even if the LG-EKF-VSLAM performs a little bit better all over the trajectory. This is confirmed through Figure 11. Indeed, we observe that during the last turn of the trajectory, the EKF-VSLAM tends to deviate from the ground truth, implying a higher estimation error.

6. Conclusions

In this paper, we propose an original modelling on LGs in order to perform monocular VSLAM. After reformulating the classical pinhole on LGs, we propose a EKF-VSLAM algorithm on LG for coded-pattern landmarks. We take into account the particularity of the SLAM problem by proposing a LG method to initialize new patterns. Our algorithm has been compared with a classical EKF-VSLAM using a Euler parametrization on simulated data as well as on real-world data. Both the simulation and the experiments showed the validity and the performance improvement provided by the proposed LG-EKF-VSLAM, compared to the state-of-the-art solution. In addition, we have shown that a misspecified value on the size of the coded pattern has a large impact on the estimation solution. When this parameter is taken into account as a new state in the estimation process, the EKF-VSLAM reduces the estimation error, but is outperformed by the LG-EKF-VSLAM, which converges asymptotically to the true value of L. This is mostly due to the LG formalism, which copes better with system non-linearities.

A pertinent perspective would be to propose a LG-based approach by taking into account the lack of knowledge of non-estimated parameters models. In our case, L could be considered uncertain, but not estimated. There exist Euclidean filtering approaches dedicated to this problem [37]; consequently, it would be pertinent to adapt them for parameters lying on LGs.

Moreover, in a general way, it would be interesting to adapt the formalism of LG (especially the optimization method) to other applications in computer vision and signal processing. For instance, we can quote the well-known problems of Structure of Motion (SfM) [38] and image registration [39].

From an application point of view, another perspective would be to fuse the camera measurements with, for instance, information from Ultra-Wide-Band sensors or with a Global Navigation Satellite System for outdoor navigation, in order to improve the performance of the algorithm, especially in situations where camera frames can be lost or where no patterns are present in the close environment of the vehicle.