Minh-Duc Hua
email: hua@i3s.unice.fr

Jochen Trumpf
email: jochen.trumpf@anu.edu.au

Tarek Hamel
email: thamel@i3s.unice.fr

Robert Mahony

Pascal Morin

M.-D Hua

Feature-based Recursive Observer Design for Homography Estimation and its Application to Image Stabilization

Keywords: Homography estimation, Nonlinear observer, Mestimator-like observer, Image stabilization

This paper presents a new algorithm for online estimation of a sequence of homographies applicable to image sequences obtained from robotic vehicles equipped with vision sensors. The approach taken exploits the underlying Special Linear group structure of the set of homographies along with gyroscope measurements and direct point-feature correspondences between images to develop temporal filter for the homography estimate. Theoretical analysis and experimental results are provided to demonstrate the robustness of the proposed algorithm. The experimental results show excellent performance and robustness even in the case of very fast camera motions (relative to frame rate) and severe occlusions.

I. INTRODUCTION

When a robotic vehicle equipped with a vision sensor is manoeuvering in a built environment, consisting primarily of planar or near planar surfaces, then the nature of the environment can be exploited in the vision processing algorithms. Different images of the same planar surface are related by homography mappings, and homographies have been used extensively in robotic applications as a vision primitive. Homography based algorithms have been used for estimation of the rigid-body pose of a vehicle equipped with a camera [START_REF] Mufti | Super resolution of speed signs in video sequences[END_REF], [START_REF] Saurer | Homography based egomotion estimation with a common direction[END_REF], [START_REF] Scaramuzza | Appearance-guided monocular omnidirectional visual odometry for outdoor ground vehicles[END_REF], [START_REF] Wang | Visual odometry based on locally planar ground assumption[END_REF]. Navigation of robotic vehicles has been developed based on homography sequences [START_REF] De Plinval | Visual servoing for underactuated vtol uavs: a linear homography-based approach[END_REF], [START_REF] Fang | Homography-based visual servoing of wheeled mobile robots[END_REF], [START_REF] Fraundorfer | Topological mapping, localization and navigation using image collections[END_REF], [START_REF] Krupínski | An inertialaided homography-based visual servo control approach for (almost) fully actuated autonomous underwater vehicles[END_REF], [START_REF] Nguyen | Inertialaided homography-based visual servo control of autonomous underwater vehicles without linear velocity measurements[END_REF] and one of the most successful visual servo control paradigms uses homographies [START_REF] Malis | 2-1/2-d visual servoing[END_REF], [START_REF] Malis | Dynamic estimation of homography transformations on the special linear group for visual servo control[END_REF]. Homography based methods are particularly well suited to navigation of unmanned aerial vehicles [START_REF] Al-Kaff | Homography-based navigation system for unmanned aerial vehicles[END_REF], [START_REF] Caballero | Homography based kalman filter for mosaic building. applications to uav position estimation[END_REF], [START_REF] De Plinval | Stabilization of a class of underactuated vehicles with uncertain position measurements and application to visual servoing[END_REF], [START_REF] Mondragón | 3d pose estimation based on planar object tracking for uavs control[END_REF], [START_REF] Ruiz | MGRAPH: A multigraph homography method to generate incremental mosaics in real-time from UAV swarms[END_REF] where the ground terrain is viewed from a distance and the relief of surface features is negligible compared to the vehicles distance from the scene.

Computing homographies from point correspondences has been extensively studied in the last fifteen years [START_REF] Hartley | Multiple View Geomerty in Computer Vision[END_REF]. The quality of the homography estimate obtained depends heavily on the number and quality of the data points used in the estimation as well as the algorithm employed. For a well pascal.morin@sorbonne-universite.fr textured scene, the state-of-the-art methods can provide high quality homography estimates at the cost of significant computational effort (see [START_REF] Mei | Efficient homographybased tracking and 3-d reconstruction for single-viewpoint sensors[END_REF] and references therein). For a scene with poor texture and consequently few reliable point-feature correspondences, existing homography estimation algorithms perform poorly. Robotic vehicle applications, however, provide temporal sequences of images and it seems natural to exploit the temporal correlation rather than try to compute individual raw homographies for each pair of frames. In [START_REF] Zhang | Self-recalibration of a structured light system via plane-based homography[END_REF] image flow computed from a pair of images is used to compute the relative homography, although this method still only considers isolated pairs of images. In recent work by the authors [START_REF] Malis | Dynamic estimation of homography transformations on the special linear group for visual servo control[END_REF], [START_REF] Mahony | Nonlinear complementary filters on the special linear group[END_REF] a nonlinear observer [START_REF] Bonnabel | Non-linear symmetrypreserving observers on lie groups[END_REF], [START_REF] Lageman | the excellent performance and robustness of the proposed observer despite severe occlusions (e.g. frames 482, 844, 964, 975) and fast rotational motions (e.g. frames 114, 200, 482). invariant dynamics on a lie group[END_REF] for homography estimation was proposed based on the group structure of the set of homographies, the Special Linear group SL(3) [START_REF] Benhimane | Homography-based 2d visual tracking and servoing[END_REF]. This observer uses velocity information to interpolate across a sequence of images and improve the individual homography estimates. The observer proposed in [START_REF] Malis | Dynamic estimation of homography transformations on the special linear group for visual servo control[END_REF], [START_REF] Mahony | Nonlinear complementary filters on the special linear group[END_REF] still requires individual image homographies to be algebraically computed for each image, which are then smoothed using filtering techniques. Although this approach [START_REF] Malis | Dynamic estimation of homography transformations on the special linear group for visual servo control[END_REF], [START_REF] Mahony | Nonlinear complementary filters on the special linear group[END_REF] provides improved homography estimates, it comes at the cost of running both a classical homography algorithm as well as the temporal filter algorithm, and only functions if each pair of images has sufficient data available to compute a raw homography.

In this paper the question of deriving an observer for a sequence of image homographies that takes image pointfeature correspondences directly as input is considered. The proposed approach takes a sequence of images associated with a continuous variation of the reference image, the most common case being a moving camera viewing a planar scene, a situation typical of robotic applications. The proposed nonlinear observer is posed on the Special Linear group SL [START_REF] Bonnabel | Non-linear symmetrypreserving observers on lie groups[END_REF], that is in one-to-one correspondence with the group of homographies [START_REF] Benhimane | Homography-based 2d visual tracking and servoing[END_REF], and uses velocity measurements to propagate the homography estimate and fuse this with new data as it becomes available [START_REF] Malis | Dynamic estimation of homography transformations on the special linear group for visual servo control[END_REF], [START_REF] Mahony | Nonlinear complementary filters on the special linear group[END_REF]. A key advance on prior work by the authors is the formulation of a point feature innovation for the observer that incorporates point correspondences directly in the observer without requiring reconstruction of individual image homographies. This saves considerable computational resources and makes the proposed algorithm suitable for embedded systems with simple point detection and matching software. In addition, the algorithm is well posed even when there is insufficient data for full reconstruction of a homography. For example, if the number of corresponding points between two images drops below four it is impossible to algebraically reconstruct an image homography and the existing algorithms fail [START_REF] Hartley | Multiple View Geomerty in Computer Vision[END_REF]. In such situations, the proposed observer will continue to operate by incorporating available information and relying on propagation of prior estimates. Finally, even if a homography can be reconstructed from a small set of feature correspondences, the estimate is often unreliable and the associated error is difficult to characterize. The proposed algorithm integrates information from a sequence of images, and noise in the individual feature correspondences is filtered through the natural low-pass response of the observer, resulting in a highly robust estimate. As a result, the authors believe that the proposed observer is ideally suited for homography estimation based on small windows of image data associated with specific planar objects in a scene, poorly textured scenes, and real-time implementation; all of which are characteristic of requirements for homography estimation in robotic vehicle applications.

The paper is organized into seven sections including the introduction and the concluding sections. Section II presents a brief recap of the Lie group structure of the set of homographies. In Section III, based on a recent advanced theory for nonlinear observer design [START_REF] Mahony | Observers for kinematic systems with symmetry[END_REF], a nonlinear observer on SL(3) is proposed using direct measurements of known inertial directions and the knowledge of the group velocity. A rigourous stability analyses is provided in this section. In Section IV, the homography and associated homography velocity are related to rigid-body motion of the camera and two observers are derived for the case where only the angular velocity of the camera is known, a typical scenario in robotic applications. Simulation results are provided in Section V. Section VI provides an application of the proposed approach to a real world problem of image stabilization. In this section, offline and real-time experimental validations are presented together with some useful practical implementation aspects. Four video links, showing the results of Sections VI, are also provided as supplementary material. The results show excellent performance and robustness even in the case of very fast camera motions (relative to frame rate) and severe occlusions.

A preliminary version of the theoretical results in this paper was presented at a conference [START_REF] Hamel | Homography estimation on the special linear group based on direct point correspondence[END_REF]. The present version provides a more formal derivation of the observer based on a recent theory [START_REF] Mahony | Observers for kinematic systems with symmetry[END_REF]. Moreover, the M-estimator-like observer design proposed in Subsection IV-C is a new result that improves significantly the robustness of the proposed approach with respect to unavoidable feature correspondence outliers encountered in practice. Finally, extensive experimental validations for the image stabilization application as reported in Section VI have also been recently carried out. Some materials of this paper were reported in our technical reports [START_REF] Hua | Point and line feature-based observer design on SL(3) for Homography estimation and its application to image stabilization[END_REF].

