Riccardo Spica
email: riccardo.spica@irisa.fr

Paolo Robuffo Giordano

Franc ¸ois Chaumette
email: francois.chaumette@irisa.fr

F Chaumette

Plane Estimation by Active Vision from Point Features and Image Moments

In this paper we experimentally validate and compare three different methods for estimating the 3D parameters of a planar scene from a (possibly time-varying) set of feature points acquired by a moving monocular camera. The first method, based on the classical decomposition of the homography matrix, is meant to serve as a baseline condition classically used in many previous works. The other two methods exploit an active Structure from Motion (SfM) scheme for either extracting the plane from the reconstructed 3D position of all the tracked points, or for directly estimating the plane parameters by considering a set of discrete image moments as visual input. The possible loss/gain of point features during the camera motion is considered in all three methods by, in particular, introducing a suitable weighting strategy for the image moment case. Finally, the results of an experimental validation are presented with a comparative discussion of the pros/cons of the three methods.

I. INTRODUCTION

Detection and estimation of 3D planes from the raw visual data acquired by a moving camera is a typical problem faced by, e.g., ground mobile robots or UAVs autonomously navigating in unknown indoor environments. Indeed, in many situations, and especially in artificial environments, the surrounding scene can be reasonably approximated as a collection of planes. This simple modelization is often sufficient for solving tasks such as navigating in a corridor or positioning inside a room [START_REF] Watanabe | Mobile Robot Navigation in Textureless Unknown Environment Based on Plane Estimation by Using Single Omni-Directional Camera[END_REF].

Several methods have been proposed for identifying the 3D plane parameters from an image sequence. A widely used approach exploits the so-called homography constraint, that is, a geometric relationship linking two views of the same planar scene. Some examples in this context can be found in [START_REF] Lin | Robust Ground Plane Detection for Obstacle Avoidance of Mobile Robots Using a Monocular Camera[END_REF], [START_REF] Grabe | Robust Optical-Flow Based Self-Motion Estimation for a Quadrotor UAV[END_REF]. Filtering techniques (such as EKF) can also be exploited to improve the estimation of the homography matrix as proposed in [START_REF] Arróspide | Homographybased ground plane detection using a single on-board camera[END_REF]. Alternative methods use special sensors (e.g., RGB-D cameras) or Structure from Motion (SfM) algorithms to first reconstruct a 3D point cloud of the surrounding environment for then extracting/classifying the planes present in the scene [START_REF] Watanabe | Mobile Robot Navigation in Textureless Unknown Environment Based on Plane Estimation by Using Single Omni-Directional Camera[END_REF], [START_REF] Vaskevicius | Fast plane detection and polygonalization in noisy 3D range images[END_REF]- [START_REF] Borrmann | The 3D Hough Transform for Plane Detection in Point Clouds: A Review and a new Accumulator Design[END_REF].

The goal of this paper is to propose and experimentally validate/compare three different methods for estimating the 3D parameters of a planar scene having as input a (possibly time-varying) set of point features acquired by a moving camera. The first method is based on the classical and well-known decomposition of the homography matrix and is indeed included as a baseline condition to compare the other two methods against. The second and third methods are instead based on the active SfM algorithm proposed in [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF] and tailored to the cases at hand. In particular, the second method exploits the active SfM scheme to first reconstruct the 3D positions of all the tracked points for then searching for the best fitting plane (in a least-square sense) in the resulting point cloud. This technique has already been introduced in [START_REF] Giordano | An Active Strategy for Plane Detection and Estimation for a Monocular Camera[END_REF] but only tested via numerical simulations. In this work we instead provide a full experimental validation of this approach in real conditions. Finally the last method (a novel contribution of this paper) tailors the active SfM scheme [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF] for exploiting a set of image moments of the observed point features as visual input so as to directly yield the estimated plane parameters as output. As shown in the reported results, the advantages of this latter method lie in its higher robustness with respect to non-perfectly planar scenes thanks to its stronger 'filtering' action, as well as in its reduced complexity w.r.t. the second method (3 estimated states for any number of tracked points).

The possibility of losing/gaining point features during the camera motion (e.g., because of the limited camera fov) is also considered in all three methods. While this is easily accomplished for the first two methods, a non-trivial extension is instead needed in the third (moment-based) one, i.e., the introduction of a suitable weighting function in the definition of the image moments for smoothly taking into account new/lost features. This weighting strategy (and the associated weighted moment dynamics) is, to the best of our knowledge, a novel contribution and could also be exploited for dealing with dense and/or photometric image moments in the context of visual servoing [START_REF] Bakthavatchalam | An Improved Modelling Scheme for Photometric Moments with Inclusion of Spatial Weights for Visual Servoing with Partial Appearance/Disappearance[END_REF].

Finally, the proposed machinery is termed active since it optimizes online the camera motion (as only a function of the available measurements) in order to maximize the convergence rate of the plane estimation error and thus obtain a higher estimation accuracy in a shorter time compared to a non-active case. This then additionally differentiates our contribution w.r.t. most of the previous literature which in general assumes a camera moving in a 'non-informed' way, i.e., without attempting to facilitate the plane estimation task during navigation. Finally, we remark that the active SfM approach adopted in this work has in fact several similarities with the notion of "sensor-based" or "ego-centric" Visual SLAM, see [START_REF] Guerreiro | Globally Asymptotically Stable Sensor-Based Simultaneous Localization and Mapping[END_REF] for a recent overview. In both cases, a robot/camera builds a 3D model of the environment in its own body/sensor frame via a filtering technique: a Kalman filter in [START_REF] Guerreiro | Globally Asymptotically Stable Sensor-Based Simultaneous Localization and Mapping[END_REF] and similar works, and the deterministic (but with a fully characterized and actively optimized transient response) filter derived from [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF] in our case.

The paper is organised as follows: in Sect. II we introduce the problem of plane estimation from measured point features and we briefly review the classical homography-based solution used for ground-truth comparison; in Sect. III we summarize the active SfM framework presented in [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF] and describe how to apply it to the case of 3D reconstruction from a collection of feature points and from discrete image moments; in Sect. V we then present an experimental validation of the three proposed methods by discussing the various pros/cons; finally in Sect. VI we draw some conclusions and discuss possible future directions.

II. PROBLEM DESCRIPTION

Let P : n T E + d = 0 be the equation of a planar scene (expressed in the camera frame), with n ∈ S 2 being the unit normal vector and d ∈ R the distance to the plane. Let also P k = (X k , Y k , Z k ) be a collection of N 3D points belonging to P, and

p k = (x k , y k , 1) = (X k /Z k , Y k /Z k , 1)
be the corresponding normalized feature positions measured on the image plane (the camera is assumed calibrated). The problem addressed in this work is how to estimate the 3D plane parameters (n, d) from the tracked N point features p k gathered by a moving monocular camera (with N possibly time-varying). We stress that we are not interested in estimating one particular plane, but rather in identifying the parameters of the plane currently (i.e. within some time interval) dominating the scene observed by the camera.

Method A. Reconstruction from the homography constraint

A classical (and widely-used) possibility to recover the plane parameters (n, d) is to exploit the homography constraint linking two views of the same planar scene [START_REF] Ma | An invitation to 3D vision[END_REF]: in brief, define 0 F C and F C as the camera frames at the beginning of the motion and at the current time, and let 0 p k and p k represent the measured locations of the kth feature point in frames 0 F C and F C . By matching the N feature pairs ( 0 p k , p k ), it is possible to algebraically compute the homography matrix relating the two views, and to then further decompose it via standard techniques for extracting the plane normal n in F C . In order to recover the plane distance d one needs to exploit some additional 'metric' information such as known translation between the two frames or the 3D coordinates of (at least) one of the tracked points P k . Assuming an estimation P k = Ẑk p k is available for all tracked points in F C , one simply has

