# Dynamic estimation of homography transformations
on the special linear group for visual servo control

Ezio Malis, Tarek Hamel, Robert Mahony and Pascal Morin

**Abstract**—In the last decade, many vision-based robot controllers have been designed using Cartesian information encoded in the homography transformation that links two images of a planar object. For any approach, the performance of the closed-loop system depends on the quality of the homography estimates obtained. In this paper, we exploit the special linear Lie-group structure of the set of all homographies to develop a dynamic observer to estimate homographies on-line. The resulting estimates are effective and can be used to improve closed-loop response of several visual servoing algorithms.

## I. INTRODUCTION

Visual servo control uses the visual information acquired by one or multiple cameras [10], [11], [5] in order to control a robot with respect to a target. This robotic task can be considered as the regulation of a task function that depends on the robot configuration and the time [20]. In the last decade, many vision-based robot controllers have been designed using Cartesian information encoded in the homography transformation that links two images of a planar object. For example, the homography can be decomposed [8], [17] to explicitly reconstruct the pose (the translation and the rotation in Cartesian space) of the camera. Thus, the design of the task function can be done directly in the Cartesian space as, for example, in [22], [15], [2]. The task function can also be defined in both the Cartesian space and the image, i.e. the rotation error is estimated explicitly and the translation error is expressed in the image (see, for example, [16], [6], [7]). These visual servoing approaches make it possible not only to perform the control in the image but also to demonstrate analytically the stability and robustness of the control law [15]. For any of the standard methods, a measure (on-line or off-line) of some 3D information concerning the observed target is needed. Indeed, the pose reconstruction using the homography estimation is not unique (two different solutions are possible) [8], [17]. In order to choose the good solution, it is necessary to have additional information such as an estimate of the vector normal to the target plane. To avoid this requirement, an approach has been recently proposed that uses the homography matrix directly, without explicitly extracting the rotation and translation

[1]. An advantage of such an approach is to avoid poor conditioning of the structure decomposition of homography matrices at certain configurations.

In all cases, however, the performance of the closed-loop system depends on the quality of the homography estimates obtained and it is of interest to consider how to improve these estimates. In the case of visual servo control applications, where the homographies vary continuously with time, it is of interest to consider a dynamical observer (or filter) that achieves temporal smoothing of the data estimates. There has been a surge of interest recently in nonlinear observer design for systems with certain invariance properties [21], [9], [14], [3] that have mostly been applied to applications in robotic vehicles [18], [19]. From these foundations there is an emerging framework for observer design for invariant systems on Lie groups [12], [4], [13].

In [1], the authors proposed to identify the set of all homographies that can be measured by a moving camera continuously observing a planar surface with the special linear group SL(3). In this paper, we exploit the special linear Lie-group structure of the set of all homographies to develop a dynamic observer to estimate homographies on-line. The proposed homography observer is based on constant velocity invariant kinematics on the Lie group. We assume that the velocity is unknown and propose a non-linear observer for both the homography and the velocity estimates. We prove the existence of a Lyapunov function for the system, that guarantees almost global stability and local exponential stability around the desired equilibrium point. Thus, we obtain a high quality temporal smoothing of the homography data along with a velocity estimate. The estimation algorithm has been extensively tested in simulation and on real data. The resulting estimates will be highly effective in improving closed-loop response of visual servo algorithm.

After the introduction, Section II provides a recap of the Lie group structure of the set of homographies. The main contribution of the paper is given in Section III. Sections IV and V provide an experimental study with simulated and real data. They are followed by a short paragraph of conclusions.

## II. THEORETICAL BACKGROUND

A homography is a mapping between two images of a planar scene P. Let $p = (u, v)$ represent the pixel coordinates of a 3D point $\xi \in P$ as observed in the normalized image plane of a pinhole camera. Let $\mathcal{A}$ (resp. $\mathcal{B}$) denote

Ezio Malis is with INRIA-Sophia Antipolis, France, Ezio.Malis@sophia.inria.fr

Tarek Hamel is with I3S-CNRS, Nice-Sophia Antipolis, France, thamel@i3s.unice.fr

Robert Mahony is with the Department of Engineering, ANU, ACT, 0200, Australia, Robert.Mahony@anu.edu.au

Pascal Morin, is with INRIA-Sophia Antipolis, France, Pascal.Morin@sophia.inria.fr
projective coordinates for the image plane of a camera A (resp. B), and {A} (resp. {B}) denote its frame of reference. A (3×3) homography matrix $H : A \to B$ defines the following mapping: $p^B = w(H, p^A)$, where

$$w(H, p) = \begin{bmatrix} (h_{11}u + h_{12}v + h_{13})/(h_{31}u + h_{32}v + h_{33}) \\ (h_{21}u + h_{22}v + h_{23})/(h_{31}u + h_{32}v + h_{33}) \end{bmatrix}$$