II. PRELIMINARY MATERIAL

A. Camera projections

Visual data is obtained via a projection of an observed scene onto the camera image surface. The projection is parameterized by two sets of parameters: intrinsic ("internal" parameters of the camera such as the focal length, the pixel aspect ratio, the principal point, etc.) and extrinsic (the pose, i.e. the position and orientation of the camera). Let Å (resp. A) denote projective coordinates for the image plane of a camera Å (resp. A), and let { Å} (resp. {A}) denote its (right-hand) frame of reference. Let ξ ∈ R 3 denote the position of the frame {A} with respect to { Å} expressed in { Å}. The orientation of the frame {A} with respect to { Å} is represented by a rotation matrix R ∈ SO [START_REF] Bonnabel | Non-linear symmetrypreserving observers on lie groups[END_REF]. The pose of the camera determines a rigid body transformation from {A} to { Å} (and visa-versa). One has

P = RP + ξ (1)
as a relation between the coordinates of the same point in the reference frame ( P ∈ { Å}) and in the current frame (P ∈ {A}). The camera internal parameters, in the commonly used approximation [START_REF] Hartley | Multiple View Geomerty in Computer Vision[END_REF], define a 3 × 3 matrix K so that one can write 1 :

pim ∼ = K P , p im ∼ = KP, (2)
where p im ∈ A is the image of a point when the camera is aligned with the frame {A}, and can be written as (u, v, 1) using the homogeneous coordinate representation for that 2D image point. Likewise, pim ∈ Å is the image of the same point viewed when the camera is aligned with the frame { Å}.

If the camera is calibrated (i.e. the intrinsic parameters are known), then all quantities can be appropriately scaled so that:

p := P | P | = K -1 pim |K -1 pim | , p := P |P | = K -1 p im |K -1 p im | . (3)

B. Homographies

Since homographies describe image transformations of planar scenes, we begin by fixing a plane that contains the points of interest (target points).

Assumption 1: Assume that the camera is calibrated and the observed planar scene Π contains a set of n target points (n ≥ 4) so that

Π = P ∈ R 3 : η P -d = 0 ,
where η is the unit normal to the plane expressed in { Å} and d is the distance of the plane to the origin of { Å}.

From the rigid-body relationship (1) one deduces P = R P -R ξ. Define ζ := -R ξ. Since all target points lie in a single planar surface, one has

P i = R Pi + ζη d Pi , i = {1, . . . , n}, (4)
and thus, using (3) the projected points obey

p i ∼ = R + ζη d pi , i = {1, . . . , n}. (5)
The projective mapping

H : A → Å, H : ∼ = R + ζη d -1
is termed homography and it relates the images of points on the plane Π when viewed from two poses defined by the coordinate systems A and Å, respectively. It is straightforward to verify that the homography H can be written as follows:

H ∼ = R + ξη d , (6)
where η is the normal to the observed planar scence expressed in the frame {A} and d is the orthogonal distance of the plane to the origin of {A}. One verifies that [START_REF] Benhimane | Homography-based 2d visual tracking and servoing[END_REF]:

η = R η (7) d = d -η ξ = d + η ζ. (8)
The homography matrix encodes the pose information (R, ξ) of the camera from the frame {A} (termed current frame) to the frame { Å} (termed reference frame). However, since the relationship between the image points and the homography is a projective relationship, it is only possible to determine H up to a scale factor (using the image points relationships alone).

C. Homographies as elements of the Special Linear Group SL(3)

Recall that the Special Linear group SL(3) is defined as the set of all real valued 3 × 3 matrices with unit determinant:

SL(3) := {H ∈ R 3×3 | det H = 1}.
The Lie-algebra sl(3) of SL(3) is the set of matrices with trace equal to zero:

sl(3) := {X ∈ R 3×3 | tr(X) = 0}.
The adjoint operator is a mapping Ad : SL(3) × sl(3) → sl(3) defined by:

Ad H X := HXH -1 , H ∈ SL(3), X ∈ sl(3).
Since a homography matrix H is only defined up to scale then any homography matrix is associated with a unique matrix H ∈ SL(3) by re-scaling H = det(H) - [START_REF] Bonnabel | Non-linear symmetrypreserving observers on lie groups[END_REF]. The geometrical meaning of the above property is that the 3D motion of the camera between views A 0 and A 1 , followed by the 3D motion between views A 1 and A 2 is the same as the 3D motion between views A 0 and A 2 . As a consequence, one can think of homographies as described by elements of SL(3). Since any homography is defined up to a scale factor, it is assumed from now on that H ∈ SL(3):

H = γ R + ξη d . (9)
There are numerous approaches for determining H, up to this scale factor, cf. for example [START_REF] Malis | Deeper understanding of the homography decomposition for vision-based control[END_REF]. Note that direct computation of the determinant of H in combination with the expression (8) of d and using the fact that det(H) = 1 shows that γ = (d/ d) 1 3 . Extracting R and ξ/d from H is in general quite complex [START_REF] Benhimane | Homography-based 2d visual tracking and servoing[END_REF], [START_REF] Faugeras | Motion and structure from motion in a piecewise planar environment[END_REF], [START_REF] Malis | Deeper understanding of the homography decomposition for vision-based control[END_REF], [START_REF] Zhang | 3D reconstruction based on homography mapping[END_REF] and is beyond the scope of this paper.

III. NONLINEAR OBSERVER DESIGN ON SL(3) BASED ON DIRECT MEASUREMENTS

In this section the design of an observer for H ∈ SL(3) is based on the theory for nonlinear observer design directly on the output space proposed in [START_REF] Mahony | Observers for kinematic systems with symmetry[END_REF].

A. Kinematics and measurements

Consider the kinematics of SL(3) given by

Ḣ = F (H, U ) := HU, (10)
with U ∈ sl(3) the group velocity. Assume that U is measured. Furthermore, a set of n measurements p i ∈ P 2 in the bodyfixed frame is available:

p i = h(H, pi ) := H -1 pi |H -1 pi | , i = {1 . . . n}, (11)
where pi ∈ P 2 are constant and known. For later use, define

p := (p 1 , • • • , pn ), p := (p 1 , • • • , p n ). Definition 1: A set M n of n ≥ 4 vector directions pi ∈ P 2 , with i = {1 . . . n}, is called consistent, if it contains a subset M 4 ⊂ M n of 4 constant
vector directions such that all vector triplets in M 4 are linearly independent. This definition implies that if the set M n is consistent, then for all pi ∈ M 4 there exists a unique set of three non vanishing scalars b j = 0 (j = i) such that pi = ẙi /|ẙ i |, with ẙi := 4 j=1(j =i) b j pj . We verify that SL(3) is a symmetry group with group actions φ : SL

(3) × SL(3) -→ SL(3), ψ : SL(3) × sl(3) -→ sl(3) and ρ : SL(3) × P 2 -→ P 2 defined by φ(Q, H) := HQ, ψ(Q, U ) := Ad Q -1 U = Q -1 U Q, ρ(Q, p) := Q -1 p |Q -1 p| .
It is straightforward to verify that φ, ψ, and ρ are right group actions in the sense that φ

(Q 2 , φ(Q 1 , H)) = φ(Q 1 Q 2 , H), ψ(Q 2 , ψ(Q 1 , U )) = ψ(Q 1 Q 2 , U ), and ρ(Q 2 , ρ(Q 1 , p)) = ρ(Q 1 Q 2 , p), for all Q 1 , Q 2 , H ∈ SL(3), U ∈ sl(3) and p ∈ P 2 . One also verifies that ρ(Q, h(H, pi )) = h(φ(Q, H), pi ) dφ Q (H)[F (H, U )] = F (φ(Q, H), ψ(Q, U )).
Thus, the kinematics are right equivariant (see [START_REF] Mahony | Observers for kinematic systems with symmetry[END_REF]), which is an important property, allowing us to apply the observer design framework proposed in [START_REF] Mahony | Observers for kinematic systems with symmetry[END_REF].

B. Nonlinear observer design

Let Ĥ ∈ SL(3) denote the estimate of H. Define the right group error E := ĤH -1 ∈ SL(3) and the output errors e i ∈ P 2 :

e i := ρ( Ĥ-1 , p i ) = Ĥp i | Ĥp i | = Ep i |Ep i | . ( 12
)
The proposed observer takes the form

Ḣ = ĤU -∆( Ĥ, p) Ĥ, (13)
where ∆( Ĥ, p) ∈ sl(3) is the innovation term to be designed so that it is right equivariant in the sense that for all Q ∈ SL(3):

∆(φ(Q, Ĥ), ρ(Q, p 1 ), • • • , ρ(Q, p n )) = ∆( Ĥ, p 1 , • • • , p n ).
Interestingly, if ∆( Ĥ, p) is right equivariant, one can verify that the dynamics of the canonical error E is autonomous and given by [16, Th. 1]:

Ė = -∆(E, p)E. (14)
In order to determine the innovation term ∆( Ĥ, p), a nondegenerate right-invariant cost function C p( Ĥ, p) is required so that ∆( Ĥ, p) can be computed as

∆( Ĥ, p) := (grad 1 C p( Ĥ, p)) Ĥ-1 , (15)
where grad 1 is the gradient in the first variable, using a rightinvariant Riemannian metric on SL(3). As a consequence of [START_REF] Mahony | Observers for kinematic systems with symmetry[END_REF], if the chosen cost function C p( Ĥ, p) is non-degenerate, meaning that (I, p) is its global minimum, then the equilibrium E = I of the autonomous system ( 14) is locally asymptotically stable.