d = - 1 N N i=1 n T P k . (1)
This first possibility for recovering (n, d) from the homography decomposition will be denoted as method A throughout the rest of the paper. We now discuss two additional methods based on an active SfM framework.

III. PLANE ESTIMATION FROM ACTIVE STRUCTURE FROM MOTION

We start by quickly reviewing the active SfM framework proposed in [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF] and exploited in this work: let s ∈ R m be the set of visual features measured on the image plane, χ ∈ R p a suitable (and locally invertible) function of the unknown structure of the scene to be estimated by the SfM algorithm, and u = (v, ω) ∈ R 6 the camera linear/angular velocity expressed in the camera frame. With these choices, one can show that the SfM dynamics takes the general form

ṡ = f m (s, u) + Ω T (s, v)χ χ = f u (s, χ, u) (2)
where matrix Ω(s, v) ∈ R p×m is a known quantity such that Ω(s, 0) ≡ 0. Let now (ŝ , χ) ∈ R m+p be the estimated state, and define ξ = sŝ as the 'visual feedback' error (measured s vs. estimated ŝ) and z = χχ as the 3D structure estimation error. An estimation scheme for system (2) meant to build a converging estimation χ(t) from the measured s(t) (i.e., such that the estimation error z(t) → 0) can be devised as

ṡ = f m (s, u) + Ω T (s, v) χ + Hξ χ = f u (s, χ, u) + αΩ(s, v)ξ (3)
where H > 0 and α > 0 are suitable gains. We note that the scheme (3) does not require knowledge of ṡ (i.e., measurement of velocities on the image plane), but it only needs measurement of s (the 'visual features') and of (v, ω) (the camera linear/angular velocity in the camera frame). Following [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF], it is possible to characterize the transient response of the SfM estimation error z(t) = χ(t) -χ(t), as well as to affect it by acting online on the camera motion. One can indeed show that the convergence rate of z(t) results dictated by the norm of the square matrix ΩΩ T , in particular by its smallest eigenvalue σ 2 1 . For a given choice of gain α (a free parameter), the larger σ 2 1 the faster the error convergence, with in particular σ 2 1 = 0 if v = 0 (as well-known, only a translating camera can estimate the scene structure). Being Ω = Ω(s, v), one has

(σ 2 1 ) = J v v + J s ṡ, (4)
where the Jacobian matrices J v ∈ R 1×3 and J s ∈ R 1×m have a closed form expression function of (s, v) (known quantities), see [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF]. This relationship can then be inverted w.r.t. vector v for affecting online σ 2 1 (t) during motion, e.g., in order to maximize its value for increasing the convergence rate of z(t). We note that this step represents the active component of the estimation strategy since, in the general case, inversion of (4) will yield a camera velocity v(t) function of the system measured state s(t).

We now apply this general active SfM framework to the problem of structure estimation for a planar scene.

Method B. Reconstruction from 3D points

If the (estimated) 3D points P k are available, a second possibility to recover the pair (n, d) is to directly search for the best fitting plane in F C . By rearranging the plane constraint n

T P k + d = 0, k = 1 . . . N , one has     P T 1 1 . . . . . . P T N 1     n d = A n d = 0 (5)
with A ∈ R N ×4 . Assuming N ≥ 4 and rank(A) = 3, the linear system (5) has a unique solution (up to a scalar factor) for the pair (n, d) which can be found by standard least-square techniques (svd decomposition of matrix A).

As for the issue of optimally recovering the unknown depths Z k for the N tracked point features p k , this can be addressed by exploiting the SfM scheme (3). As discussed in [START_REF] Giordano | An Active Strategy for Plane Detection and Estimation for a Monocular Camera[END_REF], let s = (x 1 , y 1 , . . . , x N , y N ) ∈ R 2N be the vector of measured visual features, and χ = (1/Z 1 , . . . , 1/Z N ) ∈ R N be the 3D structure to be estimated (the depths of all tracked points). This choice results in the matrix

ΩΩ T = diag(σ 2 1,1 , σ 2 1,2 , . . . σ 2 1,N ), (6)
with

σ 2 1,k = (x k v z -v x ) 2 + (y k v z -v y ) 2 (7)
being the eigenvalue determining the convergence speed of the k-th estimation error

z k (t) = χ k (t) -χk (t) = 1/Z k (t) -1/ Ẑk (t)
for the k-th feature point. Exploiting (4), optimization of the convergence of the whole vector z(t) can then be obtained by, e.g., maximizing the minimum eigenvalue

σ 2 m = min k=1...N σ 2 1,k (8)
w.r.t. the camera linear velocity v.

We finally note that this method does not require the exact matching of point features across distant frames (initial and current ones) as it is instead the case for method A, but it only needs a frame-by-frame tracking. As a consequence, the method can straightforwardly cope with loss/gain of feature points because of, e.g., limited field of view: new estimated points P k can be added to system (5) by initializing the corresponding estimated depth Ẑk so as to belong to the current estimation of the plane P. The only assumption (common to all the methods) is that all the tracked points seen by the moving camera belong to a common plane 1 .

In the following, this second possibility for recovering (n, d) will be denoted as method B.

Method C. Plane estimation from image moments

Having summarized methods A and B, we now propose a third novel possibility based on the machinery of pointbased image moments originally introduced in [START_REF] Tahri | Point-Based and Region-Based Image Moments for Visual Servoing of Planar Objects[END_REF]. This method, hereafter denoted as method C, can be seen as a further improvement of method B in that it exploits the active estimation scheme (3) for directly estimating the pair (n, d) (3 independent quantities) instead of the N depths Z k of the N considered point features p k for then algebraically solving system [START_REF] Vaskevicius | Fast plane detection and polygonalization in noisy 3D range images[END_REF]. Thus, the complexity of the SfM scheme results reduced w.r.t. method B as the number of estimated states is independent of the number of tracked points. Furthermore, since (n, d) are directly estimated via a filtering process, one can expect method C to be more robust than method B w.r.t. non perfectly planar scenes as no algebraic step is involved (contrarily to method B that still requires the leastsquare solution of the linear system (5)). Indeed, these considerations are also supported by the experimental results of Sect. V.

Consider then the (i, j)-th moment m ij evaluated on the collection of N observed feature points p k = (x k , y k 1)

m ij = N k=1 x i k y j k .
From [START_REF] Tahri | Point-Based and Region-Based Image Moments for Visual Servoing of Planar Objects[END_REF], the dynamics of m ij takes the expression

ṁij = f ωij (m kl , ω) + f χij (m kl , v)χ (9)
where m kl stands for the generic (k, l)-th moment of order up to i + j + 1, and χ = -n/d ∈ R 3 . Analogous considerations hold for the centered moments

µ ij = N k=1 (x k -x g ) i (y k -y g ) j
with x g = m 10 /m 00 and y g = m 01 /m 00 being the barycenter coordinates, and m 00 = N = const in this case. Furthermore, it is (see, e.g., [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF])

χ = χχ T v -[ω] × χ = f u (χ, u).
The estimation scheme (3) can then be directly applied for recovering χ = -n/d by including in s a suitable collection of m ≥ 3 image moments s = (m i1j1 , . . . , m imjm ), and thus letting