The mapping is defined up to a scale factor. That is, for any scaling factor $\mu \neq 0$, $p^B = w(\mu H, p^A) = w(H, p^A)$. If we suppose that the camera continuously observes the planar object, any homography can be represented by a homography matrix $H \in SL(3)$ such that:

$$H = \gamma K \left( R + \frac{tn^\top}{d} \right) K^{-1} \quad (1)$$

where K is the upper triangular matrix containing the camera intrinsic parameters, R is the rotation matrix representing the orientation of {B} with respect to {A}, t is the translation vector of coordinates of the origin of {B} expressed in {A}, n is the normal to the planar surface P expressed in {A}, d is the orthogonal distance of the origin of {A} to the planar surface, and:

$$\gamma = \det \left( R + \frac{tn^\top}{d} \right)^{-\frac{1}{3}} = \left( 1 + \frac{n^\top R t}{d} \right)^{-\frac{1}{3}}$$

Correspondingly, knowing the camera intrinsic parameters K, any full rank 3 × 3 matrix with unitary determinant can be decomposed according to (1) (see [8] for a numerical decomposition and [17] for the analytical decomposition). Note that there exist two possible solutions to the decomposition. The planar surface P is parametrized by

$$P = \{\xi \in \{A\} | n^\top \xi = d\}$$

For any two frames {A} and {B} whose origins lie on the same side of the planar surface P then $n^\top R t > -d$ by construction and the determinant of the associated homography $\det(H) = 1$.

The map w is a group action of SL(3) on $\mathbb{R}^2$:

$$w(H_1, w(H_2, p)) = w(H_1 H_2, p)$$

where $H_1, H_2$ and $H_1 H_2 \in SL(3)$. The geometrical meaning of this property is that the 3D motion of the camera between views {A} and {B}, followed by the 3D motion between views {B} and {C} is the same as the 3D motion between views {A} and {C}.

**Remark 2.1:** The local parametrization given by (1) is singular when {A} and {B} are collocated. That is, when $t = 0$, the differential of the mapping defined by (1) is degenerate. Indeed, in this case the normal to the plane n is not observable. The singularity of the parametrization does not affect the validity of the correspondence $H \equiv SL(3)$, however, it does mean that the parametrization (1) is very poorly conditioned for homography matrices close to $SO(3)$. This is another fundamental reason why it is preferable to do both image based visual servo control and temporal smoothing directly on the homography group rather than extracting structure variables explicitly. △

The Lie-algebra sl(3) for SL(3) is the set of matrices with trace equal to zero: $sl(3) = \{X \in \mathbb{R}^{3 \times 3} | \text{tr}(X) = 0\}$.

The adjoint operator is a mapping $Ad: SL(3) \times sl(3) \to sl(3)$ defined by

$$\mathrm{Ad}_H X = HXH^{-1}, \quad H \in SL(3), X \in sl(3).$$

For any two matrices A, B ∈ R$^{3 \times 3}$ the Euclidean matrix inner product and Frobenius norm are defined as

$$\langle\langle A, B \rangle\rangle = \text{tr}(A^T B), \quad \|A\| = \sqrt{\langle\langle A, A \rangle\rangle}$$

Let P denote the unique orthogonal projection of R$^{3 \times 3}$ onto sl(3) with respect to the inner product $\langle\langle \cdot, \cdot \rangle\rangle$

$$\mathbb{P}(H) := \left( H - \frac{\text{tr}(H)}{3} I \right) \in \mathfrak{sl}(3). \quad (2)$$

The projection onto the complementary subspace (the span of I in R$^{3 \times 3}$) is defined by

$$\mathbb{P}^{\perp}(H) := H - \mathbb{P}(H) = \frac{\text{tr}(H)}{3} I. \quad (3)$$

Clearly one has $\langle\langle \mathbb{P}(H), \mathbb{P}^{\perp}(H) \rangle\rangle = 0$

### III. NONLINEAR OBSERVER ON SL(3)

Consider the left invariant kinematics defined on SL(3)

$$\dot{H} = HA \quad (4)$$

where $H \in SL(3)$ and $A \in sl(3)$. A general framework for nonlinear filtering on the Special Linear group is introduced. The theory is developed for the case where A is assumed to be unknown and constant. The goal is to provide a set of dynamics for an estimate $\hat{H}(t) \in SL(3)$ of $H(t)$ and an estimate $\hat{A}(t) \in sl(3)$ of A to drive the estimation error $\tilde{H} = \hat{H}^{-1}H$ to the identity matrix I, and the estimation error $\tilde{A} = A - \hat{A}$ to zero.

The estimator filter equation of $\hat{H}$ is posed directly on SL(3). It includes a correction term derived from the error $\tilde{H}$. We consider an estimator filter of the form