Let us first define individual degenerate right-invariant costs at pi on the output space P 2 as follows:

C i pi : SL(3) × P 2 -→ R + , ( Ĥ, p i ) → C i pi ( Ĥ, p i ) := ki 2 Ĥpi | Ĥpi| -pi 2
with k i positive gains, chosen depending on the relative confidence in the measurements. One verifies that

C i pi ( Ĥ, p i ) are right-invariant in the sense that C i pi (φ(Q, Ĥ), ρ(Q, p i )) = C i pi ( Ĥ, p i ) for all Q ∈ SL(3).
The costs C i pi ( Ĥ, p i ) are degenerate since by taking p i = pi there exists an infinity of Ĥ such that C i pi ( Ĥ, pi ) = 0. The aggregate cost function C p( Ĥ, p) is then defined as the sum of all the individual costs as follows:

C p : SL(3)×(P 2 × • • • × P 2 ) -→ R + , ( Ĥ, p) → C p( Ĥ, p) := n i=1 k i 2 Ĥp i | Ĥp i | -pi 2 (16)
It is straightforward that C p( Ĥ, p) is also right-invariant. According to [START_REF] Mahony | Observers for kinematic systems with symmetry[END_REF]Lem. 3], the aggregate cost is non-degenerate if n i=1 stab ρ (p i ) = {I}, where the stabilizer stab ρ (p) of an element p ∈ P 2 is defined by

stab ρ (p) := {Q ∈ SL(3) | ρ(Q, p) = p}. In fact, n i=1 stab ρ (p i ) = {I} is equivalent to n i=1 s i = {0}
, where s i = ker(dρ pi (I)) is the Lie-algebra associated with stab ρ (p i ).

The following lemma establishes a sufficient condition ensuring that the aggregate cost C p( Ĥ, p) is non-degenerate.

Lemma 1: Assume that the set M n of the measured directions pi is consistent. Then, the aggregate cost C p( Ĥ, p) defined by ( 16) is non-degenerate and, consequently, (I, p) is a global minimum of the aggregate cost C p( Ĥ, p). Proof: One computes the derivative [START_REF] Malis | 2-1/2-d visual servoing[END_REF] with some U ∈ sl(3). From (17) one deduces that

dρ pi (H)[HU ] = d H -1 pi |H -1 pi| [HU ] = I -(H -1 pi)(H -1 pi) |H -1 pi| 2 U H -1 pi |H -1 pi|
s i = ker(dρ pi (I)) = {U ∈ sl(3) | π pi Up i = 0} with π x := (I -xx ) for all x ∈ S 2 . Thus, n i=1 s i = {U ∈ sl(3) | π pi Up i = 0, ∀i = 1, • • • , n}
Let us now determine U ∈ sl(3) such that π pi Up i = 0, for all i = 1, • • • , n. The relation π pi Up i = 0 can be equivalently written as Up i = λ i pi , with λ i := p i Up i . From there one deduces that λ i are eigenvalues of U and pi are the associated eigenvectors. Since the set M n of the measured directions pi is consistent, without loss of generality it can be assumed that the subset M 4 = {p 1 , p2 , p3 , p4 } is consistent. Thus, there exist 3 non-null numbers b 1 , b 2 , and b 3 such that p4 = 3 i=1 b i pi . From there one deduces that

3 i=1 b i λ i pi = 3 i=1 b i λ 4 pi , (18)
using the fact that

Up 4 = U 3 i=1 b 1 p1 = 3 i=1 b 1 Up 1 = 3 i=1 b i λ i pi and Up 4 = λ 4 p4 = λ 4 3 i=1 b i pi = 3 i=1 b i λ 4 pi . Since b i (with i = 1, 2, 3
) are non-null and the 3 unit vectors pi (with i = 1, 2, 3) are linearly independent, (18) directly yields λ 1 = λ 2 = λ 3 = λ 4 . Let λ denote the value of these four identical eigenvalues.

One easily deduces that the geometric multiplicity of the eigenvalue λ (defined as the dimension of the eigenspace associated with λ) is equal to 3, since the 3 eigenvectors pi (with i = 1, 2, 3) associated with λ are linearly independent. Since the algebraic multiplicity of the eigenvalue λ is no less than the corresponding geometric multiplicity, one deduces that it is also equal to 3. This means that U has a triple eigenvalue λ. Since the number of linearly independent eigenvectors of U is equal to 3, the matrix U is diagonalizable. Then, the diagonalizability of U along with the fact that it has a triple eigenvalue implies that U = λI. One thus obtains tr(U ) = 3λ, which is null since U is an element of sl(3). Consequently, λ = 0 and U = 0. One then deduces n i=1 s i = {0}, which concludes the proof.

Remark 1: The consistency of the set M n of the measured directions pi ∈ P 2 , with i = {1 . . . n}, provides an observability condition that is coherent with the well-known fact that the homography can only be computed from the correspondences of at least 4 non-aligned points on the same plane [START_REF] Hartley | Multiple View Geomerty in Computer Vision[END_REF].

Lemma 2: The innovation term ∆( Ĥ, p) defined by ( 15) is right equivariant and explicitly given by

∆( Ĥ, p) = - n i=1 k i π ei pi e i , with e i = Ĥp i | Ĥp i | . ( 19
)
Proof: The proof for ∆( Ĥ, p) to be equivariant is a direct result of [START_REF] Mahony | Observers for kinematic systems with symmetry[END_REF]. Let •, • : sl(3) × sl(3) -→ R be a positive definite inner product on sl(3), chosen to be the Euclidean matrix inner product on R 3×3 . Then, a right-invariant Riemannian metric on SL(3) induced by the inner product

•, • is defined by U 1 H, U 2 H H := U 1 , U 2 .
Using standard rules for transformations of Riemannian gradients and the fact that the Riemannian metric is right invariant, one obtains

D 1 C p( Ĥ, p)[U Ĥ] = grad 1 C p( Ĥ, p), U Ĥ H = grad 1 C p( Ĥ, p) Ĥ-1 Ĥ, U Ĥ H = grad 1 C p( Ĥ, p) Ĥ-1 , U = ∆( Ĥ, p), U , (20)
with some U ∈ sl(3). On the other hand, in view of ( 16) one has

D 1 C p( Ĥ, p)[U Ĥ] = d Ĥ C p( Ĥ, p)[U Ĥ] = n i=1 k i Ĥpi | Ĥpi| -pi I -( Ĥpi)( Ĥpi) | Ĥpi| 2 (U Ĥ)pi | Ĥpi| = n i=1 k i (e i -pi ) (I -e i e i )U e i = tr n i=1 k i e i (e i -pi ) (I -e i e i )U = -tr n i=1 k i e i p i π ei U = - n i=1 k i π ei pi e i , U .
(21) Finally, the explicit expression of ∆( Ĥ, p) given by ( 19) is directly obtained from ( 20) and [START_REF] Mondragón | 3d pose estimation based on planar object tracking for uavs control[END_REF].

One deduces from [START_REF] Malis | Deeper understanding of the homography decomposition for vision-based control[END_REF] that

∆(E, p) = - n i=1 k i π ei pi e i , with e i = Ep i |Ep i | ,
and, consequently, from ( 14) that

Ė = n i=1 k i π ei pi e i E = -grad 1 C p(E, p). (22)
From there the main result of this paper can be stated. Theorem 1: Consider the kinematics [START_REF] Hamel | Homography estimation on the special linear group based on direct point correspondence[END_REF] and assume that the velocity group U ∈ sl(3) is known. Consider the nonlinear observer defined by [START_REF] Krupínski | An inertialaided homography-based visual servo control approach for (almost) fully actuated autonomous underwater vehicles[END_REF], with the innovation term ∆( Ĥ, p) ∈ sl(3) defined by [START_REF] Malis | Deeper understanding of the homography decomposition for vision-based control[END_REF]. If the set M n of the measured directions pi is consistent, then the equilibrium E = I of the autonomous system [START_REF] Mufti | Super resolution of speed signs in video sequences[END_REF] is locally asymptotically stable. Proof: This theorem can be seen as a direct result of Theorem 2 in [START_REF] Mahony | Observers for kinematic systems with symmetry[END_REF], but it can also be proved using classical Lyapunov theory. The candidate Lyapunov function under consideration is L 0 := C p(E, p). Using the consistency of the set M n , one can ensure that L 0 is locally a positive definite function of E. The time-derivative of L 0 along the error flow [START_REF] Mufti | Super resolution of speed signs in video sequences[END_REF] verifies

L0 = d dt n i=1 ki 2 Epi |Epi| -pi 2 = n i=1 k i ( Epi |Epi| -pi ) (I -(Epi)(Epi) |Epi| 2 ) Ėpi |Epi| = - n i=1 k i (e i -pi ) π ei ∆e i = tr( n i=1 k i e i p i π ei ∆) = -||∆(E, p)|| 2 ,
where || • || is the Frobenius norm defined by ||A|| = A, A for any real valued square matrix A. From there one ensures that E is locally bounded. Moreover, by application of LaSalle's theorem, one deduces that ∆(E, p) converges to zero. From the definitions of ∆ (19) and e i (12), one deduces

∆E -= n i=1 I - Ep i p i E |Ep i | 2 pi p i |Ep i | .
Computing the trace of ∆E -one obtains