f m = [f ωi 1 j 1 . . . f ωi mjm ] T ∈ R m and Ω = [f T χi1j1 . . . f T χimjm ] ∈ R 3×m . (10)
This estimation strategy, however, lacks the possibility of taking into account the loss/gain of feature points over time (as it is instead the case with method B). When a feature point leaves visibility, a practical workaround could be to just redefine a moment m ij as the sum over the remaining N -1 points and feed the estimation scheme [START_REF] Giordano | An Active Strategy for Plane Detection and Estimation for a Monocular Camera[END_REF] with this new measurement (and analogously for new points entering visibility). However, this would clearly introduce a discontinuity in the measured m ij -a discontinuity not modeled by the dynamics (9) which predicts the moment evolution as only a function of the camera own motion (v, ω). Therefore, we now propose a redefinition of weighted image moments meant to explicitly cope with this issue.

Assume presence of a countable number of feature points on the plane p k = (x k , y k , 1), k = 1 . . . ∞, and define the (i, j)-th weighted moment as where the weighting function w(x, y, τ ) : R 3 → [0, 1] is a sufficiently smooth map, and t k represents the time at which the point feature p k is considered for the first time.

m ij = ∞ k=1 w(x k , y k , t -t k )x i k y j k , (11)
The weight w can be exploited to assign a 'quality' measure to each feature point so as to enforce a smooth change in m ij whenever a tracked feature leaves visibility or a new feature is taken into consideration (regardless of its position on the image plane). In particular, we design the weight w(x, y, τ ) as the product of three scalar functions w(x, y, τ ) = w 1 (x)w 2 (y)w 3 (τ ).

Weights w 1 (x) and w 2 (y) are designed so as to vanish at the image borders and are meant to smoothly take into account features entering/exiting the image plane. Weight w 3 (τ ) is finally intended to smoothly take into account the introduction of a newly detected feature point p k when already within visibility (for instance, when starting to track a new point close to the image center). Figure 1 shows a possible shape for w 1 (x) (also representative of w 2 (y)) and w 3 (τ ).

Exploiting the definition [START_REF] Guerreiro | Globally Asymptotically Stable Sensor-Based Simultaneous Localization and Mapping[END_REF], it is easy (although tedious) to obtain an expression conceptually equivalent to [START_REF] Giordano | An Active Strategy for Plane Detection and Estimation for a Monocular Camera[END_REF] for the dynamics of the weighted moments ṁij . Some details in this sense are reported in the Appendix. This then allows to directly apply the SfM scheme (3) to the case of weighted moments. We finally note that, in practice, the summation ( 11) is clearly evaluated only on the (finite but time-varying) set of currently tracked point features since w(x k , y k , t-t k ) = 0 for any p k not visible or not considered at any time t ≤ t k .

As for which moments to consider for the estimation of χ, after some experimental tests we opted for

s = (x g , y g , µ 20 , µ 02 , µ 11 ) ∈ R m , m = 5. ( 12
)
This choice is partially motivated by [START_REF] Tahri | Point-Based and Region-Based Image Moments for Visual Servoing of Planar Objects[END_REF] which proposed the triple (x g , y g , µ 20 + µ 02 ) as a good set of features for controlling the camera translational dofs in a visual servoing loop. However, we empirically found this latter set to be ill-conditioned for what concerns the estimation of χ, with instead (12) providing enough information (i.e., full rankness of matrix ΩΩ T ) for the estimation convergence 2 . Finally, analogously to the previous case, optimization of the structure estimation convergence from image moments can be achieved by maximizing w.r.t. v the smallest eigenvalue σ 2 1 of the square 3 × 3 matrix ΩΩ T from (10). 2 Alternatively, one could also resort to an adaptive/online selection of the best set of image moments as discussed in [START_REF] Giordano | Learning the Shape of Image Moments for Optimal 3D Structure Estimation[END_REF].

IV. DISCUSSION

Summarizing, although all presented methods are able to solve the plane estimation problem, the latter two have some advantages w.r.t. the (more classical) method A under several aspects. Indeed, as already explained, method B and method C, contrarily to method A, do not require the exact matching of point features across distant frames (initial and current ones), but only a frame-by-frame tracking. It is of course possible to reinitialize method A whenever the number of matched features becomes too small (i.e., by redefining 0 F C as the current camera frame), but this necessarily introduces an erratic transient phase due to the initial limited baseline.

In addition, method B and method C ought to be more robust w.r.t. non perfectly planar scenes because of their inherent filtering nature as compared to method A (a pure algebraic procedure). This is even more true for method C since method B still requires the algebraic resolution of (5). Finally, method C has also the additional advantage over method B of a reduced complexity in terms of number of estimated quantities: three (vector χ) regardless of the number of tracked points, whereas method B needs to estimate N quantities for N tracked point features.

An advantage of method A lies, instead, in its "convergence rate": method A is (in principle) able to yield a good estimation of n as soon as a non-negligible displacement has taken place between the two frames 0 F C and F C , while the accuracy of both method B and method C clearly depends on the convergence rate of the estimation scheme (3).

V. EXPERIMENTAL RESULTS

This section reports some experimental results meant to illustrate and compare the three plane estimation methods introduced in the previous section. The experiments were run by employing a greyscale camera with a resolution of 640 × 480 px and a framerate of 30 fps. The camera was mounted on the end-effector of a 6-dofs Gantry robot commanded in velocity at a frequency of 100 Hz. Image processing and feature tracking were realized via the opensource ViSP library [START_REF] Marchand | ViSP for visual servoing: a generic software platform with a wide class of robot control skills[END_REF]. In the first set of experimets (Sects. V-A and V-B) a simple dotted pattern was used for feature extraction and matching. This solution was meant to reduce as much as possible the variability between each experimental run by ensuring tracking of the very same set of points across all trials. In the last experiment (Sect. V-C) a more realistic scene was considered with the Pyramidal Lucas Kanade feature tracker implemented in OpenCV used for tracking points on the surface of a (planar) topographic map (see Fig. 2). A video of the experiments is also attached to the paper.

As explained, the convergence rate of methods B and C was optimized by actively maximizing the minimum eigenvalue σ 2 m in (8) for method B, and the smallest eigenvalue σ 2 1 of the 3 × 3 matrix ΩΩ T from (10) for method C. Exploiting (4), and recalling that the Jacobian J v can be computed in closed form in both cases, the following update

v v = v v 2 k 1 (κ des -κ) + k 2 I - vv T v 2 J T v ( 13
)
with

κ(t) = 1 2 v(t) 2 , κ des = 1 2 v(t 0 ) 2 , k 1 > 0 and k 2 ≥ 0.
The first term in (13) asymptotically enforces v(t) 2 = v(t 0 ) 2 while the second term projects J T v onto the nullspace of the constraint v = const. Therefore, by means of ( 13) the direction of the camera velocity is optimized so as to maximize the SfM estimation convergence while keeping v(t) = const.

As for the angular velocity ω, it was exploited, in the experiments in Sects. V-A and V-B, to keep the centroid of the tracked point features at the center of the image and, in the experiments in Sect. V-C, to align the camera optical axis with the (estimated) plane normal n. Indeed, we remark that matrix Ω(s, v) in ( 2) does not depend on ω, see, e.g., (6-7) for the point feature case. Thus, when employing the SfM filter (3) for recovering the scene structure, one can freely choose the camera angular velocity without affecting the estimation convergence (which is essentially dictated by the norm of matrix ΩΩ T as discussed in Sect. III).

A. Experiments of plane estimation from 3D points (method B)