$$\begin{cases} \dot{\hat{H}} = \hat{H} (\mathrm{Ad}_{\hat{H}} \hat{A} + \alpha(\hat{H}, H)), & \hat{H}(0) = \hat{H}_0, \\ \dot{\hat{A}} = \beta(\hat{H}, H), & \hat{A}(0) = \hat{A}_0. \end{cases} \quad (5)$$

This yields the following expression for the dynamics of the estimation error $(\hat{H}, \hat{A}) = (\hat{H}^{-1}H, A - \hat{A})$:

$$\left\{ \begin{array}{l} \dot{\tilde{H}} = \tilde{H} (\tilde{A} - \mathrm{Ad}_{\tilde{H}^{-1}} \alpha) \\ \dot{\tilde{A}} = -\beta \end{array} \right. \quad (6)$$

with the arguments of $\alpha$ and $\beta$ omitted to lighten the notation. The main result of the paper is stated next.

**Theorem 3.1:** Assume that the matrix A in (4) is constant. Consider the nonlinear estimator filter (5) along with the innovation $\alpha$ and the estimation dynamics $\beta$ defined as

$$\left\{ \begin{array}{l} \alpha = -k_H \mathrm{Ad}_{\tilde{H}} \mathbb{P}(\tilde{H}^T(I - \tilde{H})), \quad k_H > 0 \\ \beta = -k_A \mathbb{P}(\tilde{H}^T(I - \tilde{H})), \quad k_A > 0 \end{array} \right. \quad (7)$$

with the projection operator $\mathbb{P}: \mathbb{R}^{3 \times 3} \to sl(3)$ defined by (2). Then, for the estimation error dynamics (6),
i) All solutions converge to $E = E_s \cup E_u$ with:

$$ 
\begin{aligned}
E_s &= (I, 0) \\
E_u &= \{(\tilde{H}_0, 0) : \tilde{H}_0 = \lambda(I + (\lambda^{-3} - 1)vv^\top, v \in \mathbb{S}^2\}
\end{aligned}
$$

where $\lambda < 0$ is the unique real solution of the equation $\lambda^3 - \lambda^2 + 1 = 0$.

ii) The equilibrium point $E_s = (I, 0)$ is locally exponentially stable.

iii) Any point of $E_u$ is an unstable equilibrium. More precisely, for any $(\tilde{H}_0, 0) \in E_u$ and any neighborhood $\mathcal{U}$ of $(\tilde{H}_0, 0)$, there exists $(\tilde{H}_1, \tilde{A}_1) \in \mathcal{U}$ such that the solution of System (6) issued from $(\tilde{H}_1, \tilde{A}_1)$ converges to $E_s$.

**Proof of Theorem 3.1:**

*Proof of Part i)* Let us consider the following candidate Lyapunov function

$$ 
\begin{aligned}
V(\tilde{H}, \tilde{A}) &= \frac{1}{2}\|I - \tilde{H}\|^2 + \frac{1}{2k_A}\|\tilde{A}\|^2 \\
&= \frac{1}{2}\text{tr}((I - \tilde{H})^T(I - \tilde{H})) + \frac{1}{2k_A}\text{tr}(\tilde{A}^T\tilde{A}),
\end{aligned}
\quad (8) $$

The derivative of $V$ along the solutions of System (6) is

$$ 
\begin{aligned}
\dot{V} &= -\text{tr}((I - \tilde{H})^T \dot{\tilde{H}}) + \frac{1}{k_A}\text{tr}(\tilde{A}^T \dot{\tilde{A}}) \\
&= -\text{tr}((I - \tilde{H})^T \tilde{H} \tilde{A} - (I - \tilde{H})^T \tilde{H} \text{Ad}_{\tilde{H}^{-1}} \alpha) \\
&\quad - \frac{1}{k_A}\text{tr}(\tilde{A}^T \beta)
\end{aligned}
$$

Knowing that for any matrices $G \in SL(3)$ and $B \in sl(3)$, $\text{tr}(B^TG) = \text{tr}(B^T\mathbb{P}(G)) = \langle\langle B, \mathbb{P}(G)\rangle\rangle$, one obtains:

$$ 
\begin{aligned}
\dot{V} &= \langle\langle \mathbb{P}(\tilde{H}^T(I - \tilde{H})), \text{Ad}_{\tilde{H}^{-1}} \alpha \rangle\rangle \\
&\quad - \langle\langle \tilde{A}, \mathbb{P}(\tilde{H}^T(I - \tilde{H})) + \frac{1}{k_A} \beta \rangle\rangle \quad (9)
\end{aligned}
$$

Introducing the expressions of $\alpha$ and $\beta$ (Eq. (7)) in the above equation yields

$$ \dot{V} = -k_H \|\mathbb{P}(\tilde{H}^T(I - \tilde{H}))\|^2 \quad (10) $$