tr(∆E -) = n i=1 1 |Ep i | 3 |Ep i | 2 |p i | 2 -((Ep i ) pi ) 2 . Define X i := Ep i and Y i := pi . It is straightforward to verify that tr(∆E -) = n i=1 1 |X i | 3 |X i | 2 |Y i | 2 -(X i Y i ) 2 ≥ 0
Using the fact that ∆ = 0 at the equilibrium and therefore tr(∆E -) = 0, as well as the Cauchy-Schwarz inequality, it follows that X i Y i = ±|X i ||Y i | and consequently one has:

(Ep i ) pi = ±|Ep i ||p i |, ∀i = {1, • • • , n},
which in turn implies the existence of some scalars

λ i = ±|Ep i | such that Ep i = λ i pi . (23)
Using [START_REF] Nguyen | Inertialaided homography-based visual servo control of autonomous underwater vehicles without linear velocity measurements[END_REF] and exploiting the consistency of the set M n , one can proceed analogously to the proof of Lemma 1 to deduce that E has a triple eigenvalue λ and E = λI. Then, evoking the fact that det(E) = 1, one deduces that λ = 1 and E = I.

Consequently, E converges asymptotically to the identity I.

Remark 2: The boundaries of the stability domain associated with Theorem 1 are extremely difficult, and probably impossible, to analytically characterise. The nature of the Lyapunov function L 0 is always well conditioned in the neighbourhood of the correct homography matrix, but the global geometry of SL( 3) is complex and there will be critical points and non-convex cost structure when the rotation component of the homography matrix has errors approaching π rads from the true homography. The authors believe, based on extensive simulation and experimental studies (cf. Sections V and VI) and our intuition for such problems, that the stability domain is very large in practice, encompassing all realistic scenarios where the camera actually observes the desired scene (rotations of less than π/2 rads and moderate displacements).

IV. APPLICATION TO ROBOTIC SYSTEMS A. Homography kinematics from a camera moving with rigidbody motion

In this section the case where a sequence of homographies is generated by a moving camera viewing a stationary planar surface is considered. The goal is to develop a nonlinear filter for the image homography sequence using the velocity associated with the rigid-body motion of the camera rather than the group velocity of the homography sequence, as was assumed in Section III. In fact, any group velocity (infinitesimal variation of the homography) must be associated with an instantaneous variation in measurement of the current image A and not with a variation in the reference image Å. This imposes constraints on two degrees of freedom in the homography velocity, namely those associated with variation of the normal to the reference image, and leaves the remaining six degrees of freedom in the homography group velocity depending on the rigid-body velocities of the camera.

Denote the rigid-body angular velocity and linear velocity of {A} with respect to { Å} expressed in {A} by Ω and V , respectively. The rigid body kinematics of (R, ξ) are given by:

Ṙ = RΩ × (24) ξ = RV ( 25
)
where Ω × is the skew symmetric matrix associated with the vector cross-product, i.e. Ω × y = Ω × y, for all y ∈ R 3 .

Recalling (8) one easily verifies that:

ḋ = -η V, d dt d = 0.
This constraint on the variation of η and d is precisely the velocity constraint associated with the fact that the reference image is stationary. Consider a camera attached to the moving frame {A} moving with kinematics ( 24) and ( 25) viewing a stationary planar scene. The group velocity U ∈ sl(3) induced by the rigid-body motion, and such that the dynamics of H satisfies [START_REF] Hamel | Homography estimation on the special linear group based on direct point correspondence[END_REF], is given by [15, Lem. 5.3]

U = Ω × + V η d - η V 3d I.
Note that the group velocity U induced by camera motion depends on the additional variables η and d that define the scene geometry at time t as well as the scale factor γ. Since these variables are unmeasurable and cannot be extracted directly from the measurements, in the sequel, let us rewrite:

U := Ω × + Γ, with Γ = V η d - η V 3d I. (26)
Since { Å} is stationary by assumption, the vector Ω can be directly obtained from the set of embedded gyroscopes. The term Γ is related to the translational motion expressed in the current frame {A}. If we assume that ξ d is constant (e.g. the situation in which the camera moves with a constant velocity parallel to the scene or converges exponentially toward it), and using the fact that

V = R ξ, it is straightforward to verify that Γ = [Γ, Ω × ], (27)
where [Γ, Ω × ] = ΓΩ × -Ω × Γ is the Lie bracket. However, if we assume that V d is constant (the situation in which the camera follows a circular trajectory over the scene or performs an exponential convergence towards it), it follows that

Γ1 = Γ 1 Ω × , with Γ 1 = V d η . (28)

B. Observer with partially known velocity of the rigid body

In this section it is assumed that, instead of the group velocity U , only the angular velocity Ω is measured. The goal is to provide an estimate Ĥ ∈ SL(3) for H ∈ SL(3) to drive the group error E (= ĤH -1 ) to the identity matrix I and the error term Γ := Γ -Γ (resp. Γ1 := Γ 1 -Γ1 ) to 0 if Γ (resp. Γ 1 ) is constant or slowly time varying. The observer when ξ d is constant is chosen as follows (compare to [START_REF] Krupínski | An inertialaided homography-based visual servo control approach for (almost) fully actuated autonomous underwater vehicles[END_REF]):

Ḣ = Ĥ(Ω × + Γ) -∆( Ĥ, p) Ĥ, (29)
Γ = [ Γ, Ω × ] -k I Ad Ĥ ∆( Ĥ, p). ( 30
)
and the observer when V d is constant is defined as follows:

Ḣ = Ĥ(Ω × + Γ1 - 1 3 tr( Γ1 )I) -∆( Ĥ, p) Ĥ, (31)
Γ1 = Γ1 Ω × -k I Ad Ĥ ∆( Ĥ, p). ( 32
)
with some positive gain k I and ∆( Ĥ, p) given by [START_REF] Malis | Deeper understanding of the homography decomposition for vision-based control[END_REF].

Proposition 1: Consider a camera moving with kinematics ( 24) and ( 25) viewing a planar scene. Assume that Å is stationary and that the orientation velocity Ω ∈ {A} is measured and bounded. Let H : A → Å denote the calibrated homography [START_REF] Fraundorfer | Topological mapping, localization and navigation using image collections[END_REF] and consider the kinematics [START_REF] Hamel | Homography estimation on the special linear group based on direct point correspondence[END_REF] along with [START_REF] Scaramuzza | Appearance-guided monocular omnidirectional visual odometry for outdoor ground vehicles[END_REF]. Assume that H is bounded and that Γ (resp. Γ 1 ) is such that it obeys (27) (resp. ( 28)). Consider the nonlinear observer defined by [START_REF] Zhang | Parameter estimation techniques: a tutorial with application to conic fitting[END_REF][START_REF] Zhang | 3D reconstruction based on homography mapping[END_REF], (resp. (31-32)) along with the innovation ∆( Ĥ, p) ∈ sl(3) given by [START_REF] Malis | Deeper understanding of the homography decomposition for vision-based control[END_REF]. Then, if the set M n of the measured directions pi is consistent, the equilibrium (E, Γ) = (I, 0) (resp. (E, Γ1 ) = (I, 0)) is locally asymptotically stable. Proof: We will consider only the situation where the estimate of Γ is used. The same arguments can also be used for the case where the estimate of Γ 1 is considered. Differentiating e i given by ( 12) and using ( 29) one obtains ėi = -π ei (∆ + Ad Ĥ Γ)e i .

Consider the following candidate Lyapunov function:

L = L 0 + 1 2k I || Γ 2 = n i=1 ki 2 |e i -pi | 2 + 1 2k I || Γ 2 . ( 33
)
Differentiating L and using the fact that tr Γ Γ, Ω = 0, it follows:

L = n i=1 k i (e i -pi ) ėi + tr Γ Ad ĤT ∆ = - n i=1 k i (e i -p i ) π ei (∆+Ad Ĥ Γ)e i +tr Ad Ĥ-1 ∆ Γ .
Using the fact that tr(AB) = tr(B A ) one deduces

L = n i=1 k i p i π ei (∆ + Ad Ĥ Γ)e i + tr Ad Ĥ-1 ∆ Γ =tr n i=1 k i e i p i π ei (∆ + Ad Ĥ Γ) + Ad Ĥ-1 ∆ Γ =tr n i=1 k i e i p i π ei ∆+Ad Ĥ-1 (∆ + n i=1 k i e i p i π ei ) Γ
Finally, using the expression [START_REF] Malis | Deeper understanding of the homography decomposition for vision-based control[END_REF] of ∆ one obtains L = -∆ 2 . The derivative of the Lyapunov function is negative semi-definite, and equal to zero when ∆ = 0. Given that Ω is bounded, one easily verifies that L is uniformly continuous and Barbalat's lemma can be used to prove the asymptotic convergence of ∆ to zero. Using the same arguments as in the proof of Theorem 1, it is straightforward to verify that E converges to I. Consequently, the left-hand side of the Lyapunov expression (33) converges to zero and Γ 2 converges to a constant.

Computing the time derivative of E and using the fact that ∆ converges to zero and E converges to I, it is straightforward to show that lim t→∞ Ė = -Ad Ĥ Γ = 0. Using boundedness of H one ensures lim t→∞ Γ = 0.

C. Outlier rejection with M-estimator-like nonlinear observer