We report here the results in estimating the plane parameters (n, d) with method B. The experiment started from an initial guess ( n(t 0 ), d(t 0 )) with an error of 40 deg w.r.t the true n(t 0 ) and a relative error of 50% w.r.t. the true d(t 0 ). The initial depths of all the tracked points p k were initialized so as to force P k (t 0 ) to belong to the estimated plane described by ( n(t 0 ), d(t 0 )). In order to demonstrate the importance of the active camera velocity optimization, we first ran a set of four exeriments starting from the same initial conditions but using different initial camera velocities with the same norm v(t 0 ) = 0.0224m/s. In these experiments we used k 1 = 10 in (13) but we either substituted k 2 J T v with a random acceleration vector (purple dashed line) or we set k 2 = 0, thus keeping a v(t) = v(t 0 ) = const during motion (green, red and cyan dashed lines). Finally we started the experiment again from the same initial camera velocity as in the experiment that performed worst in the previous set (cyan line) and we adopted the update rule [START_REF] Tahri | Point-Based and Region-Based Image Moments for Visual Servoing of Planar Objects[END_REF] with k 1 = 10 and k 2 = 1.

Finally, for the sake of allowing a fair comparison between the convergence rates of method B and method C, we first collected all the data during a first execution of all trajectories, and then ran the two estimation schemes offline on the collected dataset by properly adjusting the estimation gains α B and α C of both methods. Indeed, let

σ2 m = 1 T t0+T t0 σ 2 m (τ )dτ and σ2 1 = 1 T t0+T t0
σ 2 1 (τ )dτ be the average values of the eigenvalues σ 2 m (t) and σ 2 1 (t) during motion in the active estimation cases, with T representing the experiment duration (blue lines in Figs. 3 and4). After having computed σ2 m and σ2 1 during the first run, the estimation gains α B and α C were chosen so as to satisfy α B σ2 m = α C σ2 1 for imposing the same closed-loop dynamics to both method B and method C3 . This resulted in gain α B = 1043.4 (used in these experiments), and in gain α C = 20000 (used in the experiments of the next Sect. V-B).

Fig. 3(a) shows the behavior of the norm of the estimation error z between the real and estimated inverse feature depths, normalized w.r.t. its initial value. The normalization is meant to allow a comparison of this plot with the analogous one in Fig. 4(a). The angle between vectors n(t) and n(t) and the relative error ( d(t) -d(t))/d(t) are also shown in the bottom plots. We can then note how the plane estimation task is solved in all cases (the estimation errors converge towards zero) but, clearly, in the active case (blue line) the error convergence is significantly faster than in the other experiments. This is further evident from Fig. 3(b) where the value of the α B σ m (t) is shown for all experiments (same color code): thanks to the active optimization of the direction of v(t), during the active experiment, α B σ m (t) results approximately 12.5 times larger than in the worst experiment (cyan) which started from the same initial camera velocity. Finally, Fig. 3(c) depicts the camera trajectory in all cases with arrows indicating the direction of the camera optical axis. The green patch represents the location of the plane to be estimated. We encourage the reader to also look at the attached video for better appreciating the effects of the active strategy on the camera trajectory.

B. Experiments of plane estimation from image moments (method C)

In this second set of experiments we show the results of using the weighted discrete image moments for the estimation of the plane parameters. As before the initial guess for χ(t 0 ) has an error of approximately 40 • w.r.t. n(t 0 ) and a relative error of around 50% w.r.t. d(t 0 ). Again, we first ran a set of four exeriments starting from the same initial conditions but using different initial camera velocities with the same norm v(t 0 ) = 0.0206m/s and using [START_REF] Tahri | Point-Based and Region-Based Image Moments for Visual Servoing of Planar Objects[END_REF] with k 1 = 10 and either substituting k 2 J T v with a random acceleration vector (purple dashed line) or setting k 2 = 0, thus keeping a v(t) = v(t 0 ) = const during motion (green, red and cyan dashed lines). Finally, in the active case we started again from the initial camera velocity of the experiment that performed worst in the previous set (cyan line), and we adopted the update rule [START_REF] Tahri | Point-Based and Region-Based Image Moments for Visual Servoing of Planar Objects[END_REF] with k 1 = 10 and k 2 = 1. In all cases, as explained in Sect. V-A, we set α C = 20000.

We show again in Fig. 4(a) the behavior of the normalized norm of the estimation error z, the angle between the actual

0.2 0.4 -0.2 0 0.2 -0.8 -0.6 -0.4 -0.2 0 X [m] Y [m] Z[m] 0.2 0.4 -0.2 0 0.2 -0.8 -0.6 -0.4 -0.2 0 Y [m] X [m] Z[m] ((c))
Fig. 3: Experimental results for the estimation of the plane parameters using 3D points (method B) with an active strategy (blue lines) or a random acceleration (purple line) or a constant linear velocity (green, red and cyan lines): ((a)) normalized norm of the estimation error z, angle between the estimated and actual normal n and relative error between estimated and actual distance d; ((b)) smallest eigenvalue σ 2 m of the N × N matrix ΩΩ T multiplied by αB and linear velocity norm; ((c)) geometric 3d trajectory of the camera with arrows indicating the optical axis and a green patch representing the plane to be estimated. and estimated normal direction n(t) and the relative error on d(t). As evident from the plots, the active strategy results again in a faster convergence of the estimation error, as also clear from Fig. 4(b) where the behavior of α C σ 2 1 (t) is shown for all cases. The trajectory of the camera in the various experiments is finally shown in Fig. 4(c) (and as well in the attached video).

C. Comparison of the three methods A, B and C

This final set of experiments is meant to provide a comparative analysis of the differences between methods B and C against the classical method A taken as a baseline condition.

As already discussed in Sect. IV, the 'convergence' of method A for the estimation of the plane normal direction is, in general, faster w.r.t. the other two methods due to its algebraic nature (no filtering process is present in this case). On the other hand the use of an estimation scheme in method B and method C allows for the possibility of tuning the estimation gain α (a free parameter) against the noise level present in the system (i.e., trading off convergence speed for noise robustness).

In order to test the three methods in a more challenging scenario, we added to the scene a small planar picture with a non-negligible inclination w.r.t. the main plane (see Fig. 5) so as to introduce the presence of some 'outliers' w.r.t. the main dominant plane 4 . The picture was located to be in visibility at the beginning of the experiment and to leave the camera field of view shortly after. The camera linear velocity was optimized via [START_REF] Tahri | Point-Based and Region-Based Image Moments for Visual Servoing of Planar Objects[END_REF] by maximizing the smallest eigenvalue σ 2 1 of the matrix ΩΩ T for the image moment case, and then the same trajectory was used for the other two methods. This resulted in a non-optimal, but still observable, trajectory for method B.

As done in the previous experimental sections, for the sake of obtaining a fair comparison between the convergence rates of method B and method C, we adjusted the estimation gains of both methods in such a way that α B σ2 m = α C σ2 1 , where σ2

m and σ2 1 are the average values of the smallest eigenvalues for the two estimators along the (this time common) trajectory. This resulted in α B = 200 for method B and α C = 26179 for method C.

The behavior of the estimation error on the plane parameters is depicted in Fig. 6(a) for method A (green lines), method B (blue lines) and method C (red lines). In Fig. 6(b) the products α B σ 2 m (t) and α C σ 2 1 (t) are plotted. It can be noticed that in all three cases at the beginning of the experiment (i.e. when the 'outlier' effect of the inclined image over the main planar scene is more present) the error in the estimation of the normal is significant although not diverging. All methods estimate a plane with an intermediate normal direction (as one would expect). Subsequently, the estimation errors for method C and method B start converging toward zero at t ≈ 8s (first dashed vertical line), that is, when the outlier image starts leaving the image plane. However, note how the homography method still yields a very noisy estimation during this phase. Furthermore, once all the outliers are lost (t ≈ 20.3s and second vertical dashed lines in the plots) all the methods yield a converging estimation error. However we can still notice two facts: (i) method C results in the fastest convergence. This is also because the weight w of the outliers starts approaching 0 as they get close to the image border (and thus their disturbing effect is more quickly discarded); (ii) method A has a faster convergence rate w.r.t. method B once all the outliers are lost, but it also yields a noisier estimation until the end of the experiment. In particular one can notice the presence of considerable "jumps" in the estimation of method A due to the reinitialization performed each time the number of matched features falls below a given threshold.