The derivative of the Lyapunov function is negative semi-definite, and equal to zero when $\mathbb{P}(\tilde{H}^T(I - \tilde{H}) = 0$. The dynamics of the estimation error is autonomous, i.e. it is given by

$$ 
\left\{ 
\begin{array}{l}
\dot{\tilde{H}} = \tilde{H} (\tilde{A} + k_H \mathbb{P}(\tilde{H}^T(I - \tilde{H}))) \\
\dot{\tilde{A}} = k_A \mathbb{P}(\tilde{H}^T(I - \tilde{H}))
\end{array}
\right.
\quad (11) $$

Therefore, we deduce from LaSalle's theorem that all solutions of this system converge to the largest invariant set contained in $\{(\tilde{H}, \tilde{A}) : \mathbb{P}(\tilde{H}^T(I - \tilde{H})) = 0\}$.

We now prove that, for System (11), the largest invariant set $E$ contained in $\{(\tilde{H}, \tilde{A})|\mathbb{P}(\tilde{H}^T(I - \tilde{H})) = 0\}$ is equal to $E_s \cup E_u$.

We need to show that the solutions of System (11) belonging to $\{(\tilde{H}, \tilde{A})|\mathbb{P}(\tilde{H}^T(I - \tilde{H})) = 0\}$ for all $t$ consist of all fixed points of $E_s \cup E_u$. Note that $E_s = (I, 0)$ is clearly contained in $E$. Let us thus consider such a solution $(\tilde{H}(t), \tilde{A}(t))$. First, we deduce from (11) that $\dot{\tilde{A}}(t)$

is identically zero since $\mathbb{P}(\tilde{H}^T(t)(I - \tilde{H}(t)))$ is identically zero on the invariant set $E$ and therefore $\tilde{A}$ is constant. We also deduce from (11) that $\tilde{H}$ is solution to the equation $\dot{\tilde{H}} = \tilde{H}\tilde{A}$. Note that at this point one cannot infer that $\tilde{H}$ is constant. Still, we omit from now on the possible time-dependence of $\tilde{H}$ to lighten the notation.

Since $\mathbb{P}(\tilde{H}^T(I - \tilde{H})) = 0$, we have that

$$ \tilde{H}^\top(I - \tilde{H}) = \frac{1}{3}\text{trace}(\tilde{H}^\top(I - \tilde{H}))I \quad (12) $$

which means that $\tilde{H}$ is a symmetric matrix. Therefore, it can be decomposed as:

$$ \tilde{H} = UDU^\top \quad (13) $$

where $U \in SO(3)$ and $D = \text{diag}(\lambda_1, \lambda_2, \lambda_3) \in SL(3)$ is a diagonal matrix which contains the three real eigenvalues of $\tilde{H}$. Without loss of generality let us suppose that the eigenvalues are in increasing order: $\lambda_1 \le \lambda_2 \le \lambda_3$. Plugging equation (13) into equation (12), one obtains:

$$ D(I - D) = \frac{1}{3}\text{trace}(D(I - D))I $$

Knowing that $\det(D) = 1$, the $\lambda_i$'s satisfy the following equations:

$$ \lambda_1(1 - \lambda_1) = \lambda_2(1 - \lambda_2) \quad (14) $$

$$ \lambda_2(1 - \lambda_2) = \lambda_3(1 - \lambda_3) \quad (15) $$

$$ \lambda_3 = 1/(\lambda_1\lambda_2) \quad (16) $$

which can also be written as follows:

$$ \lambda_1 - \lambda_2 = (\lambda_1 - \lambda_2)(\lambda_1 + \lambda_2) \quad (17) $$

$$ \lambda_1 - \lambda_3 = (\lambda_1 - \lambda_3)(\lambda_1 + \lambda_3) \quad (18) $$

$$ \lambda_3 = 1/(\lambda_1\lambda_2) \quad (19) $$

First of all, let us remark that if $\lambda_1 = \lambda_2 = \lambda_3$ then $\lambda_1 = \lambda_2 = \lambda_3 = 1$. This solution is associated with the equilibrium point $E_s = (I, 0)$.

If $\lambda_1 = \lambda_2 < \lambda_3$ then:

$$ 1 = \lambda_2 + \lambda_3 \quad (20) $$

$$ \lambda_3 = 1/(\lambda_2^2) \quad (21) $$

where $\lambda_2 \in (-1, 0)$ is the unique real solution of the equation $\lambda_2^3 - \lambda_2^2 + 1 = 0$. This solution is associated with the equilibrium set $E_u$.

If $\lambda_1 < \lambda_2 = \lambda_3$ then:

$$ 1 = \lambda_1 + \lambda_2 \quad (22) $$

$$ \lambda_1 = 1/\lambda_2^2 \quad (23) $$

so that $\lambda_2$ is also solution of the equation $\lambda_2^3 - \lambda_2^2 + 1 = 0$. But this is impossible since we supposed $\lambda_1 < \lambda_2$ and the solution of the equation is such that $-1 < \lambda_2 < 0$ and $0 < \lambda_1 = 1/\lambda_2^2 < 1$.

If $\lambda_1 \ne \lambda_2 \ne \lambda_3$, then:

$$ 1 = \lambda_1 + \lambda_2 \quad (24) $$

$$ 1 = \lambda_1 + \lambda_3 \quad (25) $$

$$ \lambda_3 = 1/(\lambda_1\lambda_2) \quad (26) $$
which means that $\lambda_2 = \lambda_3$. This is in contradiction with our initial hypothesis.

In conclusion, $\tilde{H}$ has two equal negative eigenvalues $\lambda_1 = \lambda_2 = \lambda < 0$ ($\lambda$ is the unique real solution of the equation $\lambda^3 - \lambda^2 + 1 = 0$) and the third one is $\lambda_3 = 1/\lambda^2$. Writing the diagonal matrix D as follows:

$$D = \lambda(I + (\lambda^{-3} - 1)e_3 e_3^{\top})$$

and plugging this equation into equation (13), the homography for the second solution ($\lambda_1 = \lambda_2$) can be expressed as follows:

$$\tilde{H} = \lambda(I + (\lambda^{-3} - 1)(Ue_3)(Ue_3)^{\top})$$

Setting $v = Ue_3$, we finally find that $\tilde{H}$ must have the following form:

$$\tilde{H} = \lambda(I + (\lambda^{-3} - 1)vv^{\top})$$

where $v$ is a unitary vector: $\|v\| = 1$ and $\lambda$ is the unique real constant value that verifies the equation $\lambda^3 - \lambda^2 + 1 = 0$.

It remains to show that $\tilde{A} = 0$. The inverse of $\tilde{H}$ is

$$\tilde{H}^{-1} = \lambda^{-1}(I + (\lambda^3 - 1)vv^{\top})$$

The derivative of $\tilde{H}$ is

$$\dot{\tilde{H}} = \lambda(\lambda^{-3} - 1)(\dot{v}v^{\top} + v\dot{v}^{\top})$$

so that

$$\tilde{A} = \tilde{H}^{-1} \dot{\tilde{H}} = (\lambda^{-3} - 1)(I + (\lambda^3 - 1)vv^{\top})(\dot{v}v^{\top} + v\dot{v}^{\top})$$

Knowing that $v^{\top}\dot{v} = 0$, this equations becomes:

$$\tilde{A} = (\lambda^{-3} - 1)(\dot{v}v^{\top} + v\dot{v}^{\top} + (\lambda^3 - 1)v\dot{v}^{\top})$$

and knowing that $\lambda^3 = \lambda^2 - 1$, we obtain:

$$\tilde{A} = (\lambda^{-3} - 1)(\dot{v}v^{\top} + v\dot{v}^{\top} + (\lambda^2 - 2)v\dot{v}^{\top}) \quad (27)$$

$$= (\lambda^{-3} - 1)(\dot{v}v^{\top} - v\dot{v}^{\top} + \lambda^2 v\dot{v}^{\top}) \quad (28)$$

Since $\dot{v}v^{\top} - v\dot{v}^{\top} = [[v]_{\times}\dot{v}]_{\times}$, we finally obtain

$$\tilde{A} = (\lambda^{-3} - 1)([[v]_{\times}\dot{v}]_{\times} + \lambda^2 v\dot{v}^{\top})$$

Since $[[v]_{\times}\dot{v}]_{\times}$ is a skew-symmetric matrix, the diagonal elements of $\tilde{A}$ are $a_{ii} = (\lambda^{-3} - 1)\lambda^2 v_i \dot{v}_i$. Knowing that each $a_{ii}$ is constant we have two possible cases. The first one is $a_{ii} = 0$ for each $i$. Then $v$ is constant so that $\tilde{H}$ is also constant and $\tilde{A} = 0$. If there exists $i$ such that $a_{ii} \neq 0$, then there exists $i$ such that $a_{ii} < 0$. This is due to the fact that $\tilde{A} \in sl(3)$ and therefore $\sum_i a_{ii} = 0$. In this case, the corresponding $v_i$ diverges to infinity because $v_i \dot{v}_i$ is a strictly positive constant. This contradicts the fact that $\|v\| = 1$. This concludes the proof of Part i) of the theorem.

*Proof of Part ii)*: We compute the linearization of System (11) at $E_s = (I, 0)$. Let us define $X_1$ and $X_2$ as elements of sl(3) corresponding to the first order approximations of $\tilde{H}$ and $\tilde{A}$ around $(I, 0)$:

$$\tilde{H} \approx (I + X_1), \quad \tilde{A} \approx X_2$$

Substituting these approximations into (11) and discarding all terms quadratic or higher order in $(X_1, X_2)$ yields

$$\begin{pmatrix} \dot{X}_1 \\ \dot{X}_2 \end{pmatrix} = \begin{pmatrix} -k_H I_3 & I_3 \\ -k_A I_3 & 0 \end{pmatrix} \begin{pmatrix} X_1 \\ X_2 \end{pmatrix} \quad (29)$$

Since $k_H, k_A > 0$, the linearized error system is exponentially stable. This proves the local exponential stability of the equilibrium $(I, 0)$.

*Proof of Part iii)*: First, we remark that the function $V$ is constant and strictly positive on the set $E_u$. This can be easily verified from (8) and the definition of $E_u$, using the fact that on this set $\tilde{A} = 0$, $\tilde{H}^T \tilde{H} = \tilde{H}^2 = \lambda^2 I + (\frac{1}{\lambda^2} - \lambda)vv^T$, and $\text{tr}(vv^T) = 1$ since $\|v\| = 1$. We denote by $V_u$ the value of $V$ on $E_u$. The fact that $V_u$ is strictly positive readily implies (in accordance with Part ii)) that $E_s$ is an asymptotically stable equilibrium, since $V$ is non-increasing along the system's solutions, and each of them converges to $E_s \cup E_u$. Using the same arguments, the proof of Part iii) reduces to showing that for any point $(\tilde{H}_0, 0) \in E_u$, and any neighborhood $\mathcal{U}$ of this point, one can find $(\tilde{H}_1, \tilde{A}_1) \in \mathcal{U}$ such that