The cost function [START_REF] Mahony | Observers for kinematic systems with symmetry[END_REF] previously used for observer design is the sum of individual squared residuals r 2 i , with r i := Ĥpi | Ĥpi| -pi . This function is, however, extremely unstable when outliers are present in the measurements. In practice outliers are a result of wrong feature matching, which is almost unavoidable. Inspired by the robust M-estimator techniques (see, e.g., [START_REF] Zhang | Parameter estimation techniques: a tutorial with application to conic fitting[END_REF] and the references therein), we consider the following cost function [instead of ( 16)]:

C p( Ĥ, p) := n i=1 k i ρ(r i ), (34)
where ρ(•) is a symmetric, positive-definite function with a unique minimum at zero, and is chosen less increasing than the square. From there, the proposed observer is still given by the general form ( 13) and the innovation term ∆( Ĥ, p) is still derived from [START_REF] Mahony | Nonlinear complementary filters on the special linear group[END_REF]. From (34) and analogously to [START_REF] Mondragón | 3d pose estimation based on planar object tracking for uavs control[END_REF], one deduces Several possible choices of ρ and its associated w function are discussed in [START_REF] Zhang | Parameter estimation techniques: a tutorial with application to conic fitting[END_REF]. For instance, the Tukey ρ function, which is chosen for the experimental Section VI, is given by

D 1 C p( Ĥ, p)[U Ĥ] = d Ĥ C p( Ĥ, p)[U Ĥ] = n i=1 k i w(r i ) Ĥpi | Ĥpi| -pi I -( Ĥpi)( Ĥpi) | Ĥpi| 2 (U Ĥ)pi | Ĥpi| = - n i=1 k i w(r i )π ei pi e i , U , (35)
ρ(x) = (c 2 /6)(1 -[1 -(x/c) 2 ] 3 ) if |x| ≤ c c 2 /6 if |x| > c
and, thus,

w(x) = [1 -(x/c) 2 ] 2 if |x| ≤ c 0 if |x| > c (36)
with c a tuning parameter. From [START_REF] Mei | Efficient homographybased tracking and 3-d reconstruction for single-viewpoint sensors[END_REF] and (35) one obtains [instead of [START_REF] Malis | Deeper understanding of the homography decomposition for vision-based control[END_REF]]

∆( Ĥ, p) = - n i=1 k i w(r i )π ei pi e i . ( 37
)
From there, the same results as in Theorem 1 and Proposition 1 can be stated but with the innovation term ∆( Ĥ, p) given by (37) instead of [START_REF] Malis | Deeper understanding of the homography decomposition for vision-based control[END_REF]. The proofs proceed identically to the proofs of Theorem 1 and Proposition 1 where the function L 0 is chosen equal to C p( Ĥ, p) defined in (34) instead of [START_REF] Mahony | Observers for kinematic systems with symmetry[END_REF]. By definition, an observer is robust to outliers if the influence of any outlier is small enough to create any important offset. Therefore, the weight function w should reduce the effect of large residuals. For instance, the Tukey weight function (36) even suppresses residuals larger than the tuning parameter c. In [START_REF] Zhang | Parameter estimation techniques: a tutorial with application to conic fitting[END_REF], a quantitative analysis on the influence of the associated tuning parameter of some ρ functions is summarized in the case where the residuals r i belong to R. However, in the proposed work the residuals r i evolve in a compact set [0, 2]. Quantitatively analyzing the improvement of precision and robustness of the homography estimation when using the proposed M-estimator-like observer thus remains an open subject which by its own right needs to be carefully addressed in our future works. Nevertheless, based on extensive experimental tests (e.g. as reported in Section VI) we have observed that the precision and robustness of the homography estimate are greatly enhanced with the proposed M-estimator-like observer. For instance, the latter is very robust even in presence of extreme occlusions (see Sections VI-B2 and VI-C) while the basic version of the proposed observer tends to diverge in such situations.

V. SIMULATION RESULTS

In this section the performance and robustness of the proposed approach are illustrated through simulation results. The camera is attached to an aerial vehicle moving in a circular trajectory belonging to a plane parallel to the ground. The reference camera frame { Å} is chosen as the North-East-Down frame situated above four observed points on the ground. The four observed points form a square whose center lies on the Z-axis of the frame { Å}. The vehicle's trajectory is chosen such that the term Γ 1 defined by ( 28) remains constant, and the observer (31-32) is applied with the following gains: k P = 4, k I = 1. Distributed noise of variance 0.01 is added to the angular velocity measurements. The chosen initial estimated homography Ĥ(0) corresponds to i) an error of π/2 in both pitch and yaw angles of the attitude, and ii) an estimated translation equal to zero. The initial value of Γ1 is set to zero. From 40s to 45s, it is assumed that the measurements of two observed points are lost. Then, from 45s the measurements of all four points are regained as previously. The results reported in Fig. 1 show a good convergence rate of the estimated homography to the real homography (see from 0 to 40s and from 45s). The loss of point measurements marginally affects the global performance of the proposed observed. Note that in this situation, no existing method for extracting the homography from measurements of only two points is available.

VI. APPLICATION TO IMAGE STABILIZATION WITH REAL-TIME IMPLEMENTATION

A. Experimental implementation aspects

Feature detection and matching: Code has been implemented in C++ with OpenCV library. Due to real-time constraints, feature detection and descriptor extraction in images are carried out using the FAST Feature Detector and ORB Descriptor Extractor algorithms already implemented in the OpenCV library. Then, feature matching is performed using OpenCV's brute-force matcher routine with L 2 -norm. We have purposefully avoided using more sophisticated image processing routines in order to demonstrate the raw performance of our observer.

It is quite unrealistic to track one and the same set of point-features through a long image sequence. We have hence opted to match point-features between the reference image and each subsequent image frame separately. To do this, we first compute a predicted homography estimate Ĥ+ k that will be used to transform the current image I k (i.e. warp the current

image using Ĥ+

k to obtain a prediction of the reference image) using the OpenCV's warpPerspective function before applying feature extraction and matching. More precisely, in the case where the angular velocity measurements are available, Ĥ+ k is obtained by forward integrating the observer equations ( 29)- [START_REF] Zhang | 3D reconstruction based on homography mapping[END_REF] during the time period [t k-1 , t k ] using only the gyrometer measurements (i.e. setting the observer gains k i (i = 1, • • • , n) and k I to zero) and using ( Ĥk-1 , Γk-1 ) as initial conditions. Note that the brute-force matching algorithm is well suited to this approach since it favors translational mo-tion over rotational motion, and most of the rotational motion has already been compensated for by forward integrating the angular velocity. On the other hand, in the case of absence of angular velocity measurements, Ĥ+ k is simply set equal to Ĥk-1 . Note that the procedure described above does not satisfy the assumptions of our theoretical convergence results because different point features are selected for each video frame. However, it is intuitively clear that such a procedure will still work as long as the selected features provides a consistent set. Indeed, our experimental results strongly support this claim. . Again, we have purposefully avoided the use of more sophisticated (and much more computationally expensive) alternative algorithms for outlier removal, such as RANSAC. Our simple and fast outlier removal method has yielded quite good matching results (see, e.g., Fig. 2). However, outliers cannot be completely removed especially in case of occlusions. This in turn justifies the usefulness of our M-estimator-like observer for outlier rejection. Correction step of the observer: After the steps of feature detection and matching, we use the observer gains of k i = 80 (i = 1, • • • , n), k I = k i /10 and the Tukey ρ function (see [START_REF] Zhang | Parameter estimation techniques: a tutorial with application to conic fitting[END_REF]) with parameter c = 0.05 to rapidly iterate the observer equations 200 times per video frame 2 . The computational effort for this last step is negligible compared to the previous image processing steps.

B. Offline validations using Youtube videos

Two video sequences (downloaded from Youtube) filmed from a camera mounted on an underwater robotic vehicle are used to test our algorithm for the classical image stabilization problem. Since the camera parameters are not known, we just make use of very rough (and certainly very erroneous) estimates of these parameters. The reported results presented hereafter indicate that, as long as the image homography estimate (used to warp the current image) is concerned, the stabilization performance of the proposed observer is very robust with respect to these parameter uncertainties.

These two video sequences show some realistic scenarios of inspection of underwater infrastructures using underwater vehicles. Some challenging issues such as occlusions (due to moving fishes or a robotic manipulator), low-light and lowtextured scene, large translation motions, poor image quality, etc. allow us to test the robustness and performance of the proposed approach. Moreover, the absence of the angular velocity measurements in association with each image frame also renders the feature matching procedure more challenging.

The full validation videos of these two video sequences are available at:

• Video sequence 1: https://youtu.be/-eMjWSWVm2A • Video sequence 2: https://youtu.be/XqZkOC01eys

1) Video sequence 1: In this video sequence (30f ps, 632×480px of resolution, 680 frames), the underwater vehicle that carries the camera approaches a 3D complex structure composed of several roughly planar surfaces. The proposed homography observer is tested using the image frame 180 as the reference image. Feature detection is carried out within a small region of interest (yellow rectangle depicted in Fig. 2) that corresponds to one of the planar surfaces of the "box" structure. The initial homography estimate is set equal to the identity matrix, but the initial homography error is quite large since the camera's initial position is relatively far from the reference one (corresponding to the reference image). However, as shown in Fig. 3 and in view of the location of the yellow quadrangle (i.e. warped reference rectangle using the image homography estimate) in the reported images (i.e. frames 0, 2, 4, 6, 8, 10, 12, 14, 16, 18), a very fast convergence of the image homography estimate to the real one can be observed. From the warped images in Fig. 4, very good and robust quality of image stabilization can be appreciated despite poor image quality, large motions of the camera (frames 20, 275, 678) and large occlusions due to fishes (frames 127, 422, 465) which in turn highlights the excellent performance of our approach. A complete video showing this offline experiment can be viewed at https://youtu.be/-eMjWSWVm2A where the second part of the video (starting from second 23) shows a comparison between our approach with the classical algebraic least-square homography algorithm using the OpenCV's findHomography function with RANSAC option activated for outlier removal. One easily observes that, by contrast with our algorithm, the performance and robustness of the classical least-square algorithm are very poor even when combined with the sophisticated RANSAC outlier removal procedure.