In order to demonstrate the effectiveness of the adopted weighting functions in the computation of the discrete moments, the behavior of m 00 (t) is shown in Fig. 6(b) (bottom plot) . This is meant to illustrate how the number of active points changes over time due to losses at the image border

-0.2 0 X [m] Y [m] Z[m] 0 0.5 1 -0.5 0 0.5 -0.8 -0.6 -0.4 -0.2 0 Y [m] X [m] Z[m] ((c))
Fig. 6: Experimental results for the estimation in presence of outlier measurements using homography decomposition (method A -green lines), 3D points estimation (method B -blue lines) or image moments (method C -red lines): ((a)) angle between the estimated and actual normal n and relative error between estimated and actual distance d; ((b)) product ασ method B and method C and evolution of the image moment m00; ((c)) geometric 3d trajectory of the camera with arrows indicating the optical axis and a green patch representing the plane to be estimated.

or detection of new features. The presence of the weighting strategy function guarantees the desired continuity of m 00 (t) (and similarly of all other image moments not plotted here).

Finally Fig. 7 shows the evolution of the individual switching functions w 1 (x), w 2 (y) and w 3 (τ ), and of their product w = w 1 w 2 w 3 for three representative point features. At t ≈ 3 s the red feature starts leaving the image plane (first along the x direction and then along the y direction) and its total weight goes to zero by the action of both w 1 (x) and w 2 (y). After the feature has completely left the plane, the tracker detects a new feature (the blue one) at t ≈ 12 s. Being far from the image border, it is smoothly taken into account thanks to the effect of weight w 3 (t) (note the zoomed views on the right side of the plots where the smooth rise of w 3 (t) can be seen). Finally, the green feature is close to the border of the image at the time of detection. In this case, even if weight w 3 (t) is rising towards 1, the total wight of the feature is kept small by the action of w 2 (y).

VI. CONCLUSIONS

In this paper we presented and critically compared via several experiments three methods for estimating the 3D parameters (n, d) of a plane from a set of (possibly timevarying) point features tracked by a moving camera. The first one (the baseline method) is based on the classical decomposition of the homography constraint, while the other two methods exploit an active SfM algorithm for estimating the 3D structure of the scene: the depths of the tracked point features for then extracting (n, d) in one case, and directly the plane parameters (n, d) from the measured image moments in the other case. In both these latter methods an active strategy is also presented for controlling online the camera motion in order to optimize the estimation convergence speed. Furthermore, the possible loss/gain of point features over time because of the limited camera fov is considered in all three methods. For the third (moment-based) case this required the extension to the case of weighted image moments to smoothly take into account new/lost features.

The reported experiments confirmed the effectiveness of these methods in estimating (n, d) in real conditions, also in the (intentional) presence of some outliers w.r.t. the main dominant plane. In particular, the third (moment-based) method resulted the most robust against outliers because of its better filtering capabilities (as expected). Motivated by these findings, we are currently investigating the possibility of extending our results to the case of dense photometric moments. Moreover it would also be interesting to combine the proposed methods with some (RANSAC-based) classification technique for outlier rejection and plane clustering so as to address the issue of estimating the parameters of multiple planes at the same time.

Fig. 1 :

 1 Fig. 1: Left: shape of weight w1(x) with limits xmin = -xmax = 0.2884 (normalized size of the image plane). Right: shape of weight w3(τ ).

Fig. 2 :

 2 Fig. 2: Experimental set-up with the dotted pattern and the topographic map used for feature extraction and tracking.rule was implemented for the camera linear velocity v

Fig. 4 :

 4 Fig.4: Experimental results for the estimation of the plane parameters using image moments (method C) with an active strategy (blue lines) or a random acceleration (purple line) or a constant linear velocity (green, red and cyan lines): ((a)) normalized norm of the estimation error z, angle between the estimated and actual normal n and relative error between estimated and actual distance d; ((b)) smallest eigenvalue σ 2 1 of the 3 × 3 matrix ΩΩ T multiplied by αC and linear velocity norm; ((c)) geometric 3d trajectory of the camera with arrows indicating the optical axis and a green patch representing the plane to be estimated.

Fig. 5 :

 5 Fig. 5: Experimental setup for the estimation of the plane parameters in presence of outlier measurements. Note the introduction of the inclined picture in the observed scene on the right.

4 Fig. 7 :

 47 Fig. 7: Evolution of the switching funtions and of their product for three representative point features. On the right: detailed views of the corresponding plots on the left in the time interval immediately following the introduction of the blue feature in the estimator.

The results of Sect. V will nevertheless test the robustness of the methods against this hypothesis.

As explained in[START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF], the convergence rate of the SfM scheme (3) is actually dictated by the smallest eigenvalue of ΩΩ T times the chosen estimation gain α.

Of course one could utilize a RANSAC-based classification (exploiting the homography constraint) for preliminarily segmenting the two planes so as to only consider the points belonging to the main dominant plane for the estimation task. However, in a real situation, the accuracy of any classification method can never be perfect and some outliers will fail to be detected. Therefore, in this experiment we intentionally decided to not include any preliminary RANSAC-based pruning in order to just assess the "intrinsic" robustness of the proposed algorithms (which would clearly be improved by any preliminary outlier rejection step).

APPENDIX Let

and χ = n/d = (A, B, C). By leveraging on the developments of [START_REF] Tahri | Point-Based and Region-Based Image Moments for Visual Servoing of Planar Objects[END_REF], the dynamics of the (i, j)-th weighted moment ( 11) is given by

with

We can note that the dynamics [START_REF] Marchand | ViSP for visual servoing: a generic software platform with a wide class of robot control skills[END_REF] involves moments of order up to (i + j + 2) associated to the terms m x ij and m y ij . Also, it is easy to check that in the unweighted case (w ≡ 1 and m x ij = m y ij = m t ij ≡ 0), one obviously recovers the classical moment dynamics reported in [START_REF] Tahri | Point-Based and Region-Based Image Moments for Visual Servoing of Planar Objects[END_REF].


Nicolas Cazy
email: nicolas.cazy@inria.fr

Pierre-Brice Wieber
email: pierre-brice.wieber@inria.fr

Paolo Robuffo Giordano

Franc ¸ois Chaumette
email: francois.chaumette@inria.fr

P.-B Wieber

F Chaumette

Visual Servoing when Visual Information is Missing: Experimental Comparison of Visual Feature Prediction Schemes

One way to deal with occlusions or loss of tracking of the visual features used for visual servoing tasks is to predict the feature behavior in the image plane when the measurements are missing. Different prediction and correction methods have already been proposed in the literature. The purpose of this paper is to compare and experimentally validate some of these methods for eye-in-hand and eye-to-hand configurations. In particular, we show that a correction based both on the image and the camera/target pose provides the best results.

I. INTRODUCTION

In the visual servoing literature (see [START_REF] Chaumette | Visual servo control, part i: Basic approaches[END_REF] for a tutorial) there exist two main approaches for dealing with occlusions and/or loss of feature tracking due to the limited camera field of view. In the first approach, one tries to develop control schemes for avoiding that an occlusion or loss of tracking never occurs while the second approach proposes strategies for coping with the possibility that such events may occur.