$$V(\tilde{H}_1, \tilde{A}_1) < V_u \quad (30)$$

Let $\tilde{H}(\cdot)$ denote a smooth curve on SL(3), solution of $\dot{\tilde{H}} = \tilde{H}C$ with $C$ a constant element of sl(3) that will be specified latter on. We also assume that $(\tilde{H}(0), 0) \in E_u$. Let $f(t) = \|I - \tilde{H}(t)\|^2/2$ so that, by (8), $f(0) = V_u$. The first derivative of $f$ is given by

$$\begin{aligned} \dot{f}(t) &= -\text{tr}((I - \tilde{H}(t))^T \dot{\tilde{H}}(t)) \\ &= -\text{tr}((I - \tilde{H}(t))^T \tilde{H}(t)C) \\ &= -\langle \mathbb{P}(\tilde{H}^T(t)(I - \tilde{H}(t))), C \rangle \end{aligned}$$

For all elements $(\tilde{H}_0, 0) \in E_u$, one has $\mathbb{P}(\tilde{H}_0^T(I - \tilde{H}_0)) = 0$, so that $\dot{f}(0) = 0$. We now calculate the second order derivative of $f$:

$$\begin{aligned} \ddot{f}(t) &= \text{tr}(\dot{\tilde{H}}(t)^T \dot{\tilde{H}}(t)) - \text{tr}((I - \tilde{H}(t))^T \ddot{\tilde{H}}(t)) \\ &= \text{tr}(\dot{\tilde{H}}(t)^T \dot{\tilde{H}}(t)) - \text{tr}((I - \tilde{H}(t))^T \dot{\tilde{H}}(t)C) \end{aligned}$$

where we have used the fact that $C$ is constant. Evaluating the above expression at $t = 0$ and replacing $\dot{\tilde{H}}(0)$ by its value $\tilde{H}(0)C$ yields

$$\ddot{f}(0) = \|\tilde{H}(0)C\|^2 - \text{tr}((I - \tilde{H}(0))^T \tilde{H}(0)C^2) \quad (31)$$

When $(\tilde{H}_0, 0) \in E_u$, one has

$$\tilde{H}_0^2 = \lambda^2 I + (\frac{1}{\lambda^2} - \lambda)vv^T = \tilde{H}_0 + (\lambda^2 - \lambda)I$$

Therefore, we deduce from (31) that

$$\ddot{f}(0) = \|\tilde{H}(0)C\|^2 + \lambda(\lambda - 1)\text{tr}(C^2) \quad (32)$$

Since $(\tilde{H}(0), 0) \in E_u$, there exists $v \in \mathbb{S}^2$ such that $\tilde{H}(0) = \lambda I + (\frac{1}{\lambda^2} - \lambda)vv^T$. From this expression and using the fact that $\lambda^3 - \lambda^2 + 1 = 0$, one verifies that

$$\|\tilde{H}(0)C\|^2 = \lambda^2 \|C\|^2 + (\frac{1}{\lambda^2} - \lambda)\text{tr}(C^Tvv^T C) \quad (33)$$
Now let us set $C = [v]_\times$ with $[v]_\times$ the skew-symmetric matrix associated with the cross-product by $v$, i.e. $[v]_\times y = v \times y \forall y$. Clearly, $C \in \mathfrak{sl}(3)$. Then, it follows from (32) and (33) that