2) Video sequence 2: This video sequence (25f ps, 640 × 360px of resolution, 3220 frames) shows a scenario where an underwater vehicle is stabilized in front of an artificial panel and a mounted manipulator arm is teleoperated to turn valves on this panel. We apply our homography observer to this sequence using the image frame 530 as the reference image. The region of interest for feature detection is the whole image. Similarly to the first video sequence, we initially set ( Ĥ0 , Γ0 ) = (I, 0). As the initial homography error is not very large like in the first test and the feature quality is good, the challenge is here essentially related to: 1) extreme and permanent occlusions due to the manipulator arm and/or camera rotation, and 2) the fact that some parts of the observed scene are modified throughout the video sequence (i.e. valves are turned by the manipulator arm). Analogously to the first test, Fig. 5 shows an excellent and extremely robust performance of our approach despite the above-mentioned challenges related to occlusions and changes inside the target. A complete video showing this experiment is available at https://youtu.be/XqZkOC01eys

C. Real-time experimental validations using an IMU-Camera system

In this section we show that real-time implementation of the proposed algorithm is possible. We make use of a Visual-Inertial (VI) sensor developed by the Autonomous Systems Lab (ETH Zurich) and the company Skybotix. This VI-sensor is composed of two cameras (Aptina MT9V034 CMOS) and an IMU (Analog Devices ADIS16375 MEMS). However, only the left camera is used to validate the proposed algorithm. The main reason for using the VI-sensor in this experimental setup is the possibility of obtaining perfectly time-synchronized images and IMU readings (20Hz for the camera and 200Hz for the IMU). The implementation has been carried out on an Intel Core i7-6400 CPU running at 3.40Ghz. The transmission of data from the camera to the PC is carried out through a high speed ethernet cable. The PC has a Linux based operating system and is responsible for two major tasks: 1) interfacing with the camera hardware and acquisition of images and IMU data from the VI-sensor; and 2) real-time estimation of the homography at 20Hz using the proposed algorithm.

Real-time implementation of the proposed observer has been performed successfully, as illustrated by the two following videos (as depicted in Fig. 6) that show the excellent performance and robustness of our algorithm: • Fast motion: https://youtu.be/PeoaUzDkyUo • Very fast motion: https://youtu.be/cctG jKelXo As shown in Fig. 7, our algorithm is robust with respect to very fast translational and rotational motions, poor image quality, severe image blurs due to fast motions and extreme occlusions. Even when our algorithm selects wrong feature matches (e.g. from frame 1140 to frame 1190 of the first video), the observer continues to track the region of interest well and quickly recovers from any tracking errors.

VII. CONCLUSIONS

In this paper we developed nonlinear observers for a sequence of homographies represented as elements of the Special Linear group SL(3). The observers directly use point-feature correspondences from an image sequence without requiring explicit computation of the individual homographies between any two given images and fuses these measurements with measurements of angular velocity from onboard gyroscopes using the correct Lie group geometry. The stability of the proposed algorithms has been proved for both cases of known full group velocity and partially known rigid-body velocities.

Even if the characterization of the stability domain still remains an open issue, simulation and experimental results have been provided as a complement to the theoretical approach to demonstrate a large domain of stability. A potential application to image stabilization in the presence of very fast camera motions and severe occlusions has been demonstrated with very encouraging results even for a relatively low video frame rate.

  where w(•) is the weight function defined by w(x)

Fig. 1 .

 1 Fig. 1. Estimated homography components (solid line) and true homography ones (dashed line) vs. time.

  Outlier removal: To remove matched point-feature outliers, we first compute the standard deviation (sd u , sd v ) and mean values (m u , m v ) of the differences of coordinates in pixel (du k , dv k ) of the point correspondences and then keep only those satisfying    m u -max(sd u , S) ≤ du k ≤ m u + max(sd u , S) m v -max(sd v , S) ≤ dv k ≤ m v + max(sd v , S) |du k | ≤ D, |dv k | ≤ D with S, D pre-defined positive thresholds (S = 30, D = 80 in our experiments)

Fig. 2 .

 2 Fig. 2. Good quality of feature matching between the warped image frame 420 (Top-Right) and the reference frame (Top-Left) of the video sequence 1 and between the warped image frame 2200 (Bottom-Right) and the reference frame (Bottom-Left) of the video sequence 2 when applying our outlier removal procedure despite severe occlusions by fishes or a manipulator arm.

Fig. 3 .

 3 Fig.3. Video sequence 1 (offline validation). Fast convergence of the image homography estimates to the real ones (less than 0.3s), which can be appreciated by the positions of the yellow quadrangle (i.e. warped reference rectangle using the image homography estimate) in the reported image frames 0, 2, 4, 6, 8, 10, 12, 14, 16, 18.

Fig. 4 .

 4 Fig. 4. Warped and current image frames 20, 127, 275, 422, 465, 678 of the video sequence 1 (offline validation). Excellent and robust performance of the proposed approach can be appreciated from either the warped images (Right) or the yellow quadrangle in the current images (Left) despite image low resolution, large camera motions (e.g. frames 20, 275, 678) and important occlusions due to fishes (e.g. frames 127, 422, 465). A complete video showing this experiment is available at https://youtu.be/-eMjWSWVm2A.

Fig. 5 .Fig. 6 .

 56 Fig. 5. Warped and current image frames 70, 1158, 1732, 2195, 2220, 2300, 2640, 3025 of the video sequence 2 (offline validation).Excellent and robust performance of the proposed approach is illustrated by stable warped images (Right) and the correct location of the red quadrangle (that warps the interested red rectangle in the reference image using the estimated homography) in the current images (Left) despite extreme occlusions due to the manipulator arm (e.g. frames 1158, 1732, 2195, 2300, 2640, 3025) and changes in the target due to turned valves (e.g. frames 1158, 1731, 3025). A complete video showing this experiment is available at https://youtu.be/XqZkOC01eys

Most statements in projective geometry involve equality up to a multiplicative constant denoted by ∼ =.

Note that the observer gains used for each iteration are divided by the number of iteration, i.e. ki = k i /N iter and kI = k I /N iter with N iter = 200.

Acknowledgments: This work was supported by the FUI GreenExplorer project, the EQUIPEX Robotex project, and the Chaire d'excellence en robotique RTE-UPMC, and the


Jean-Philippe Chancelier
email: jean-philippe.chancelier@enpc.fr

Sébastien Furic
email: sebastien.furic@inria.fr

Pierre Weis
email: pierre.weis@inria.fr

Translating Simulink Models to Modelica using the Nsp Platform

Keywords: Nsp, Simulink, Modelica

We present a new Simulink (Simulink) to Modelica (Modelica) translation chain embedded into Nsp. Translated models can be edited (original Simulink diagrams are preserved through translation) and simulated. This translation chain makes use of the Simport tool, originally designed to translate Simulink models to Scicos models, and also relies on Modelicac, i.e. Scicos' Modelica companion compiler.

Using some examples, we demonstrate the effectiveness of the translation process and detail some technical aspects of it. This new Nsp feature extends Nsp's simulation capabilities and makes it a reference platform for users looking for means to simulate Simulink models within a Modelica framework. Resulting Modelica code can even be exported to other Modelica compatible tools.

Introduction

Nsp (Nsp) is a Matlab-like numerical environment which can run the Scicos modeling environment, a Simulink-like block diagram editor and simulator.

From 2003 to 2008, in the course of funded projects SimPA and SimPA2, a Modelica compiler named Modelicac has been developed allowing Scicos to handle genuine Modelica models. This integration of Modelica within Scicos has been the subject of several papers published at the Modelica conference [START_REF] Nikoukhah | Initialization of modelica models in scicos[END_REF][START_REF] Nikoukhah | Towards a full integration of modelica models in the scicos environment[END_REF]. The purpose of using Modelica is to serve as a high-level description language to extend Scicos expressiveness: Modelica allows users to compose "acausal" models where the original environment forced users to describe their models as block diagrams.

In this paper we focus on a new application of Modelica within Scicos under Nsp, that is as a target language for Simulink model translation.

Several Simulink to Modelica translation tools have already been proposed in the past, we mention in particular Mike Dempsey's Simelica and AdvancedBlocks library [START_REF] Dempsey | Automatic translation of simulink models into modelica using simelica and the advancedblocks library[END_REF] and Dirk Reusch's Coselica initiative (Reusch). AdvancedBlocks was a fairly complete library of Modelica blocks which allowed users to use Modelica blocks as a one-to-one replacement for Simulink blocks. Up to our knowledge this work remains the most advanced effort in that direction. It is however no longer maintained. In the Scicos software environment, the Coselica library offers signal models to allow users to better exploit Modelica from within Scicos by proposing a large set of Modelica submodels in the same spirit as the standard Modelica library (MSL). Many Simulink-like blocks are also available in Coselica.