On the control side, many techniques have been considered: occlusions avoidance is expressed as a secondary task combined with a classical visual task using the gradient projection method in [START_REF] Marchand | Dynamic sensor planning in visual servoing[END_REF]. This method has also been used in [START_REF] Folio | A controller to avoid both occlusions and obstacles during a vision-based navigation task in a cluttered environment[END_REF], combined with sequencing tasks to also consider obstacle avoidance. Avoiding that features leave the camera field of view is explicitly taken into account in the visual features design in [START_REF] Morel | Dynamic sensor planning in visual servoing[END_REF], while switching control methods have been proposed in [START_REF] Chesi | Keeping features in the field of view in eye-in-hand visual servoing: a switching approach[END_REF], [START_REF] Gans | Stable visual servoing through hybrid switched-system control[END_REF]. Visual trajectory planning taking into account several constraints such as occlusions and feature loss has led to a series of works [START_REF] Mezouar | Path planning for robust image-based control[END_REF]- [START_REF] Kazemi | Randomized kinodynamic planning for robust visual servoing[END_REF]. More recently, model predictive control has been applied to visual servoing since this approach allows an easy handling of hard constraints [START_REF] Murao | Predictive visual feedback control with eye-in-hand system via stabilizing receding horizon approach[END_REF]- [START_REF] Assa | Robust model predictive control for visual servoing[END_REF]. Finally, another method consisting in integrating the constraints in the feature set used as input of the control scheme has been proposed in [START_REF] Kermorgant | Dealing with constraints in sensorbased robot control[END_REF].

On the other side, a visual servoing task can still be successful in case of occlusions or feature loss provided a sufficient number of features remains visible during motion (so that the rank of the corresponding Jacobian matrix can still match the number of controlled degrees of freedom). The most simple solution is thus to just remove from the feature set those no longer available. However, a change in the feature set implies a change in the number of rows of the Jacobian matrix, and thus a discontinuity in the output of the control scheme. To solve this problem, a continuous weight can be associated to each feature as proposed in [START_REF] Garcia-Aracil | Continuous visual servoing despite the changes of visibility in image features[END_REF]: this weight varies between 1, when the feature is located near the center of the image, to 0 when the feature reaches the image limit. This then ensures continuity of the output of the control scheme. This strategy is of course no more efficient when the number of visible features becomes too small and cannot ensure full rankness of the Jacobian matrix. A general solution, explored in [START_REF] Folio | Dealing with visual features loss during a vision-based task for a mobile robot[END_REF], is thus to predict the location of the features when they are no more visible, and to then use this prediction in the control scheme as well as for guiding the feature re-tracking when it becomes visible again. This is also the strategy considered in our previous work [START_REF] Cazy | Pose Error Correction For Visual Features Prediction[END_REF] and in this paper.

The purpose of this paper is to validate experimentally the conclusions of this previous work, which were validated only in simulation using a priori given camera motions while visual servoing is considered here. Section II begins with reviewing the prediction and correction methods previously discussed in [START_REF] Cazy | Pose Error Correction For Visual Features Prediction[END_REF]. Section III recalls the basic visual servoing equations for both eye-in-hand and eye-to-hand configurations. Section IV presents a number of experimental results aiming at comparing the different methods available. Finally, Section V concludes the paper.

II. VISUAL FEATURES MODEL ERROR

CORRECTION In this paper, we consider a monocular perspective camera and a target object made of N 3D points P i with F C and F O two Cartesian frames attached respectively to the camera and target. We also denote with x = (x, y, z, θu) ∈ R 6 the pose of F O with respect to F C , with (u, θ) ∈ S 2 × R the axis/angle parameterization of the orientation1 , and with v(t) = (v(t), ω(t)) ∈ R 6 the linear and angular velocity of the camera, expressed in the camera frame.

Let

O P i = ( O X i , O Y i , O Z i ) and C P i = ( C X i , C Y i , C Z i )
be the coordinates of the 3D points P i in the object and camera frames. Assuming a calibrated camera, the normalized perspective 2-D projection of each point P i on the image plane is

s i = u i v i = C X i / C Z i C Y i / C Z i = f si (x), (1)
where the dependence on the camera/target relative pose x has been emphasized. The set of all visual features is therefore

s =    s 1 . . . s N    = f s (x) ∈ R 2N . (2)
We finally let x m (t) represent a model/approximation of the true target pose, with s m (t) = f s (x m (t)) the corresponding model of s(t), derived from x m (t) via (2).

In the following, we assume that the vector s(t) can be measured until some t = t occ after which some visual features or the full set s(t) become unavailable for a time period T > 0 because of some external 'event' such as, e.g., an occlusion, loss of tracking because of the limited camera field of view (fov), or a generic error of the image processing step. The following sections discuss then two possible strategies for (i) propagating the internal models s m (t) and x m (t) during t ∈ [t occ , t occ + T ], exploiting the known linear and angular velocity v(t) = (v(t), ω(t)) of the camera, and for (ii) correcting s m (t) and x m (t) as long as the measurements s(t) are available (thus for t < t occ and t ≥ t occ + T ).

A. Model prediction

We assume the time interval [t occ , t occ + T ] is discretized in H uniform steps (corresponding to the number of camera frames without visual feature measurement) of duration τ = T /H. We also set v

(t) = v(t k ) = const for t ∈ [t k , t k+1 ], with k = 1 . . . H.
1) Prediction in the image: the prediction in the image proposed in [START_REF] Allibert | Predictive control for constrained image-based visual servoing[END_REF], [START_REF] Cazy | Pose Error Correction For Visual Features Prediction[END_REF] is a simple forward integration of s m (t) via the interaction matrix L s given by [START_REF] Chaumette | Visual servo control, part i: Basic approaches[END_REF]

L s =   -1 Z 0 u Z uv -(1 + u 2 ) v 0 -1 Z v Z 1 + v 2 -uv -u   . (3)
More precisely, we have

s m (t k ) = s m (t k-1 ) + τ L s (s m (t k-1 ), Z m (t k-1 ))v(t k-1 ), (4) with
Z m = [Z m1 , . . . , Z m N ]
a model of the depths associated to the N feature points. The vector Z m (t) can also be updated from an initial Z m (t occ ) with

Z m (t k ) = Z m (t k-1 )+τ L Z (s m (t k-1 ), Z m (t k-1 ))v(t k-1 ), (5) with
L Z (s(t k-1 ), Z(t k-1 )) =    L Z1 . . . L Z N    (6)
and

L Zi (s i (t k-1 ), Z i (t k-1 )) = 0 0 -1 -v i Z i u i Z i 0 . (7
) By exploiting ( 4) and ( 5), the behavior of the feature positions s m (t) and depths Z m (t) can be predicted during the interval [t occ , t occ + T ] without requiring an explicit knowledge of the camera/target relative pose x(t) nor the 3D model of the target. Obviously the accuracy of this prediction scheme depends on the accuracy of the initial values s m (t occ ) and Z m (t occ ).

Note that if only camera rotational motions are involved in the control scheme, such as for gaze control where a pan-tilt camera is used to track a moving target, the depth does not come anymore into the interaction matrix (Z only appears in the first three colomns corresponding to the three translational motions), which makes useless the prediction Z m (t).

2) Prediction in the pose: an alternative is to propagate instead the model x m (t) of the target pose w.r.t. the camera, with the kinematic relationship

x m (t k ) = x m (t k-1 ) + τ L x v(t k-1 ), (8)
where