$$ 
\begin{aligned}
\ddot{f}(0) &= \lambda^2 \|C\|^2 + \lambda(\lambda - 1)\mathrm{tr}(C^2) \\
&= \lambda^2 \mathrm{tr}(v_\times^T v_\times) + \lambda(\lambda - 1)\mathrm{tr}((v_\times)^2) \\
&= -\lambda^2 \mathrm{tr}((v_\times)^2) + \lambda(\lambda - 1)\mathrm{tr}((v_\times)^2) \\
&= -\lambda \mathrm{tr}((v_\times)^2) = 2\lambda \|v\|^2 = 2\lambda < 0
\end{aligned}
$$

Therefore, there exists $t_1 > 0$ such that for any $t \in (0, t_1)$,

$$ 
\begin{aligned}
f(t) &\approx f(0) + t\dot{f}(0) + t^2/2\ddot{f}(0) \\
&\approx V_u + t^2/2\ddot{f}(0) < V_u
\end{aligned}
$$

Eq. (30) follows by setting $(\tilde{H}_1, A_1) = (\tilde{H}(t), 0)$ with $t \in (0, t_1)$ chosen small enough so as to have $(\tilde{H}(t), 0) \in \mathcal{U}$. This concludes the proof of Part iii) and the proof of the theorem.

## IV. SIMULATIONS WITH GROUND TRUTH

We validated the proposed observer with several simulations. In this section, we illustrate and discuss three examples. We use the known ground truth to assess the quality of the homography and velocity estimations.

In order to simulate a real experiment, we build a sequence of reference homographies, starting from an initial homography $H_0 \in SL(3)$. The reference set of homographies was built using the following formula:

$$ H_{k+1} = H_k \exp(A\Delta t + Q_k\Delta t) $$

where $A \in \mathfrak{sl}(3)$ is a constant velocity, $Q_k \in \mathfrak{sl}(3)$ is a random matrix with Gaussian distribution, and $\Delta t$ is the sampling time (in the simulation we set the variance to $\sigma = 0.1$). By building the homographies in this way, we guarantee that the measured $H_k \in SL(3)$, $\forall k$.

We implemented a discretized observer in order to process the data. In all examples the gains of the observer were set to $k_H = 2$ and $k_A = 1$.

In the figures, we show a $(3 \times 3)$ table of plots. Each plot represents an element of a $(3 \times 3)$ matrix.

In this simulation the initial “error” for the homography is chosen at random and it is very large. The initial velocity estimate $\hat{A}_0$ is set to zero. Figure 1 shows that, after a fast transient, the estimated homography converges towards the measured homography. Figure 2 shows that the estimated velocity also converges towards the true one.

## V. EXPERIMENTS WITH REAL DATA

In this section, we present results obtained with real data. In the first image the user selects a rectangular area of interest. The homographies that transform the area of interest in the current image are measured using the ESM visual tracking software<fnref n="1" /> [1]. Figure 3 shows four images extracted from the sequence Corkes. The first image in the figure shows a red rectangle containing the area of interest that must be tracked in all the images of the video sequence.

<figure><img src="image_1.png" /><figcaption>Fig. 1. Red line: the measured homography matrix $\tilde{H}$. Blue line: the observed homography $\hat{H}$.</figcaption></figure>

<figure><img src="image_2.png" /><figcaption>Fig. 2. Red line: the true homography velocity $A$. Blue line the observed homography velocity $\hat{A}$.</figcaption></figure>

For each image of the sequence, the output of the ESM visual tracking algorithm is the homography that encodes the transformation of each pixels of the rectangular area from the current to the first image.

The measured homographies are the input of the proposed nonlinear observer. In this experiment the gains were $k_H = 5$ and $k_A = 1$. The filtering effect of the observer on the estimated homography are visible in Figure 4.

In this experiment with real data, the velocity $A$ is unknown and not constant. Nevertheless, when the velocity varies slowly the observer is able to give us an approximation of the velocity.

<footnote n="1">¹Available for download at http://esm.gforge.inria.fr</footnote>
<figure><img src="image_3.png" /><figcaption>Fig. 3. Images from the Corkes sequence. The red quadrilateral represents the tracked area. The visual tracking is correctly performed in real-time. However, the noise in the images and modeling errors affect the accuracy of the measured homographies.</figcaption></figure>

<figure><img src="image_4.png" /><figcaption>Fig. 4. Corkes sequence. Left: two elements of the (3×3) homography matrix; in red the measured homography matrix $\bar{H}$, in blue the observed homography $\tilde{H}$. Right: two corresponding elements of the observed homography velocity matrix $\dot{A}$.</figcaption></figure>

## VI. CONCLUSION