The approach presented here differs from Mike Dempsey's and Dirk Reusch's approaches in that translation of original blocks is not attempted on a one-to-one basis. Instead, we use a two steps translation process: the first step translates the Simulink model into an equivalent Scicos native model; the second step translates the Scicos native model to Modelica code.

To handle the initial Simulink to Scicos translation, we use an external tool named Simport: it translates a Simulink model by translating each block in the original Simulink model with one or several blocks of the Scicos native block library, so that original semantics is preserved with a high degree of confidence.

For the second step, we developed a set of Nsp special purpose compilation routines to translate Scicos blocks to genuine Modelica blocks. When a Scicos block has a direct Coselica equivalent, the compilation routine simply emits thre corresponding Coselica block; when there is no Coselica equivalent, the compilation routine generates an entirely new block containing ad-hoc Modelica code to handle the Scicos block behavior. Using this compiletime Modelica code generation and the two steps translation process was the key ideas to fill the huge semantic gap between Simulink and Modelica.

We give in this paper a detailed description of this new translation chain hosted by the Nsp environment.

Involved Tools

As mentioned above, the translation chain relies on a combination of several tools. We give hereafter a short description of each of them.

Nsp, a Programming Environment for Numerical Applications

Nsp (Nsp) is a mature Matlab-like Scientific Software Package developed under the GPL license. Nsp features a high-level, safe imperative programming language with automatic memory management. This language can be used interactively, giving users an easy access to efficient numerical routines; It can also be used as a more conventional programming language to extend Nsp's capabilities.

Nsp contains internally a class system with simple inheritance and interface implementation. When used as an interactive computing environment, it comes with online help facilities and an easy access to GUI facilities and graphics.

A large set of libraries are available and it is moreover easy to implement new functionalities. External libraries can also be used: this requires writing some wrapper code (also called interface) to live in harmony with Nsp's internal state. The interface mechanism can be either static or dynamic. By using dynamic functionalities one is able to build toolboxes.

Nsp shares many traits with other Matlab-like Scientific Softwares such as Matlab, Octave, ScilabGtk (ScilabGtk; [START_REF] Campbell | Modeling and Simulation in Scilab/Scicos[END_REF], and also with scripting languages such as Python.

The main toolbox used in this work is Scicos that we describe now. Scicos users often needs to use Nsp functions such as those dedicated to filter design for signal processing or controller design in the construction of simulation models. Nsp's programming language can be used for batch processing of multiple simulation tasks, and more generally, models designed by Scicos can be used as functions in Nsp. Nsp's graphical facilities can be used for post processing simulation results. But the integration of Scicos and Nsp goes beyond that. The Scicos editor is entirely written in Nsp's language. This provides many advantages and was in particular of tremendous importance in the current work, indeed: Scicos model data structure is a Nsp structure and thus Scicos models can be programmatically manipulated and build using Nsp scripts. We use this facility in two ways. First to obtain Scicos models from Simulink models, using the fact that the Simport converter produces a Nsp script whose execution in Nsp produces a Scicos model data structure. Second, using Nsp scripts we are able to convert, in a Scicos model data structure, some Scicos blocks to Modelica blocks.

Scicos

In the conversion process from Simulink to Modelica, the scicos compiler/scheduler also plays a key role. It infers dimensions and types used in the Modelica blocs. This is quite an exciting feature since it gives the possibility to have Modelica blocks for which the associated Modelica model is not a fixed Modelica class but a specific one adapted to specific dimensions and types generated on the fly.

Modelicac, a Simple Yet Useful Modelica Compiler

Development of Modelicac started in 2003 as a joint work between Inria and TNI-Valiosys (now Dassault Systèmes) in the course of the SimPA (SIMulation pour le Procédé et l'Automatique) french funded project. The goal was to make Scicos compatible with a significant subset of the Modelica language in order for users to be able to describe complex hybrid models without having to resort to low-level block diagram descriptions. Indeed, building a block diagram from a physical model requires 1)performing a complete analysis of physical phenomena into play (to determine which elementary blocks to use in the diagram), and 2)determining how data flows between blocks (to connect elementary blocks together). On the other hand, Modelica tools considerably ease physical model construction by automatically analysing the overall structure of physical models described in a much more userfriendly way: familiar physical components (e.g. springs, transistors, hydraulic pumps, etc.) can be used to build models. Translation from this high-level description to low-level data flow is performed automatically in a quite satisfactory way, which frees users from a painful work. Moreover, even slight modifications of physical models may require considerable changes in corresponding blockdiagram descriptions; this is not the case with a high-level description.

In its initial version, Modelicac essentially focussed on the "continuous part" of hybrid models. This mainly comprises differential equations and event-trigerring mechanisms (e.g. , "when equations"). Difference equations were however also be described, although with many restrictions, because the idea was to discourage users from writing discrete equations in Modelica. Indeed, Scicos is primarily a hybrid modelling environment and, in particular, it handles discrete, event-trigered changes, much more robustly than Modelica because of its synchronous roots.

In the course of the SimPA project, the Scicos editor has been extended to enable graphical handling of Modelica, native Scicos, as well as hybrid Modelica-Scicos blocks in the same design (see Figure 1). This combination of synchronous and Modelica-based features offered enough modelling expressiveness to enable useful libraries to be developed. Coselica is one of these libraries, and is one of the ingredients of our In 2005, the funded project SimPA2 started, having as objective enhancement of the original Modelicac compiler. Among others, support of multiple-file Modelica libraries and interactive initialization of complex hybrid systems have been added.

As a result, the new Modelicac compiler was able to compete with industrial compilers (it even ranked number two in terms of performance on an industrial thermohydraulic benchmark proposed by EDF in 2009).

Today, the Scicos toolbox with its Modelica-compatible extension is freely available under several environments including Scilab, ScicosLab and Nsp.

Simport, a Simulink Model Importer for Scicos

Capabilities

The Simport [START_REF] Chancelier | Simport[END_REF][START_REF] Chancelier | Simport: A Simulink Model Importer for Scicos[END_REF] Simport is a free software distributed with Nsp Scicos (Scicos) and Activate (Altair Activate).

Capabilities

Simport aims at preserving the original Simulink model semantics: simport performs passes of semantic analysis to explicit the Simulink model meaning and translate it into an equivalent Scicos model.

In any case, the resulting Scicos model preserves the model hierarchy and diagram topology, and the visual aspects of the original model. Simport also supports both the MDL and SLX formats as input for Simulink models.

Simulink block translation

Simport maps Simulink bloks to Scicos blocks using the block translation library. More precisely, each Simulink source block is translated either into

• a single basic block, if there exists an equivalent Scicos block,

• a super-block that implements the Simulink block via a combination of Scicos blocks,

• an empty super-block for user completion, if the Simulink block translation is unsupported

The Simport translation library covers a large subset of Simulink basic blocks, in particular the so-called action blocks.

Simport generated code

The Simport back-end translates explicit semantics of Simulink models to concrete code of the Scicos host language (Nsp or Oml). In addition, the concrete code provides the definition of simulation parameters and embeds various outputs to the host language (e.g. Matlab supporting M-files).

Example

Given the Simulink model described in Figure 2 and saved as file model.mdl.

Limitations

Simport indeed supports a large subset of Simulink basic blocks, but exotic blocks from specific Simulink libraries cannot be translated since they have no Scicos equivalent; When converters are available, Scicos blocks are replaced by Modelica blocks as a one-to-one process. We have developed a specific library of Modelica blocks to ease the replacement. For example Scicos integrator blocks are replaced by MB_Integral Modelica blocks. For some translation we rely on the already available library Coselica (Reusch), but for many blocks a direct Coselica translation fails because of size lim- One way to encompass that difficulty is to generate super blocks for enabling n-dimensional block operations from 1-dimensional basic blocks (See Figure 7 for an example with adder). We have chosen this approach for the converter blocks (See Figure 6) as explained below, but we also implemented specific blocks to deal with n-dimensional signals. For example, the MB_Integral block is a special purpose Modelica-Scicos block which produces at compile time a new Modelica model for each instance of the block in a specific model. As an example, in Figure 5 each MB_Integral Modelica block integrate a 4-dimensional variable without saturation and thus the generated code will be given by Most of the one-to-one block conversion follows the same mechanism. Building a library of Modelica-Scicos blocks is an on-going work and for the time being it only contains about 20 blocks. Indeed, this Library can also be used to directly build models in the Scicos editor, it complements the set of Modelica block available in Scicos giving access to Modelica counterpart of known Scicos blocks.

The one-to-one block conversion is in fact also a multistep process. We proceed as follows.

First block-to-block conversions are performed but converted Modelica-Scicos blocs are not fully usable because they lack local information (for example the final matrix sizes are unknown at first step). Notice that this first step requires Nsp evaluation of block parameters since they may be used to infer types and dimensions. For example the sizes of a Gain block parameter gives the input/output port sizes of the block, except when the parameter size if 1. But in order to obtain the sizes of a given Gain block parameter we need to evaluate Nsp expressions, since parameters can be given through context (produced by Simport from Matlab companion files).

In a second step, links are modified and converters are inserted where appropriate. Notice however that converters sizes are also unknown.

In a third step, sizes and types are obtained by calling the Scicos model compiler. However, since the Scicos model compiler only infers types and dimension for Scicos blocks this step requires a hidden conversion of the hybrid Modelica-Scicos model into a pure Scicos model before trying to infer sizes and types. When sizes and types are inferred for a Modelica-Scicos block, its internal Modelica code can be generated. The code is thus consistent with respect to sizes, types and parameters. The fact that models can be manipulated and generated programmatically is also used in the conversion process. We illustrate this point by describing more precisely MB_MO2Sn the block used to convert Modelica signals to Scicos Signals. The communication between Scicos and Modelica can only be realized using scalar links (for historical reasons, not because of limitations of any of the languages), thus to have converters on links which transfer n-dimensional signals we have implemented a block named MB_MO2Sn as a super-block. That is, the MB_MO2Sn block contains a model and this model is generated dynamically when the link signal size is known. We give in Figure 6 the internal model of a 4-dimensional Modelica to Scicos converter as used in the model displayed in Figure 5. It contains four 1-dimensional Modelica to Scicos converters. To illustrate the possibility to generate models by program, we give in Figure 7 an example of a model which performs a 5-dimensional addition of Modelica signals. Implementing a n-dimensional adder block could be implemented that way, even if we chose to directly embed the Modelica n-dimensional adder block in a unique block.

As already pointed above, during the conversion from Scicos models to Modelica-Scicos models, inferring types and sizes is of utmost importance and it partially relies on Nsp block parameter and context expression evaluation. This is mostly why the conversion cannot completely be performed by Simport. Indeed, inferring types and sizes could have been implemented directly in Simport, if evaluation of Matlab expression were not required in the process.

Translation from Modelica to C

Modelica source code is translated to C thanks to the Modelicac compiler. The idea is as follow. Once the model is being run by the user, Scicos gathers all the blocks whose execution semantics is described by means of Modelica code into a unique Modelica program whose source code is given to Modelicac. This program is actually a highlevel description of the Modelica part of the model. Modelica programs generated by Scicos contain five (possibly empty) sections declaring respectively:

• the parameters of the model,

• the components appearing in the model (i.e. Modelica "blocks" used to build the model),

• the connectors to and from the Scicos world (declared as InPutPorts and OutPutPorts),

• the connection equations (corresponding to links between components of the model, introduced by means of the connect keyword), and

• the correspondence between some Scicos ports and some Modelica connectors used to exchange information between both worlds (introduced by means of an equal sign).

Figure 1 .

 1 Figure 1. A mixed Scicos-Modelica model as displayed by Scicos's editor

  development started in 2007 at Inria: it has now turned into a comprehensive Simulink import assistant for the Scicos and Altair Activate block system modelers: Simport reads a textual representation of a Simulink model (MDL or SLX file format) and generates the corresponding equivalent Scicos model. Based on compilation techniques, Simport is a fast and reliable translator from Simulink models to Scicos or Altair Activate models.

Figure 2 .

 2 Figure 2. Segway controller as a Simulink model

Figure 3 .

 3 Figure 3. Segway controller as a Scicos model

3

  Translation from Simulink to ModelicaThe Translation from Simulink to Modelica is implemented as a two step process. First, as already described, using Simport, we translate a Simulink model into a Scicos model. Second, using Nsp scripts we convert the Scicos model into an hybrid Modelica-Scicos model. Conversion is obtained by 1) translation of Scicos blocks to Modelica blocks, and 2) addition in the model of converters along the links which connect Scicos Blocks to Modelica-Scicos blocks. The hybrid Modelica-Scicos models can be edited and simulated in Scicos editor; thus, even if during the Translation process we cannot obtain a full Modelica model 1 , the resulting hybrid model may still be used for simulation because users have the possibility to complete untranslated parts thanks to the Scicos editor.Figure4is a Scicos model simulating the Lorenz dynamical system. The same model after conversion to Modelica is shown in Figure5. As it can be seen in Figure5the Scopes are not translated to Modelica blocks and converters from Modelica signals to Scicos signals are inserted in the links connected to the entry ports of scopes.