L x = O R C (u, θ) 0 0 L ω (u, θ) with O R C (u, θ) = I 3 + [u] × sin(θ) + [u] 2 × (1 -cos(θ)) is the rotation matrix from F C to F O , and L ω (u, θ) = I 3 - θ 2 [u] × + 1 - sinc(θ) sinc 2 (θ/2) [u] 2 × .
and sinc(θ) = sin(θ)/θ, see [START_REF] Malis | 2 1/2 D Visual Servoing[END_REF]. Since det(L ω (u, θ)) = 1/sinc 2 (θ/2), matrix L x is invertible for θ = 2kπ, k ∈ Z * , i.e., within the workspace of any normal application. Combined with [START_REF] Marchand | Dynamic sensor planning in visual servoing[END_REF], we obtain a model s m (t) = f s (x m (t)) of the features in the image during the interval [t occ , t occ +T ]. The difference with the previous prediction scheme is that the evolution of the camera/target pose x m (t) is continuously tracked here while only s m (t) and Z m (t) are tracked with (4) and [START_REF] Chesi | Keeping features in the field of view in eye-in-hand visual servoing: a switching approach[END_REF]. As before, the accuracy of this prediction scheme depends on the accuracy of the initial value x m (t occ ).

B. Model correction

When measures s(t) are available, they can be exploited to correct the features and the pose models s m (t) and x m (t) in the following ways.

1) Correction in the image: this correction scheme, proposed in [START_REF] Allibert | Predictive control for constrained image-based visual servoing[END_REF], [START_REF] Cazy | Pose Error Correction For Visual Features Prediction[END_REF], aims at correcting the model s m (t k ) by exploiting the measures s(t k ) as follows:

           s m (t k ) ← s(t k ), C Z m (t k ) ← C Z m (t k ), C X mi (t k ) ← u mi (t k ) C Z mi (t k ), C Y mi (t k ) ← v mi (t k ) C Z mi (t k ). (9)
This clearly ensures that s m (t occ ) = s(t occ ) and s m (t occ + T ) = s(t occ + T ) when the occlusion begins and ends, but it fails in correcting any error in the depth model Z m (t occ ) and Z m (t occ + T ) since it only accounts for the error in the image, without trying to correct any potential discrepancy between the real and the model poses x(t) and x m (t). As a result, the model s m (t) will generally diverge from the real values s(t) for t > t occ .

2) Correction in the image and in the pose: this improved correction scheme additionally exploits the mismatch between the model s m (t k ) = f s (x m (t k )) and the measures s(t k ) to compute a pose correction

∆x(t k ) = L x L † s (s(t k ) -s m (t k )), (10)
with L † s the Moore-Penrose inverse of L s (s m (t), x m (t)) (see [START_REF] Cazy | Pose Error Correction For Visual Features Prediction[END_REF] for more details):

                 x m (t k ) ← x m (t k ) ⊕ ∆x(t k ), s m (t k ) ← s(t k ), C Z m (t k ) ← f Z (x m (t k )), C X mi (t k ) ← u mi (t k ) C Z mi (t k ), C Y mi (t k ) ← v mi (t k ) C Z mi (t k ). (11)
Here, f Z (•) provides depths Z obtained directly from the camera/target pose model and point 3D coordinates.

Note that the pose correction scheme corresponds to a single iteration of a standard pose estimation by virtual visual servoing [START_REF] Marchand | Virtual visual servoing: a framework for real-time augmented reality[END_REF]. More iterations could be realized if necessary. Note also that because of the Moore-Penrose inverse property

L † s L s L † s = L † s , (12)
one has

L † s s m (t) ≈ L † s s(t) (13)
(see again [START_REF] Cazy | Pose Error Correction For Visual Features Prediction[END_REF] for more details). Therefore, a typical visual servoing control law (see Section III just below) v = -λL † s (s(t) -s * ) ≈ -λL † s (s m (t) -s * ) will yield similar camera velocities when using either s(t) or s m (t). Classical image-based visual servoing will thus not be perturbed by this pose correction method. This is of particular interest for t > t occ when s(t) is no more available.

III. IMAGE-BASED VISUAL SERVOING

An IBVS control strategy consists in finding the camera linear and angular velocity v that will minimize the norm of an error function defined on the image plane e = ss * , where s * represents the desired location of the visual features [START_REF] Chaumette | Visual servo control, part i: Basic approaches[END_REF]. By exploiting the relationship between the time variations of s and v, one has ṡ = ė = L s v.

A. Eye-In-Hand configuration

If we consider a camera mounted on a robot arm, one can implement the following control law

v = -λ L s † (s -s * ), (14)
where L s is an estimation of L s , for driving the error e to zero.

B. Eye-To-Hand configuration

In order to illustrate how the proposed correction can be applied to different scenarios, we also consider an Eye-To-Hand system in which a static camera is observing a target fixed to the end effector of the robot arm, see [START_REF] Horaud | Visually guided object grasping[END_REF]. The resulting control law is slightly different from [START_REF] Allibert | Real-time visual predictive controller for image-based trajectory tracking of mobile robot[END_REF] and results in

v e = λ e V c L s † (s -s * ), (15)
where v e is the end effector velocity expressed in the end effector frame and e V c is the velocity twist matrix given by

e V c = e R c [ e t c ] × e R c 0 e R c .
Note that the measured robot velocity has to be expressed in the camera frame for using the prediction schemes (4), ( 5) and ( 8). This is simply obtained by:

v = c V e v e . (16)
The main difficulty of the eye-to-hand configuration appears in the non-exact knowledge of the end effector pose in the camera frame, which is involved in c V e in [START_REF] Assa | Robust model predictive control for visual servoing[END_REF]. Indeed, even if the camera/target pose model x m is perfectly corrected, the target/effector pose has to be perfecly calibrated to avoid dissimilarities between real and predicted movements.

In the following experiments and for both configurations previously described, we have chosen L s = L s (s * , Z * ), that is, the value of the interaction matrix computed at the desired pose, to demonstrate that, even if the depth Z(t) is not involved in the control scheme, its use in the predicion step is crucial.

IV. EXPERIMENTAL RESULTS

This section presents results obtained when applying the corrections discussed in Section II to a standard Image-Based Visual Servoing (IBVS) task in presence of occlusions or loss of tracking of the controlled visual features.

The first two following experiments were realized with an eye-in-hand monocular camera mounted on a 6 dof robot arm. The target consists of four coplanar points forming a square of about 10 cm in length with coordinates in . The software ViSP [START_REF] Marchand | ViSP for visual servoing: a generic software platform with a wide class of robot control skills[END_REF] was used for image processing and visual tracking of 2D point coordinates.