In this paper, we proposed an observer for the homographies defined on SL(3) and their velocities defined on sl(3). We proved that the observer is almost globally stable. We also proved that isolated critical points exist but that they are far from the equilibrium point and unstable. We performed several simulations with ground truth to validate the theoretical results. Experiments with real data show that the observer performs well even when the constant velocity assumption does not hold. Future work will be dedicated to the application of such observer to improve vision based control.

**Acknowledgments:** The authors gratefully acknowledge the contribution of INRIA, CNRS and ANU. Part of this work was supported by the PEGASE EU project.

## REFERENCES

[1] S. Benhimane and E. Malis. Homography-based 2d visual tracking and servoing. *International Journal of Robotic Research*, 26(7): 661–676, 2007.

[2] S. Benhimane, E. Malis, P. Rives, and J. R. Azinheira. Vision-based control for car platooning using homography decomposition. *IEEE Conf. on Robotics and Automation*, pages 2173–2178, 2005.

[3] S. Bonnabel, P. Martin, and P. Rouchon. Symmetry-preserving observers. *IEEE Trans. on Automatic Control*, 53 (11): 2514–2526, 2008.

[4] S. Bonnabel, P. Martin, and P. Rouchon. Non-linear observer on lie groups for left-invariant dynamics with right-left equivariant output. *IFAC World Congress*, pages 8594–8598, 2008.

[5] F. Chaumette and S. Hutchinson. Visual servo control, part ii: Advanced approaches. *IEEE Robotics and Automation Magazine*, 14(1): 109–118, 2007.

[6] K. Deguchi. Optimal motion control for image-based visual servoing by decoupling translation and rotation. *IEEE/RSJ Conf. on Intelligent Robots and Systems*, pages 705–711, 1998.

[7] Y. Fang, W. Dixon, D. Dawson, and P. Chawda. Homography-based visual servoing of wheeled mobile robots. *IEEE Trans. on Systems, Man, and Cybernetics - Part B*, 35(5): 1041–1050, 2005.

[8] O. Faugeras and F. Lustman. Motion and structure from motion in a piecewise planar environment. *International Journal of Pattern Recognition and Artificial Intelligence*, 2(3): 485–508, 1988.

[9] T. Hamel and R. Mahony. Attitude estimation on SO(3) based on direct inertial measurements. *IEEE Conf. on Robotics and Automation*, pages 2170–2175, 2006.

[10] Hashimoto, editor. *Visual Servoing: Real Time Control of Robot manipulators based on visual sensory feedback*, volume 7 of World Scientific Series in Robotics and Automated Systems. World Scientific Press, 1993.

[11] S. Hutchinson, G. D. Hager, and P. I. Corke. A tutorial on visual servo control. *IEEE Trans. on Robotics and Automation*, 12(5): 651–670, 1996.

[12] C. Lageman, R. Mahony, and J. Trumpf. State observers for invariant dynamics on a lie group. *Conf. on the Mathematical Theory of Networks and Systems*, 2008.

[13] C. Lageman, J. Trumpf, and R. Mahony. Gradient-like observers for invariant dynamics on a lie group. *IEEE Trans. on Automatic Control*, to appear.

[14] R. Mahony, T. Hamel, and J.-M. Pflimlin. Non-linear complementary filters on the special orthogonal group. *IEEE Trans. on Automatic Control*, 53(5): 1203–1218, 2008.

[15] E. Malis and F. Chaumette. Theoretical improvements in the stability analysis of a new class of model-free visual servoing methods. *IEEE Trans. on Robotics and Automation*, 18(2): 176–186, 2002.

[16] E. Malis, F. Chaumette, and S. Boudet. 2 1/2 d visual servoing. *IEEE Trans. on Robotics and Automation*, 15(2):234–246, 1999.

[17] E. Malis and M. Vargas. Deeper understanding of the homography decomposition for vision-based control. Research Report 6303, INRIA, 2007.

[18] P. Martin and E. Salaün. Invariant observers for attitude and heading estimation from low-cost inertial and magnetic sensors. *IEEE Conf. on Decision and Control*, pages 1039–1045, 2007.

[19] P. Martin and E. Salaün. An invariant observer for earth-velocity-aided attitude heading reference systems. *IFAC World Congress*, pages 9857–9864, 2008.

[20] C. Samson, M. Le Borgne, and B. Espiau. Robot Control: the Task Function Approach, volume 22 of Oxford Engineering Science Series. Clarendon Press, 1991.

[21] J. Thielen and R. M. Sanner. A coupled nonlinear spacecraft attitude controller and observer with an unknow constant gyro bias and gyro noise. *IEEE Trans. on Automatic Control*, 48(11): 2011–2015, 2003.

[22] W. J. Wilson, C. C. W. Hulls, and G. S. Bell. Relative end-effector control using cartesian position-based visual servoing. *IEEE Trans. on Robotics and Automation*, 12(5): 684–696, 1996.