Figure 4 .

 4 Figure 4. A Scicos model as displayed by Scicos's editor

  }}; RealInput u[4]; RealOutput y[4](signal(start=xinit[:,1])); equation der(y[1].signal) = u[1].signal; der(y[2].signal) = u[2].signal; der(y[3].signal) = u[3].signal; der(y[4].signal) = u[4].signal; end integral2;

Figure 6 .Figure 7 .

 67 Figure 6. Scicos internal model of a 4-dimensional Modelica to Scicos converter

Figure 8 .

 8 Figure 8. A mixed Scicos-Modelica model of a RLC circuit

From

  such Modelica programs Modelicac generates native, C-based Scicos blocks. It starts by resolving the names appearing in the Modelica description and instantiates required classes (found in libraries) to form the set of all equations governing the dynamics of the Modelica part of the model. It then flattens the structure of the Modelica model, simplifies equations, and generates C code. The following listing is the result of calling Modelicac with previous Modelica code: / * Scicos block's entry point * / void RLC_circuit_test_im( scicos_block * block, int flag) { int * ipar = GetIparPtrs(block); double * rpar = GetRparPtrs(block); double * z = GetDstate( block); double * x = GetState(block); double * xd = GetDerState(block); double * res = GetResState(block); double ** y = GetOutPtrs(block); double ** u = GetInPtrs(block); double ** work= GetPtrWorkPtrs(block); double * g = GetGPtrs(block); double * alpha= NULL; double * beta = NULL;

developed at ENPC. Scicos/Nsp is a Nsp toolbox and runs in the Nsp environment. Having ac- cess to Nsp functions when designing simulation models is of great importance.

  , a Block Diagram Modeler and Simulator

	Scicos (Scicos) is a graphical dynamical system modeler
	and simulator originally developed in the Metalau project
	at INRIA, Paris-Rocquencourt center. With Scicos, users
	can create block diagrams to model and simulate the dy-
	namics of hybrid dynamical systems and compile models
	into executable code. Scicos is used for signal process-
	ing, systems control, queuing systems, and to study phys-
	ical and biological systems. Extensions allow generation
	of component-based modeling of electrical and hydraulic
	circuits using the Modelica language.
	We consider in this paper the Scicos/Nsp version of Sci-
	cos maintained and

  The following listing illustrates what such a description looks like. It contains the Modelica description generated by Scicos for the model of Figure 8:

	model RLC_circuit_test_im parameter Real VA_VsourceAC_(fixed=false) = 2.000000e+00 "VA_VsourceAC_"; parameter Real f_VsourceAC_(fixed=false) = 1.000000e+00 "f_VsourceAC_"; parameter Real R_Resistor_(fixed=false) = 2.000000e-01 "R_Resistor_"; parameter Real C_Capacitor_(fixed=false) = 1.000000e-01 "C_Capacitor_"; parameter Real v_Capacitor_(fixed=false) = 0.000000e+00 "v_Capacitor_"; parameter Real L_Inductor_(fixed=false) = 1.000000e-04 "L_Inductor_"; VsourceAC VsourceAC_(VA=VA_VsourceAC_, f=f_VsourceAC_); Resistor Resistor_(R=R_Resistor_); Capacitor Capacitor_(C=C_Capacitor_, v(start=v_Capacitor_)); Inductor Inductor_(L=L_Inductor_); CurrentSensor CurrentSensor_; Ground Ground_; VoltageSensor VoltageSensor_; OutPutPort OutPutPort_; OutPutPort OutPutPort_1; equation connect (CurrentSensor_.CurrentSensor_.i = OutPutPort_.vi; VoltageSensor_.v = OutPutPort_1.vi; end RLC_circuit_test_im;

n,VoltageSensor_.n); connect (Capacitor_.p,VoltageSensor_.n); connect (Inductor_.p,VoltageSensor_.n); connect (Ground_.p,VsourceAC_.n); connect (CurrentSensor_.p,VsourceAC_.n); connect (VoltageSensor_.p,Resistor_.p); connect (Inductor_.n,Resistor_.p); connect (Capacitor_.n,Resistor_.p); connect (Resistor_.n,VsourceAC_.p);

In case some blocks are unknown to Simport. Indeed, Simulink blocks are black boxes, so Simport cannot translate blocks or combinations of blocks that are not already described in its translation tables.

Acknowledgements

We would like to thank Ramine Nikoukhah from Altair for his considerable help in the design and implementation of the tools we used, in particular Scicos of course, but also Modelicac and Simport.

Working with Acausal Models

Although supported Simulink models are limited to explicit input-output blocks, one should not conclude that our tool chain only deals with "causal" modeling. Indeed, Scicos provides a powerful editor (written in Nsp's language) by means of which imported Simulink models can be graphically connected to Modelica "acausal" models. This allows, for instance, control models written in Simulink to be imported in Nsp and connected to Modelica models (handled by Modelicac). Complete models featuring both "causal" and "acausal" aspects can then be simulated and possibly exported as Modelica code (Figure 1 and Figure 8 show examples of Modelica "acausal" models with control and displays modeled as block-diagrams).

Conclusion and Future Work

In this paper we have shown that NSP can be used as a powerful environment to translate, possibly edit, and simulate some Simulink models. The tool chain comprises two external tools, namely Simport and Modelicac, and the Scicos toolbox with its hybrid Scicos-Modelica library Coselica.

This addition to NSP allows users to simulate many Simulink models with few changes, if any. Moreover, original models can be edited after translation, and possibly connected to native Scicos models, or to Modelica models. This opens many interesting perpectives for users willing to run heterogeneous models at a very high level, using the graphical editor provided by Scicos as the sole GUI.

Several enhancements can be made to this preliminary work. The most significant enhancement would probably consists in enriching the Scicos to Modelica translation table, to allow more Simulink models to be translated automatically.