F 0          O P 0 = (-0.
The pose of F 0 w.r.t. F C at the beginning of the task was

x(t 0 ) = (-0.072, -0.035, 1.21, -0.35, 0.34, 0.050) T while the initial model of the pose was taken intentionally incorrect to highlight the value of the correction [START_REF] Murao | Predictive visual feedback control with eye-in-hand system via stabilizing receding horizon approach[END_REF], such as x m (t 0 ) = (0, 0, 2, 0, 0, 0.79) T .

This was then used to compute the initial values of the model s m (t 0 ) = f s (x m (t 0 )) of visual features via (2) as well as C Z m (t 0 ) = f Z (x m (t 0 )). During the servoing, we also simulated a camera field of view (fov) smaller than the real one for exacerbating the loss of tracking during motion.

A. Reference behavior

Figure 1a shows the initial camera image before the control law ( 14) is applied. Green crosses represent the desired locations s * for the visual features, and red crosses their current (measured) location s(t). This initial experiment does not include any occlusion or loss of tracking and is only meant to provide a reference baseline for comparison (see the first part of the video submitted as supplementary material). The solid red curves in Figure 1b represent the paths taken by each point in the image during the camera motion, while Figure 1c shows the evolution of the error e i (t) = s i (t) -s * i (t) between the current and desired locations, eventually converging to zero.

B. Limited field of view

Let us now consider a case where the visual features leave the field of view of the camera, which is artificially limited to a small section of the real view obtained from the camera, represented in the following figures by a blue box. Figures 2a and3a show the camera image at the end of the servoing loop, when using respectively the correction (9) and the correction [START_REF] Murao | Predictive visual feedback control with eye-in-hand system via stabilizing receding horizon approach[END_REF]. Green curves represent the paths taken by the features when measurements are available, while blue curves represent their predicted paths computed thanks to (8) when measurements are not available.

As expected, the non-corrected pose x m (t) in ( 9) results in a significant drift of the prediction in the Fig. 2a, while the correction [START_REF] Murao | Predictive visual feedback control with eye-in-hand system via stabilizing receding horizon approach[END_REF] leads to an almost perfect prediction in the Fig. 3a. As a result, the camera converges to an incorrect pose in the first case (see the second part of the video submitted as supplementary material), while converging to the desired pose in the second case (see the third part of the video). Note how the image processing is able to recover the tracking of the visual features when they re-enter the camera view, by exploiting their predicted location.

Figures 2b and3b show the corresponding errors e i (t), and Figures 2c and3c the error in estimating the depths Z i (t). In each plot, the blue parts of the curves correspond to occlusion phases. We can verify that the depth estimations converge to the real values when applying the correction [START_REF] Murao | Predictive visual feedback control with eye-in-hand system via stabilizing receding horizon approach[END_REF], but naturally not when applying the correction [START_REF] Shen | Visual servoing path planning for cameras obeying the unified model[END_REF].

As additional experimentation the fourth part presented in the video submitted as supplementary material leads to the same conclusion as the one with the pose and image correction but with a longer occlusion caused by visual features leaving the camera field of view and hidden by a sheet of paper.

C. Eye-To-Hand configuration

This last experiment was realized with an eye-to-hand monocular camera observing a target mounted on the end effector of a 6 dof robot arm which has to grasp a cube of 4 cm for each side. The pose of the cube in the camera frame is computed at the beginning of the experimentation, and is used to generate desired visual features of the target in the camera image plane. The target is composed by [START_REF] Morel | Dynamic sensor planning in visual servoing[END_REF] The pose of the target in the camera frame at the beginning of the task was x(t 0 ) = (0.307, -0.522, 0.753, -1.155, -0.776, -0.977) T while the initial model of the pose was taken as x m (t 0 ) = (0, 0, 1, 0, 0, 0) T .

The visual servoing has to realize 2 steps using the control law [START_REF] Allibert | Predictive control for constrained image-based visual servoing[END_REF]. The first step consists in moving the grasp above the cube to avoid any collision, and the second step consists in moving the arm towards the cube for grasping it as shown on Figure 4a. Once again, the field of view of the camera is artificially limited to a small section of the real view obtained from the camera, which causes loss of measurements replaced by prediction computed thanks to (4), [START_REF] Chesi | Keeping features in the field of view in eye-in-hand visual servoing: a switching approach[END_REF].

Figures 4b and4c show respectively the camera image at the end of the servoing loops and the correponding errors e i (t). This demonstrates the efficiency of [START_REF] Murao | Predictive visual feedback control with eye-in-hand system via stabilizing receding horizon approach[END_REF] for an Eye-To-Hand system (see the last part of the video submitted as supplementary material). Indeed, the two steps required to grasp the cube are realized even when visual features leave the camera field of view during the first step. We can note a slight difference between real and predicted visual features at the end of the servoing loop because of the non-exact knowledge of the effector/target pose.

V. CONCLUSION

In this paper, we applied the prediction/correction scheme recently proposed in [START_REF] Cazy | Pose Error Correction For Visual Features Prediction[END_REF] to a classical image-based visual servoing task in order to cope with possible occlusions or loss of feature tracking due to the limited camera field of view. To this end, we reported several experimental results (for both an eye-in-hand and eye-to-hand configurations) that showed the effectiveness of the proposed approach. Indeed, thanks to the prediction/correction scheme [START_REF] Cazy | Pose Error Correction For Visual Features Prediction[END_REF], the servoing task is shown to gain a high level of robustness against occlusions/loss of tracking also during extended periods of time. Future works will investigate how to exploit the proposed machinery in the context of Model Predictive Control. c) is the evolution of Z mi (t) -Z i (t) . Note how, compared to Fig. 2, convergence towards the desired pose is now correctly achieved thanks to the superior performance of the correction [START_REF] Murao | Predictive visual feedback control with eye-in-hand system via stabilizing receding horizon approach[END_REF], that allows for a more accurate prediction of s m (t) when no measurement is available.

  05, -0.05, 0) O P 1 = (0.05, -0.05, 0) O P 2 = (0.05, 0.05, 0) O P 3 = (-0.05, 0.05, 0)

O

  P 0 = (0, 0.023, -0.134)O P 1 = (0, 0.023, -0.247) O P 2 = (-0.057, 0, -0.132) O P 3 = (-0.057, -0.113, -0.13) .

Fig. 1 :

 1 Fig. 1: Reference trajectory: (a) is the intial state where the locations of desired features s * on the camera image plane are in green and the locations of measured features s(t 0 ) are in red. (b) is the final state where the measured trajectory followed by each feature for reaching the green desired values are in red. (c) is the behavior of the error e i (t) = ||s i (t) -s * i (t)|| for each visual feature error during the servoing.

Fig. 2 :

 2 Fig. 2: Limited field of view with correction (9): (a) is the final state where the green curves represent the measured path of the visual features while the blue curves represent the predicted path (when no measurement is available). (b) is the evolution of the error e i (t) = ||s i (t) -s * i || (red parts) and of e i (t) = ||s m (t) -s * i || (blue parts). (c) is the evolution of Z mi (t) -Z i (t) . Because of the poor performance of the correction (9), the predicted paths substantially diverge from the expected ones (in red), causing a complete failure of the servoing loop.

Fig. 3 :

 3 Fig. 3: Limited field of view with correction (11): (a) is the final state. (b) is the evolution of the error e i (t) = ||s i (t) -s * i || (red parts) and of e i (t) = ||s m (t) -s * i || (blue parts). (c) is the evolution of Z mi (t) -Z i (t) . Note how, compared to Fig.2, convergence towards the desired pose is now correctly achieved thanks to the superior performance of the correction[START_REF] Murao | Predictive visual feedback control with eye-in-hand system via stabilizing receding horizon approach[END_REF], that allows for a more accurate prediction of s m (t) when no measurement is available.

Fig. 4 :

 4 Fig. 4: Limited field of view with correction (11) applied to the Eye-To-Hand system. (a) is the initial state where desired visual features are in green for step 1 and in purple for step 2. (b) is the final state. (c) is the evolution of the error e i (t) = ||s i (t) -s * i || (red parts) and of e i (t) = ||s m (t) -s * i || (blue parts). The 2 servoing tasks are correctly realized. This again confirms the effectiveness of correction (11) what allows quickly recovering any initial error in the camera/target model pose for then allowing an accurate prediction s m (t) during the occlusion phase.

Among all the possible minimal representations for the orientation between F O and F C , we chose the vector θu ∈ R 3 because its singularities lie at θ =

2kπ, k ∈ Z * , i.e., out of the possible workspace in any normal application (see[START_REF] Malis | 2 1/2 D Visual Servoing[END_REF]).

ACKNOWLEDGMENTS

This work was partly funded by the Oseo Romeo 2 Project.