Riccardo Spica
email: riccardo.spica@irisa.fr

Paolo Robuffo Giordano

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

F Chaumette

Active Structure from Motion for Spherical and Cylindrical Targets

Structure estimation from motion (SfM) is a classical and well-studied problem in computer and robot vision, and many solutions have been proposed to treat it as a recursive filtering/estimation task. However, the issue of actively optimizing the transient response of the SfM estimation error has not received a comparable attention. In this paper, we provide an experimental validation of a recently proposed nonlinear active SfM strategy via two concrete applications: 3D structure estimation for a spherical and a cylindrical target. The experimental results fully support the theoretical analysis and clearly show the benefits of the proposed active strategy. Indeed, by suitably acting on the camera motion and estimation gains, it is possible to assign the error transient response and make it equivalent to that of a reference linear second-order system with desired poles.

I. INTRODUCTION

The problem of Structure from Motion (SfM), i.e., how to reconstruct a 3D observed scene from images taken by a moving camera, is a very classical and well-studied topic in computer and robot vision. When acquiring consecutive images over time, a possibility is to treat SfM as a recursive/filtering process: images and camera motion can be elaborated online for obtaining an incremental estimation of the scene structure. Other approaches (e.g., bundle adjustment) rely instead on global/offline optimization methods meant to solve SfM problems by processing altogether information acquired over an extended time period. A recent discussion about the pros/cons of both approaches in the context of Visual SLAM can be found in [START_REF] Strasdat | Visual SLAM: Why filter?[END_REF]. Furthermore, within the first class of (online) methods, a vast literature exists for addressing SfM: for instance, as a non-exhaustive list, Extended Kalman Filter-based solutions have been proposed in [START_REF] Matthies | Kalman filter-based algorithms for estimating depth from image sequences[END_REF]- [START_REF] Civera | 1-Point RANSAC for EKF Filtering: Application to Real-Time Structure from Motion and Visual Odometry[END_REF], and other approaches exploiting techniques from (deterministic) nonlinear observation can be found in [START_REF] Dixon | Range Identification for Perspective Vision Systems[END_REF]- [START_REF] Martinelli | Vision and IMU data fusion: Closed-form solutions for attitude, speed, absolute scale, and bias determination[END_REF] and references therein. Finally, in [START_REF] Guerreiro | Globally Asymptotically Stable Sensor-Based Simultaneous Localization and Mapping[END_REF] the authors nicely discuss the advantages of a sensor-centered recursive SLAM algorithm sharing the same theoretical setting of what presented in this work.

Much less attention has instead been devoted to the problem of actively optimizing the convergence rate of a SfM estimation task by acting on the motion imposed to the camera and on the employed estimation gains. For instance, in [START_REF] Chaumette | Structure from controlled motion[END_REF] an active strategy for minimizing the effects of image noise and discretization errors was proposed and experimentally tested, but without the aim of also imposing a desired estimation transient response. In [START_REF] Davison | Mobile Robot Localisation Using Active Vision[END_REF], the problem of actively selecting which features to track for improving the indoor localization of a wheeled mobile robot is successfully addressed: however, no attempt is made to actively shape the robot motion so as to optimize the SfM convergence (the robot navigates in an 'uninformed' way w.r.t. the estimation task).

With respect to these and other prior works, in [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF] we have recently proposed a general framework for designing a class of nonlinear observers (which includes SfM as particular case) with an estimation error transient response that can be (i) explicitly characterized and (ii) actively shaped so as to match that of a reference linear second-order system with assigned poles. This is achieved by simultaneously acting on the estimation gains and on the system inputs (i.e., the camera linear velocity in the SfM case).

The methodology proposed in [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF] applies to all those problems in which an invertible function of the unknown states can appear linearly in the system dynamics, as it is indeed the case for SfM. In this paper, we exploit this fact and consider a concrete application involving SfM for two 3D objects, namely, a spherical and a cylindrical target. A first contribution of our work is the development of two novel minimal parameterizations of the sphere and cylinder 3D geometries which allow to reduce the SfM problem to the estimation of the sole sphere/cylinder radius, with all the remaining quantities obtained from image measurements and camera velocities. A second contribution is the reported successful experimental validation which (i) results in excellent agreement with the developed theoretical analysis and (ii) demonstrates the feasibility of our approach in real conditions.

We finally note that the ability to both characterize and optimize the transient behavior of SfM problems brings a significant added value compared to more classical 'inactive' estimation strategies: for instance, it allows to obtain the 'best' estimation error convergence when subject to realworld constraints such as limited camera velocity or upper bounds on the estimation gains due to noise, discretization, or other typical non-idealities. As shown in [START_REF] Spica | Coupling Image-Based Visual Servoing with Active Structure from Motion[END_REF], one can also embed the active SfM strategy within the execution of typical image-based visual servoing tasks. Furthermore, from a more theoretical perspective, the proposed methodology can also be used to get insights into the 'optimal' camera trajectories needed to estimate the scene structure for particular classes of SfM problems (e.g., when dealing with points features, planar objects or specific 3D geometrical primitives). We believe the reported results well support these considerations.

The rest of the paper is organized as follows: Sec. II reviews the SfM problem in the context of nonlinear state observation and briefly summarizes the methodology developed in [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF] for actively imposing a desired transient response to the estimation error. Section III then focuses on the two SfM problems considered in this work, i.e., radius estimation for spherical and cylindrical targets. Subsequently, Sec. IV reports the corresponding experimental results obtained with a manipulator equipped with an eye-in-hand camera. Finally, Sec. V concludes the paper and discusses some future directions.

II. AN ACTIVE ESTIMATION STRATEGY

Let s ∈ R m be the set of visual features measured on the image plane of a (assumed calibrated) camera, χ ∈ 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) (1)
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 (1) meant to recover the unmeasurable χ(t) from the measured s(t) can be devised as

ṡ = f m (s, u) + Ω T (s, v) χ + Hξ χ = f u (s, χ, u) + αΩ(s, v)ξ (2)
where H > 0 and α > 0 are suitable gains. We note that the scheme (2) 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).

From (1-2), the estimation error dynamics is given by

   ξ = -Hξ + Ω T (t) z ż = -αΩ (t) ξ + (f u (s, χ u) -f u (s, χ u)) = -αΩ (t) ξ + g(e, t) (3
) with g(e, t) being a vanishing term w.r.t. the error vector e, i.e., such that g(0, t) = 0, ∀t. Assuming m ≥ p, that is, more measurements s are available than the number of estimated quantities χ, the origin of (3) can be proven to be locally exponentially stable if

Ω (t) Ω T (t) ≥ γ T I, ∀t, (4)
see [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF]. Remark 2.1: In the special situation χ = 0 (unknown but constant parameters), one has g(e, t) ≡ 0 and global exponential convergence for the error system (3). The sphere/cylinder cases considered in the next sections meet this condition. However, we stress that the estimation scheme (2) is not restricted to this particular situation but it can be applied to the general case of state observation problems in which the unknown χ is subject to a nonnegligible dynamics as in [START_REF] Strasdat | Visual SLAM: Why filter?[END_REF], see [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF] for some examples.

It is further possible to show that, by degisning the estimation gain H as a function of the current s and v, one can obtain a completely decoupled transient behavior for the estimation error z (expressed in some suitable coordinates

η ∈ R p ) ηi + c i ηi + ασ 2 i η i = 0, i = 1 . . . p, (5)
where σ 2 i are the eigenvalues of martix ΩΩ T and c i are the scalar damping factors embedded in H. By then taking, for instance,

c i = c * i = 2
√ ασ i a critically damped state evolution is imposed to the estimation error behavior.

From (5) it also follows that the convergence speed of the estimation error z(t) results dictated by the smallest eigenvalue σ 2 1 of ΩΩ T . Being Ω = Ω(s, v), one has

(σ 2 1 ) = J v,1 v + J s,1 ṡ, (6)
with the Jacobian matrices J v,1 and J s,1 available in closed form and function of known quantities. 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 (6) will yield a camera velocity v(t) function of the system measured state s(t).

Remark 2.2: As discussed in [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF], in the general case the 'ideal' estimation error dynamics [START_REF] Civera | 1-Point RANSAC for EKF Filtering: Application to Real-Time Structure from Motion and Visual Odometry[END_REF] can result perturbed by some (unavoidable) disturbing terms. However, in the special case p = 1 (only one quantity to be estimated), if σ 1 (t) ≡ const then the disturbing effects can be fully compensated for and it is always possible to exactly enforce the "ideal" decoupled estimation error dynamics.

We then conclude with the following considerations on the choice of gain α in (2). In the SfM context, the norm of matrix ΩΩ T is strongly related to the norm of the camera linear velocity v. Roughly speaking, the 'faster' the motion (∼ larger v ), the 'larger' the value of σ 2 1 (∼ larger ΩΩ T ). Therefore, in order to obtain a desired estimation convergence speed for (2) (dictated by ασ 2 1 ) one can equivalently (i) travel at a larger speed v or (ii) increase the gain α. While increasing the gain α may always appear more convenient in terms of reduced control effort, practical issues such as noise, discretization or quantization errors, may impose an upper limit on the possible value of α, thus necessarily requiring a larger v for obtaining the desired convergence speed. Furthermore, as in all SfM problems, a v = 0 is also mandatorily required for guaranteeing σ 2 1 > 0 (a non-translating camera cannot estimate the scene structure).

III. STRUCTURE FROM MOTION FOR 3D

PRIMITIVES We now discuss the application of the proposed active estimation framework to two SfM problems involving a sphere and a cylinder. A main contribution of this section is the development of two novel minimal parameterizations of the sphere and cylinder 3D geometry that allow to considerably simplify the estimation task to the sole unknown sphere/cylinder radius: it is then possible to implement an estimator meeting the requirements of both Remarks 2.1 and 2.2 (estimation of a single and constant unknown state).

A. Spherical target

Consider a sphere O s of radius R and let P 0 = (X 0 , Y 0 , Z 0 ) be the coordinates of its center in the camera frame. Let also L : n T E + d = 0 represent the planar limb surface associated to the sphere in the camera frame, where E ∈ R 3 is a 3D point on the plane, n ∈ S 2 is the plane unit normal vector and d ∈ R the plane distance to the camera center [START_REF] Espiau | A new approach to visual servoing in robotics[END_REF]. Figure 1 shows the quantities of interest.

From [START_REF] Espiau | A new approach to visual servoing in robotics[END_REF], the depth Z of any point E lying on L can be expressed in terms of its normalized image coordinates p = (x, y, 1) as

1 Z = X 0 K x + Y 0 K y + Z 0 K = χ T p, (7)
where K = P T 0 P 0 -R 2 and χ = P 0 /K = -n/d ∈ R 3 represents an unmeasurable quantity. The interaction matrix of a generic (i, j)-th order moment m ij evaluated on the image of O s depends linearly on χ, see [START_REF] Giordano | 3D structure identification from image moments[END_REF], [START_REF] Chaumette | Image moments: a general and useful set of features for visual servoing[END_REF]. Therefore, a first possibility to retrieve the sphere 3D parameters (P 0 , R) would be to implement the estimation scheme (2) with s being a suitable collection of image moments (e.g., area and barycenter). It is in fact possible to show that K can be expressed in terms of image moments and of vector χ itself, so that, having estimated χ, one can consequently retrieve P 0 = χK and R = P T 0 P 0 -K. Although conceptually valid, this solution requires the concurrent estimation of three time-varying quantities (vector χ(t)). On the other hand, we now propose a novel representation that allows to reduce the estimation task to a single unknown constant parameter, i.e., the sphere radius R.

To this end, let s = (s x , s y , s z

) = P 0 /R ∈ R 3 , with ṡ = - 1 R I [s] × u (8)
and [START_REF] Fomena | Improvements on visual servoing from spherical targets using a spherical projection model[END_REF]. Let also (x g , y g , n 20 , n 11 , n 02 ) be the barycenter and normalized centered moments of order 2 measured from the elliptical projection of the sphere O s on the image plane, and a 1 be length of the minor axis of the observed ellipse. From [START_REF] Chaumette | Image moments: a general and useful set of features for visual servoing[END_REF], [START_REF] Fomena | Improvements on visual servoing from spherical targets using a spherical projection model[END_REF], one has

[v 1 ] × v 2 = v 1 × v 2 , see
x g = X 0 Z 0 Z 2 0 -R 2 , y g = Y 0 Z 0 Z 2 0 -R 2 , a 2 1 = R 2 Z 2 0 -R 2 (9)
and, additionally,

a 2 1 = 2 n 20 + n 02 -(n 20 -n 02 ) 2 + 4n 11 . (10)
Using ( 9) and the definition of vector s, it then follows

s x = x g s z a 2 1 , s y = y g s z a 2 1 , s z = 1 + a 2 1 a 2 1 , (11)
which, exploiting [START_REF] Morbidi | Observer design via immersion and invariance for vision-based leader-follower formation control[END_REF], shows that vector s can be equivalently expressed in terms of sole image quantities (i.e., measured image moments). Therefore, having obtained s from image moments, the only unknown quantity left is the sphere radius R which, if available, allows recover the 3D sphere parameters as P 0 = sR. Since ( 8) is linear in 1/R, we can define χ = 1/R, with then m = 3 and p = 1, and obtain for (1)

ṡ = [s] × ω -vχ χ = 0 . (12)
We note that, being in this case χ = 0, it is g(e, t) = 0 thus resulting in a global convergence for the error system (3). Furthermore, matrix ΩΩ T reduces to its single eigenvalue σ 2 1 = v 2 : therefore by keeping a v 2 ≡ const > 0, one can (i) satisfy condition (4) and (ii) exactly enforce the 'ideal' estimation error dynamics [START_REF] Civera | 1-Point RANSAC for EKF Filtering: Application to Real-Time Structure from Motion and Visual Odometry[END_REF], see Remark 2.2. Moreover, Ω = Ω(v) = v T with the Jacobians in [START_REF] Dixon | Range Identification for Perspective Vision Systems[END_REF] taking the expressions J v,1 = 2v T , J s,1 = 0.

We can finally note the following facts: first, since σ 2 1 is only function of the camera linear velocity v, one can freely exploit the camera angular velocity ω for, e.g., keeping the sphere at the center of the image by regulating (s x , s y ) to zero. Second, the direction of the camera motion has no influence on the estimation convergence: the convergence rate is only dictated by the norm of v. However, for the sake of imposing a desired transient response to the estimation error, one still needs to properly shape the damping gain H as a function of σ 2 1 as explained in Sec. II.

B. Cylindrical target

A cylinder O c can be described by its radius R > 0 and by its main axis a ∈ S 2 passing through a 3D point P 0 = (X 0 , Y 0 , Z 0 ), with a = 1 and, w.l.o.g., a T P 0 = 0 (P 0 can be chosen as the closest point on a to the origin of the camera frame). Moreover, analogously to the sphere, a cylinder is also associated with a planar limb surface L such that (7) holds for any point on L with projection p = (x, y, 1). Therefore, as in the sphere case, a possibility is to estimate the three unknown parameters of the limb plane (vector χ) by exploiting (at least) three image measurements, see [START_REF] Chaumette | Structure from controlled motion[END_REF] for a solution in this sense. However, we now propose a novel representation of the cylinder 3D geometry which, again, allows to reduce the estimation task of its parameters (P 0 , a, R) to the sole unknown but constant cylinder radius R.

L C P 0 O c n 1 n 2 L P 1 P 2 n R a
Let (ρ 1 , θ 1 ) and (ρ 2 , θ 2 ) be the (measured) distance/angle parameters of the two straight lines resulting from the projection of the cylinder on the image plane. We define n 1 = (cos θ 1 , sin θ 1 , -ρ 1 ), n 2 = (cos θ 2 , sin θ 2 , -ρ 2 ) as the normal vectors to the two planes passing through the origin of the camera frame and the two above-mentioned projected lines 1 . Figure 2 gives a graphical representation of the quantities of interest. Note that vectors n 1 and n 2 can be directly evaluated from image measurements (the line parameters). From [START_REF] Chaumette | Visual servoing using image features defined upon geometrical primitives[END_REF] an equivalent expression for vectors n 1 , n 2 can be obtained as

n 1 = 1 N 1 RP 0 √ K -b , n 2 = 1 N 2 RP 0 √ K + b (13)
with

                   K = P T 0 P 0 -R 2 b = (α, β, γ) = [P 0 ] × a N 1 = R X0 √ K -α 2 + R Y0 √ K -β 2 N 2 = R X0 √ K + α 2 + R Y0 √ K + β 2 , (14)
thus yielding

n 1 n 1 = RP 0 - √ Kb P T 0 P 0 , n 2 n 2 = RP 0 + √ Kb P T 0 P 0 (15)
By now defining

s = P 0 R ∈ R 3 , (16)
from ( 15) one has

∆ = 1 2 n 1 n 1 + n 2 n 2 = R 2 P T 0 P 0 s 1
The two planes are therefore tangent to the surface of the cylinder.

which can be exploited to obtain s = ∆/ ∆ 2 . This then shows how s, defined as in [START_REF] Davison | Mobile Robot Localisation Using Active Vision[END_REF], can be directly evaluated in terms of only image measurements, being ∆ a function of only measurable quantities (vectors n 1 and n 2 ).

A similar result can also be obtained for the cylinder axis a: indeed, exploiting (13) it is

[n 2 ] × n 1 = 2R N 1 N 2 √ K [b] × P 0 = 2RP T 0 P 0 N 1 N 2 √ K a (17)
where in the last step the property a T P 0 = 0 was used. Since a = 1, from ( 17) one then has

a = [n 2 ] × n 1 [n 2 ] × n 1 , (18)
that is, again, an expression in terms of only measured quantities. Therefore, the only unknown left is the cylinder radius R: once known, the 3D cylinder parameters can be fully recovered as P 0 = Rs and a from [START_REF] Spica | Coupling Image-Based Visual Servoing with Active Structure from Motion[END_REF]. In order to estimate R from image measurements, we seek an expression for ṡ in terms of the measured (s, a), of the camera linear/angular velocity (v, ω), and of the unknown R. As shown in the Appendix, it is possible to show that

ṡ = - 1 R I -aa T [s] × u. ( 19
)
Note the similarity of ( 19) with ( 8) for the sphere case. Being ( 19) linear in 1/R, we can then apply observer (2) by choosing χ = 1/R with m = 3 and p = 1, and having

ṡ = [s] × ω + aa T -I vχ χ = 0 . ( 20
)
Note how, again, being χ = 0 it is g(e, t) = 0 (global convergence for the error system (3) as in the sphere case). Matrix ΩΩ T reduces to its single eigenvalue

σ 2 1 = ΩΩ T = v 2 -(a T v) 2 . (21)
It is worth comparing [START_REF] Fomena | Improvements on visual servoing from spherical targets using a spherical projection model[END_REF] with the result obtained for the sphere (σ 2 1 = v 2 ). In the cylinder case, the convergence rate of the estimation scheme is affected by both the norm and the direction of the linear velocity v. In particular, for a given v = const, the maximum value for σ 2 1 is obtained when v has a null component along the cylinder axis a, i.e., when a T v = 0 with, in this case, σ 2 1 = σ 2 1,max = v 2 . Furthermore, as with the sphere, keeping a σ 2 1 (t) = const allows to exactly enforce the ideal estimation error dynamics [START_REF] Civera | 1-Point RANSAC for EKF Filtering: Application to Real-Time Structure from Motion and Visual Odometry[END_REF], see Remark 2.2.

Finally, from (21) one has

(σ 2 1 ) = J v,1 v + J a,1 ȧ = J v,1 v + J a,1 [a] × ω (22)
with J v,1 = 2v T Iaa T and J a,1 = 2v T av T . Although ( 22) also depends on the angular velocity ω, it is possible to fully compensate for the effects of J a,1 [a] × ω (a known quantity) when inverting ( 22) w.r.t. v as discussed in Sect. IV-B. Therefore, one can act on v to regulate the value of σ 2 1 (t) and, at the same time and in a decoupled way, exploit the camera angular velocity ω for implementing additional tasks of interest such as keeping the cylinder axis a at the center of the image plane by enforcing (s x , s y ) = 0.

IV. EXPERIMENTAL RESULTS

In this section we show some experimental results meant to validate the theoretical developments of the previous sections. 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. All the image processing and feature tracking were implemented via the open-source ViSP library [START_REF] Marchand | ViSP for visual servoing: a generic software platform with a wide class of robot control skills[END_REF]. As objects to be tracked, for the spherical and cylindrical targets we made use of a white table-tennis ball and of a white cardboard cylinder with radius of 1.9 cm and 4.2 cm, respectively. A video of the experiments is attached to the paper.

A. Experiments with a Spherical Target

As explained in Sec. III-A, the convergence rate of the estimation error for the sphere case only depends on the norm of the linear velocity v and not on its direction. This fact is proven by the first experiment where the estimation task is run twice starting from two different positions and imposing two different camera velocities but with same norm. These values were used during the experiments: 5) and v = (-0.05, 0, 0) for case I and v = (0, 0.045, 0.02) for case II, with v = 0.05 in both cases. The camera angular velocity ω was exploited to keep (s x , s y ) ≃ (0, 0) (centered sphere).

α = 2 • 10 3 , c 1 = c * 1 = 2 √ ασ 1 in (
Figure 3a shows the behavior of the estimation errors (solid blue and red lines): note how the error transient response for the two cases is essentially coincident, and also equivalent to that of the reference second order system (5) with the desired poles, i.e., by setting σ 2 1 = v 2 = const and c 1 = c * 1 in (5) (dashed black line). The higher noise level in case II (red line) is due to the larger distance between the camera and the spherical target (see Fig. 3b) which negatively affects the image processing.

Since the direction of the velocity does not play any role in this case, no optimization of σ 2 1 can be performed under the constraint v = const. On the other hand, the analysis of Sec. II clearly indicates the importance of choosing a proper value of c 1 in (5). To show this fact, we report here three experiments characterized by the same camera trajectory of the previous case I, but by employing three different values for c 1 , that is, c * 1 , 2c * 1 and 0.5c * 1 . These correspond to a critically damped, overdamped and underdamped response for the ideal system (5), respectively. The experimental results reported in Fig. 4 show that the behavior of the estimation error z (solid lines) has an excellent match with that of (5) (represented by dashed lines), thus fully confirming (i) the validity of the proposed theoretical analysis, and (ii) the importance of choosing the 'right' damping matrix H for optimizing the convergence speed in addition to a proper regulation of σ 2 1 .

B. Experiments with a Cylindrical Target

In the cylinder case, the convergence rate of the estimation error depends both on the norm of the camera linear velocity Fig. 3: Experimental results for the estimation of the radius of a sphere using different constant camera velocities with the same norm. (a): behavior of the estimation error z(t) for the two cases (solid blue and red lines), and for an 'ideal' second order system with poles at the desired locations (dashed black line). Note the almost perfect match between the three plots. (b): camera trajectories for case I (blue line) and case II (red line) with arrows indicating the direction of the camera optical axis. Note that both trajectories are circular as expected, but (i) with a different motion direction and (ii) starting from a different initial position (larger distance in case II). Fig. 4: Experimental results for the estimation of the radius of a sphere with c1 = c * 1 (blue line), c1 = 2c * 1 (green line) and c1 = 0.5c * 1 (red line). The dashed lines represent the response of an 'ideal' second order system with the corresponding poles. Note again the almost perfect match between the plots. v and on its direction w.r.t. the cylinder axis a, see [START_REF] Fomena | Improvements on visual servoing from spherical targets using a spherical projection model[END_REF]. It is then interesting to optimize the direction of v under the constraint v = const for maximizing the eigenvalue σ 2 1 (i.e., so as to obtain the fastest convergence rate for a given 'control effort' v ).

From [START_REF] Chaumette | Visual servoing using image features defined upon geometrical primitives[END_REF], maximization of σ 2 1 (t) w.r.t. vector v can be obtained by choosing

v = J T v,1 -J † v,1 J a,1 [a] × ω, (23)
with A † being the pseudoinverse of matrix A, i.e., by following the gradient of σ 2 1 w.r.t. v and by compensating for the (known) effects of input ω. In order to additionally enforce the constraint v = const during the eigenvalue maximization, we let κ = 1 2 v 2 , κ des = 1 2 v 0 2 and modify [START_REF] Marchand | ViSP for visual servoing: a generic software platform with a wide class of robot control skills[END_REF]

as v = v v 2 k 1 (κ des -κ) + k 2 I - vv T v 2 (J T v,1 -J † v,1 J a,1 [a] × ω), (24)
with k 1 > 0 and k 2 > 0. The first term in (24) asymptotically guarantees v(t) 2 = v 0 2 while the second term projects [START_REF] Marchand | ViSP for visual servoing: a generic software platform with a wide class of robot control skills[END_REF] onto the null-space of the constraint v = const. As for the angular velocity ω, we exploited it for keeping the axis of the cylinder at the center of the image plane by regulating (s x , s y ) to (0, 0).

We now present three experimental results for the cylinder case structured as follows: in the first experiment (case I), the update rule (24) is fully implemented (k 1 > 0, k 2 > 0) for actively optimizing the direction of v. In the second experiment (case II), the camera starts from the same initial pose and velocity as in case I, but (24) is implemented with k 1 > 0 and k 2 = 0, i.e., without performing any optimization of σ 2 1 . Finally, in the third experiment (case III), the camera starts from a different initial pose and with a different velocity direction (but same norm) w.r.t. the previous two cases, and (24) is again fully implemented. This last case is meant to show how the convergence properties of the estimator are not affected by the camera starting position but only by the norm and direction of v w.r.t. a.

The experiments were run with the following conditions: α = 500, c 1 = c * 1 , k 1 = 10, k 2 = 1 for cases I and III, and k 2 = 0 for case II. As for the linear velocity, we set v(t 0 ) = v 0 = (-0.01, 0.05, 0.05) for cases I and II, and v(t 0 ) = v 0 = (-0.05, 0.05, 0.01) for case III (note how v 0 = 0.0714 in all three cases). We start showing in Fig. 5a the behavior of v(t) 2 for the three experiments with the following color coding shared by all the next plots: blue -case I, red -case II, green -case III. One can note how, in all three cases, the same v(t) 2 = v 0 2 = 5.1 × 10 -3 is attained thanks to the action of the first term in (24). The behavior of σ 2 1 (t) is shown in Fig. 5b: as explained at the end of Sect. III-B, under the constraint v = const one has max v σ 2 1 = v 2 as the largest possible value for σ 2 1 (obtained when v T a = 0). By comparing Fig. 5b with Fig. 5a, it is then possible to verify that, indeed, σ 2 1 (t) → v 0 2 in cases I and III despite the different initial conditions of the experiments (different camera pose and direction of v). The optimization in (24) results in a null component of v along a, thus allowing to move faster in the 'useful' directions (while keeping a constant v ), and to increase the value of σ 2 1 to its maximum possible value. Also, note how the value of σ 2 1 (t) for case II results smaller than in the other two cases (as expected) since no optimization is present in this case.

The behavior of the estimation error z(t) is shown in Fig. 5c: again, we can note that the transient response for cases I and III results essentially coincident and in almost perfect agreement with that of the reference system (5) with desired poles (dashed black line). As expected, the response for case II (red line) is slower than in cases I and III, but still in agreement with the corresponding response of system (5) (dashed black line). Finally, Fig. 5d depicts the camera trajectories for the three experiments with arrows indicating the direction of the optical axis. In case II the camera simply travels along a straight line (v(t) ≡ v 0 ), while in cases I and III the direction of v is suitably modified As expected, the convergence rate of the estimation error of cases I and III is faster than in case II. Note also how all the transient responses are in almost perfect agreement with the corresponding response of the reference system (5) with desired poles (dashed black lines). (d): two views of the camera trajectories for the three cases with arrows indicating the direction of the camera optical axis.

resulting in a trajectory lying on a plane orthogonal to a.

V. CONCLUSIONS

In this paper we have addressed the problem of active SfM for recovering the 3D structure of a spherical and a cylindrical object by exploiting a recently proposed active estimation strategy tailored to the two cases under consideration. As an additional contribution, we have also proposed a novel minimal parameterization of the sphere/cylinder 3D geometry able to reduce the estimation task to a single unknown constant quantity (the radius R) in place of the classical (and time-varying) three parameters (scaled normal vector of the sphere/cylinder planar limb surface). The reported experimental results fully confirmed the validity of the analysis and the ability to actively impose a desired transient behavior to the (nonlinear) estimation schemes, in particular matching that of a reference linear second-order system with desired poles.

We are currently investigating the use of similar active strategies when dealing with more complex 3D scenes (e.g., planar images or other 3D primitives), and are also aiming at exploiting these techniques in the context of visual-based control of robot manipulators (see [START_REF] Spica | Coupling Image-Based Visual Servoing with Active Structure from Motion[END_REF] for a first attempt in this direction).

APPENDIX

We note that the cylinder axis a can be determined by the intersection of two planes P i : r T i E -d i = 0, i = 1, 2, with r 1 = [a] × P 0 P 0 , d 1 = 0, r 2 = -P 0 P 0 , d 2 = P 0 , (25) see Fig. 2. In particular, plane P 1 passes through the camera optical center, it is orthogonal to plane P 2 , and both planes contain the axis a passing through P 0 (by construction).

Since Rs = P 0 and P 0 belongs to the cylinder axis a, it is Rr T i s -d i = 0, i = 1, 2 (the point Rs belongs to both planes P i ). Taking the time derivative of these latter constraints (with R = const), one has

r T i ṡ = 1 R ḋi -s T ṙi , i = 1, 2. (26)
Since ṙi = [r i ] × ω and ḋi = r T i v (see [START_REF] Giordano | 3D structure identification from image moments[END_REF]), eq. ( 26) can be rewritten as

r T i ṡ = 1 R r T i v -s T [r i ] × ω, i = 1, 2. (27)
Finally, from a T P 0 = 0 and P 0 = Rs it is a T s = 0 implying that (taking a time derivative)

a T ṡ = -s T ȧ = -s T [a] × ω. (28)
We now note that equations (27-28) provide three linear constraints for ṡ which, by using (25), can be rearranged in matrix form as the following linear system

       P T 0 P 0 a T [a] × P 0 T P 0        ṡ = 1 R         P T 0 P 0 v -P T 0 [a] × ω P 0 a T ω + [a] × P 0 T P 0 v         . (29)
It is easy to verify that the 3 × 3 matrix on the lhs of ( 29) is orthonormal: by then solving (29) for ṡ and performing some simplifications we finally obtain the sought result

ṡ = - 1 R I -aa T [s] × u. (30)

Fig. 1 :

 1 Fig. 1: Camera C and spherical target Os with planar limb surface L

Fig. 2 :

 2 Fig. 2: Camera C and cylindrical target Oc with the planar limb surface L and the other planes of interest P1 and P2

Fig. 5 :

 5 Fig. 5: Experimental results for the estimation of the radius of a cylinder with the following color coding: blue -case I, redcase II, green -case III. (a): behavior of v(t) 2 over time. Note how the same constant velocity norm is reached and then kept in all cases. (b): behavior of σ 2 1 (t) for the three cases (coincident for cases I and III and larger than in case II). (c): behavior of z(t).As expected, the convergence rate of the estimation error of cases I and III is faster than in case II. Note also how all the transient responses are in almost perfect agreement with the corresponding response of the reference system (5) with desired poles (dashed black lines). (d): two views of the camera trajectories for the three cases with arrows indicating the direction of the camera optical axis.


Riccardo Spica
email: riccardo.spica@irisa.fr

Paolo Robuffo Giordano

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

F Chaumette

Coupling Visual Servoing with Active Structure from Motion

In this paper we propose a solution for coupling the execution of a visual servoing task with a recently developed active Structure from Motion strategy able to optimize online the convergence rate in estimating the (unknown) 3D structure of the scene. This is achieved by suitably modifying the robot trajectory in the null-space of the servoing task so as to render the camera motion 'more informative' w.r.t. the states to be estimated. As a byproduct, the better 3D structure estimation also improves the evaluation of the servoing interaction matrix which, in turn, results in a better closed-loop convergence of the task itself. The reported experimental results support the theoretical analysis and show the benefits of the method.

I. INTRODUCTION

In many applications the state of a robot w.r.t. the environment can only be partially retrieved from its onboard sensors, and online state estimation schemes can be exploited in order to recover the 'missing information' by incrementally processing the sensed data. When considering non-trivial cases, however, one often faces nonlinear estimation problems for which the actual robot trajectory plays an important role for a successful estimation convergence. This is, for instance, the case of all Structure from Motion (SfM) problems in which a poor choice of the system inputs (the camera linear velocity) can even make the 3D scene structure non-observable (and regardless of the employed estimation strategy). It is then interesting to study how to optimize the trajectory of a robot executing a given task with the aim of facilitating the state estimation process.

The goal of this paper is to propose an online solution to this problem in the context of visual control of robot manipulators. We consider, as case study, a classical Image-Based Visual Servoing [START_REF] Chaumette | Visual servo control, Part I: Basic approaches[END_REF] (IBVS) of a set of point features whose depths represent the unknown states to be estimated during motion. We then show how to couple the IBVS execution with the optimization of the depth estimation convergence. A coupling between visual servoing and SfM estimation was also proposed in [START_REF] Petiteville | On-line estimation of the reference visual features application to a vision based long range navigation task[END_REF] but without any active optimization of the camera velocity for the sake of enhancing the estimation convergence rate. The optimization action in our proposed solution is based on a recently proposed framework for active SfM [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF] and is here projected onto the null-space of the IBVS task considered as the primary objective. Additionally, in order to obtain the largest possible degree of redundancy w.r.t. the IBVS task, we also suitably exploit and extend the redundancy framework introduced in [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF] meant to provide a large projection operator by considering the norm of the visual error as main task. In particular, the controller originally proposed in [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF] is here extended to the second-order since the active strategy of [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF] acts on the camera linear velocity and, thus, requires an action at the acceleration level.

We then experimentally validate the proposed active estimation and control strategy. The experiments clearly show the achievement of two goals: (i) the optimization of the camera trajectory during the servoing transient which allows obtaining the fastest possible convergence of the SfM algorithm, and (ii) the concurrent improvement of the IBVS convergence thanks to the better approximation of the interaction matrix from the recovered 3D parameters. We finally stress that the proposed coupling between task execution and trajectory optimization for improved state estimation is not restricted to the sole class of IBVS problems considered in this work: indeed, one can easily generalize these ideas to other servoing tasks (e.g., considering different visual features or even geometric ones as in PBVS schemes), or apply them to other contexts not necessarily related to visual control (as long as the chosen robot trajectory has an effect on the state estimation task).

The rest of the paper is organized as follows: we start by summarizing in Sec. II the active SfM framework presented in [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF]. Then, we present in Sec. III a second-order extension of the strategy described in [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF] that allows to increase the degree of redundacy w.r.t. the considered IBVS task to its maximum extent. Finally we report in Sec. IV some experimental results validating our analysis, and we draw some conclusions in Sec. V by also proposing some possible future directions.

II. A FRAMEWORK FOR ACTIVE STRUCTURE FROM MOTION

We start by briefly summarizing the active SfM framework proposed in [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF]. Let s ∈ R m be the set of visual features measured on the image plane of a (assumed calibrated) camera, χ ∈ 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) (1)
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 (1) meant to recover the unmeasurable χ(t) from the measured s(t) can be devised as

ṡ = f m (s, u) + Ω T (s, v) χ + Hξ χ = f u (s, χ, u) + αΩ(s, v)ξ (2)
where H > 0 and α > 0 are suitable gains. We note that the scheme (2) 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 ṡ, (3)
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 (3) will yield a camera velocity v(t) function of the system measured state s(t).

Formulation (1) can be applied to the point feature case (considered in this work) by taking s = p = (x, y) = (X/Z, Y /Z) as the perspective projection of a 3D point (X, Y, Z), and χ = 1/Z with, thus, m = 2 and p = 1. Explicit expressions of the above machinery can be found in [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF] with, in particular,

     σ 2 1 = ΩΩ T = (xv z -v x ) 2 + (yv z -v y ) 2 J v = 2 v x -xv z v y -yv z (xv z -v x ) x + (yv z -v y ) y J s = 2 (xv z -v x )v z (yv z -v y )v z . (4)

III. VISUAL SERVOING COUPLED WITH ACTIVE 3D ESTIMATION

A. Problem Description

We consider the classical situation of a robot manipulator with joint configuration vector q ∈ R n carrying a eye-inhand camera that measures a set of visual features s ∈ R m to be regulated to a desired constant value s * . As wellknown, one has ṡ = L s (s, χ)u and u = J C (q) q, where L s ∈ R m×6 is the interaction matrix of the considered visual features, χ ∈ R p is a vector of unmeasurable 3D quantities associated to s (e.g., the depth Z for a feature point), and J C (q) ∈ R 6×n the Jacobian of the eye-in-hand camera w.r.t. the robot joint velocities. Let J (s, q, χ) = L s (s, χ)J C (q) ∈ R m×n be the visual task Jacobian and define e = s-s * as the visual error vector with, thus, ė = J q. In case the robot is redundant w.r.t. the visual task (n > m), a typical choice for regulating e(t) → 0 is to apply the control law [START_REF] Chaumette | Visual servo control, Part I: Basic approaches[END_REF]

q = -λ Ĵ † e + (I n - Ĵ † Ĵ )r, λ > 0. (5)
Here, A † denotes the pseudoinverse of matrix A, the task Jacobian Ĵ (s, χ, q) is evaluated on some approximation χ of the unknown true vector χ, e.g., the value at the desired pose χ = χ * , and r ∈ R n is an arbitrary vector projected on the null-space of the main visual task. When applying [START_REF] De Luca | Robot redundancy resolution at the acceleration level[END_REF] with rank(J ) = m and χ = χ, one obtains a perfectly decoupled and exponential behavior for the visual error e(t), i.e., e(t) = exp(-λ(t -t 0 ))e(t 0 ).

When, instead, χ is replaced by any approximation χ, the ideal closed-loop behavior ( 6) is no longer obtained. Since χ is not directly measurable from visual input, and special approximations such as χ * require anyway some 'pre-knowledge' of the scene, another interesting possibility is to exploit the estimation scheme (2) for recovering online a (converging) estimation χ(t) from the measured s(t) (visual features) and known u(t) (camera motion). Observer (2) can indeed run in parallel to the servoing controller [START_REF] De Luca | Robot redundancy resolution at the acceleration level[END_REF], by treating the camera linear/angular velocities generated by [START_REF] De Luca | Robot redundancy resolution at the acceleration level[END_REF] as the inputs u in (2), and by plugging the estimated state χ recovered by [START_REF] Petiteville | On-line estimation of the reference visual features application to a vision based long range navigation task[END_REF] in the evaluation of Ĵ (s, χ, q) needed by [START_REF] De Luca | Robot redundancy resolution at the acceleration level[END_REF]. This way, one can in fact: (i) improve the servoing execution by yielding a closed-loop behavior matching the ideal (6) also when far from the desired pose and without needing special assumptions/approximations of χ, since, as χ(t) → χ(t), one has Ĵ → J , and (ii) obtain, as a byproduct, the concurrent 3D structure estimation of the observed scene by exploiting the motion performed by the camera for realizing the servoing task.

In this conceptual scheme, the estimation of the 3D structure χ takes then place only during the transient of the servoing task (i.e., as long as the camera is in motion towards its goal location). Being this phase of limited duration, with the camera reaching a full stop at the end of the servoing, one is clearly interested in obtaining the fastest possible convergence for the estimation error. As explained in the previous section, it is possible to 'optimize' the convergence rate of the estimation error by (actively) maximizing the eigenvalue σ 2 1 over time. A natural possibility is to then project such optimization action within the null-space of the main visual servoing task. The next section explains a possible strategy to achieve this goal.

B. Second-order Visual Servoing using a Large Projection Operator

In order to fully take advantage of the optimization of the 3D structure estimation, it is clearly important to exploit the largest possible redundancy w.r.t. the considered visual task. Indeed, if the visual task constrains most or all the camera dofs, no optimization of the camera motion can be performed. In this sense, the redundancy framework proposed in [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF] represents a very convenient possibility: regulation of the visual error vector e can be replaced by the regulation of its norm e (a 1-dimensional task), thus resulting in a nullspace of (maximal) dimension n -1 available for additional optimizations. As discussed in [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF], this technique becomes singular for e → 0 or e ∈ Ker(J T ) and, thus, requires a proper switching to the classical law (regulation of the whole vector e) when close to convergence.

Coming to our case, the expression in [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF] shows that optimization of σ 2 1 (t) requires an action at the joint acceleration level. Indeed, from (3) and v = J C (q) q, one has

(σ 2 1 ) = J v v + J s ṡ = J v J C q + J v J C q + J s ṡ. ( 7
)
Maximization of σ 2 1 can be obtained by applying, for instance, the following joint acceleration vector

q = qσ = k σ J T C J T v -(J v J C ) † (J v J C q + J s ṡ) (8)
with k σ > 0. However, the control strategy proposed in [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF] only addresses motion control at the first-order/velocity level. We then now proceed to its extension at the secondorder/acceleration level. Assume that m ≥ n and rank(J ) = n (overconstrained visual task with no redundancy left, as in the experiments reported in the next section). We first note that, being ė = J q and, thus, ë = J q + J q, the second-order counterpart of the classical law [START_REF] De Luca | Robot redundancy resolution at the acceleration level[END_REF] for regulating the whole error vector e(t) to 0 is simply

q = qe = J † (-k v ė -k p e -J q) (9)
with k p > 0 and k v > 0. Note that, as in the firstorder case (5), when implementing (9) one should replace J (s, χ, q) with its approximation Ĵ (s, χ, q), and likewise for the evaluation of ė and J . However, for the sake of exposition, we assume for now availability of all the needed quantities.

The controller (9) would solve the visual servoing task but, clearly, without any possible additional optimization action. By now letting ν = e , it is ν = e T J e q = J e q, J e ∈ R 1×n , and ν = J e q + J e q. Regulation of ν(t) → 0 can then be achieved by applying the joint acceleration vector

q = q e = J † e (-k v ν -k p ν -J e q) + (I n -J † e J e )r = J † e (-k v ν -k p ν -J e q) + P e r, (10)
with k p > 0, k v > 0, J † e = e e T JJ T e J T e and P e = I n -J T ee T J e T JJ T e being the null-space projection operator of the error norm with (at least) rank n -1. One can then choose vector r in (10) as

r = qσ -k d1 q, k d1 > 0, (11)
so as to project the optimization action (8) onto the nullspace of the main task ν = e , together with an additional 'damping' on the joint velocities needed to stabilize motions in the null-space, see [START_REF] De Luca | Robot redundancy resolution at the acceleration level[END_REF]. Controller (8)-(10-11) can then realize the visual task by regulating its norm to zero ( e → 0) with the largest possible redundancy (of degree n -1) 1 . We note that the Jacobian J e is singular for e = 0 and, as explained in [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF], the projection matrix P e is not well-defined for e → 0. Presence of this (unavoidable) singularity motivates the introduction of a switching from the controller (10) to the classical law (9) when close to convergence. However, the 'first-order' switching strategy proposed in [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF] cannot be directly transposed to the secondorder case, but some suitable modifications must be taken into account as discussed in the next section.

C. A Second-order Switching Strategy

We start noting that, in closed-loop, controller q e in (10) imposes the following second-order dynamics to the error norm

ν + k v ν + k p ν = 0. ( 12
)
Define ν e (t) as the solution of (12) for a given initial condition (ν(t 0 ), ν(t 0 )): ν e (t) thus represents the 'ideal' evolution of the error norm, that is, the behavior one would obtain if controller (10) could be implemented ∀t ≥ t 0 . Let now t 1 > t 0 be the time at which the switch from controller (10) to the classical law qe in (9) occurs (e.g., triggered by some threshold on e as proposed in [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF]). For t ≥ t 1 and under the action of qe one has in closed-loop

ë + k v ė + k p e = 0. (13)
Let e * (t) be the solution of (13) with initial conditions (e(t 1 ), ė(t 1 )), and let ν * (t) = e * (t) be the corresponding behavior of the error norm. Ideally, one would like to have

ν * (t) ≡ ν e (t), ∀t ≥ t 1 . (14)
In other words, the behavior of the error norm should not be affected by the control switch at time t 1 , but ν * (t) (obtained from (13)) should exactly match the 'ideal' evolution ν e (t) generated by (12) as if no switch had taken place. While condition ( 14) is easily satisfied at first-order [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF], this is not necessarily the case at the second-order level. Indeed, when moving to the second-order, condition (14) holds if and only if, at time t 1 , vectors e(t 1 ) and ė(t 1 ) are parallel (proof in the Appendix). It is then necessary to introduce an intermediate phase before the switch during which any component of ė orthogonal to e is made negligible.

To this end, let

P e = I m - ee T e T e ∈ R m×m
be the null-space projector spanning the (m-1)-dimensional space orthogonal to vector e. Let also δ = P e ė = P e J q. The scalar quantity δ T δ ≥ 0 provides a measure of the misalignment among the directions of vectors e and ė (δ T δ = 0 iff e and ė are parallel). One can then minimize δ T δ compatibly with the main task (regulation of the error norm) by choosing vector r in (10) as

r = - k d2 2 ∂δ T δ ∂ q T = -k d2 J T P e J q, k d2 > 0, (15)
where the properties P e = P T e = P e P e were used. A possible switching strategy for ensuring condition ( 14) is then:

1) apply the norm controller q e given in (10) with the null-space vector r defined in (11) as long as ν(t) ≥ ν T , with ν T > 0 being a suitable threshold on the error norm. During this phase, the error norm will be governed by the closed-loop dynamics (12) and the convergence rate in estimating χ will be maximized; 2) when ν(t) = ν T , keep applying controller q e but replace (11) with (15) for vector r. Stay in this phase as long as δ T δ ≥ δ T , with δ T > 0 a suitable threshold on the alignment among vectors e and ė. Note that, during this second phase, ν(t) keeps being governed by the closed-loop dynamics (12) since r acts in the null-space of the error norm (i.e., no distorting effect is produced on the behavior of ν(t) by the change in r); 3) when δ T δ = δ T , switch to the classical controller qe given in (9) until completion of the task. Property (14) will clearly hold since, at the switch time, parallelity among e and ė has been enforced by the previous phase. We finally note that this strategy could cause a discontinuity in the commanded q when passing from phase 1) to phase 2) because of the instantaneous change of vector r from (11) to (15). This discontinuity can be easily dealt with by resorting to a suitable smoothing function λ(ν) as proposed in [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF]. Indeed by implementing in both phase 1) and phase 2)

r = λ(ν) (q σ -k d1 q) -(1 -λ(ν)) k d2 J T P e J q, (16)
vector r in (11) is gradually replaced by vector r in (15) while νT ≥ ν(t) ≥ ν T . Figure 1 shows an illustrative example of λ(ν). As for the switch from phase 2) to phase 3), discontinuities in q are avoided thanks to the alignment among vector e and ė.

Before concluding this section we remark that the proposed scheme (active SfM coupled to the second-order visual servoing and associated switching strategy) only requires, as measured quantities, the visual features s, the robot joint configuration vector q and the joint velocities q (in addition to the usual assumption of intrinsic and camera-to-robot parameters). Indeed knowing χ, a (possibly approximated) evaluation of all the other quantities entering the various steps of the second-oder control strategy can be obtained from (s, χ, q) and q (the only 'velocity' information actually needed). We also note that the level of approximation is clearly a monotonic function of χ -χ (i.e., the uncertainty in knowing χ): thus, all the previous quantities will asymptotically match their real values as the estimation error z(t) = χ(t) -χ(t) converges to zero.

We finally remark that, due to the nonlinear nature of the estimation and servoing schemes, stability of each individual block does not imply stability of their composition (one cannot invoke the separation principle only valid for linear timeinvariant systems), and thus some additional analysis should be performed to assess the overall closed-loop stability of our solution 2 . The reported experiments nevertheless showed a promising level of robustness in this sense.

IV. EXPERIMENTAL RESULTS

In this section we report the results of several experiments meant to illustrate the approach described in the previous sections. All experiments were run by making use of a greyscale camera attached to the end-effector of a 6-dofs Gantry robot. The camera has a resolution of 640 × 480 px and a framerate of 30 fps. Since the robot only accepts velocity commands, the acceleration signal generated by the proposed controller was numerically integrated before being sent to the robot. The controller (and its internal states) was updated at 1 kHz, while the commands were sent to the robot at 100 Hz. All the image processing and feature tracking were implemented via the open-source ViSP library [START_REF] Marchand | ViSP for visual servoing: a generic software platform with a wide class of robot control skills[END_REF].

As visual task, we considered the regulation of N = 4 point features p i . We then have s = (p 1 , . . . , p N ) ∈ R m , and L s = (L s1 , . . . , L s N ) ∈ R m×6 , m = 8, with L si being the 2 × 6 interaction matrix associated to the i-th point [START_REF] Chaumette | Visual servo control, Part I: Basic approaches[END_REF]. As for vector χ, it is χ = (χ 1 , . . . , χ N ) ∈ R p , p = 4, where χ i = 1/Z i as explained at the end of Sec. II. The points were positioned at the vertices of a square of 0.25 m size.

Each feature point is characterized by its eigenvalue σ 2 1,i and the associated matrices J v,i , J s,i , see [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF]. In order to optimize the estimation of the whole vector χ (the depth of all points), we simply aimed at maximizing the sum σ 2 = N i=1 σ 2 1,i and thus modified (7-8) as

(σ 2 ) = N i=1 J v,i J C q + N i=1 J v,i J C q + J s,i ṡi and qσ = k σ J T C N i=1 J T v,i - N i=1 J v,i J C † N i=1 J v,i J C q + J s,i ṡi .
(17) The first set of experiments shows the results of having coupled a servoing controller with an active SfM strategy. Fig. 3: Behavior of the estimation error z(t) = χ(t) -χ(t) when actively optimizing the camera motion for improving the estimation convergence rate (case 1 -blue line) and when not optimizing the camera motion (case 2 -red line).

To this end, we report in Fig. 2 the evolution of the error norm ν(t) for the following four cases:

1) the full strategy (three phases) illustrated in the previous section is implemented. The estimator ( 2) is concurrently run to provide an estimation χ(t) to all the control terms. This case then involves the active optimization of the camera motion for improving, as much as possible, the convergence rate of the estimation error z(t) = χ(t)-χ(t) (case 1 -blue line in the plot); 2) controller ( 9) is implemented for all the task duration while observer (2) is still run in parallel for generating χ(t). Thus, in this case no optimization of the estimation error convergence is performed, but χ(t) is still estimated during the resulting camera motion (case 2red line in the plot); 3) controller (9) is again implemented for all the task duration, but by now employing χ(t) = χ * = const, that is, the value of χ at the desired pose (case 3green line in the plot). 4) as a reference 'ground truth', the behavior of controller (9) when using the real value of χ(t) is also reported (case 0 -black line in the plot). The following gains and thresholds were used in the experiments: α = 1500, k p = 0.16, k v = 0.8 in (9) and (10), k d1 = 27, k d2 = 10, νT = 0.14, ν T = 0.12, k σ = 0.85, and δ T = 0.02 (only for case 1). Vector χ was initialized with Ẑ = 0.2 for all point (for cases 1 and 2), Finally, Fig. 3 shows the behavior of z(t) in cases 1 and 2 using the same color code of Fig. 2, while Fig. 4 depicts the camera and feature trajectory for the first experiment (case 1). We also invite the reader to watch the accompanying video for a better visualization of the robot motion.

Let us first consider Fig. 3: we can clearly note how the optimization action present in case 1 (blue line) allows to obtain a significantly quicker convergence of z(t) w.r.t. case 2 (red line). Indeed, in case 1 z(t) converges towards zero in about 4 sec and, additionally, it keeps a lower steady-state value w.r.t. case 2 (thus the depths Z i are estimated faster and more accurately in case 1). A related pattern can be found in Fig. 2: note, in fact, how in case 1 the behavior of ν(t) (blue line) quickly matches the ideal one of case 0 (black line) because of the fast convergence of z(t) . In case 2 (red line), the matching with case 0 is still obtained, but much later in the plot (at about 10 sec w.r.t. 6 sec). Note also the higher initial overshoot of case 2 compared to case 1. Finally, case 3 (green line) almost never reaches the ideal behavior of case 0 if not at the end of the motion as expected (since only in this case it is χ(t) ≈ χ * ). The trajectory depicted in Fig. 4 is also helpful in understanding the effects of the optimization action on the estimation error convergence during case 1: note, indeed, how the camera initially moves along an almost planar spiral (blue line) because of the null-space term (17) which imposes a motion maximizing the estimation of the point depths. This also allows to appreciate the benefits of having employed the norm controller (10) during the first phase: thanks to the large redundancy granted by this controller, the camera is free to perform very 'unusual' motions while still guaranteeing correct convergence of the error norm. For completeness, the red line in the plot depicts phase 2) of the switching strategy (the alignment of vectors e and ė), while the green line represents phase 3) (use of the classical controller (9)). Finally in Fig. 4b the trajectory of the point features on the image plane are reported with the same color code: clearly only in phase 3), when the full error controller (9) is used, the points correctly move in (approximately) straight lines toward their desired positions.

In the second set of experiments we instead show the importance of having introduced phase 2) in the switching strategy of Sec. III-C (i.e., of having enforced alignment of e and ė). To this end, Fig. 5a shows the behavior of the error In both plots, the blue lines represent the behavior of case 1 (full implementation of the switching strategy of Sec. III-C), while magenta lines represent the direct switch from phase 1) to phase 3) without the action of vector r in (15). norm ν(t) of the previous case 1 (blue line) together with the behavior of ν(t) when not implementing phase 2) but just switching from phase 1) to phase 3) (magenta line). The two blue vertical lines represent the switch from phase 1) to phase 2) and from phase 2) to phase 3). Note how, in this second situation, the error norm ν(t) has a large overshoot when switching to phase 3) due to the misalignment of vectors e and ė, an overshoot clearly not present in case 1. Finally, Fig. 5b shows the behavior of δ , the measure of misalignment among e and ė. Note how, in case 1, δ is correctly made negligible at the end of phase 2) thanks to the action of vector r in (15). These results then fully confirm the theoretical analysis of the closed-loop behavior under the control strategy of Sec. III.

V. CONCLUSIONS

In this paper a strategy for optimally coupling a visual servoing task with an active SfM algorithm has been presented. In particular, we showed how to implement an active optimization of the SfM error within the null-space of a IBVS task. To this end, we employed a second-order version of the norm controller originally introduced in [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF] for maximizing task redundancy, and provided a thorough analysis of the closed-loop convergence behavior when employing a suitable switching strategy for avoiding the typical singularities of the method. The experiments clearly showed the benefits of the proposed strategy in (i) obtaining a faster convergence of the structure estimation error and (ii) imposing a better closed-loop behavior to the servoing controller w.r.t. an ideal response obtained in perfect conditions.

In the future we plan to apply the proposed general technique to more complex visual servoing tasks and structure estimation problems, possibly involving additional constraints such as presence of obstacles, joint limit avoidance, as well as applications to mobile (ground/flying) robotics. APPENDIX Lemma 1.1: Condition (14) holds if and only if, at the switching time t 1 , vectors e(t 1 ) and ė(t 1 ) are parallel.

Proof: Let Φ(t) = [Φ ij (t)] ∈ R 2×2 be the statetransition matrix associated to the linear time-invariant system (12). From classical system theory [START_REF] Kailath | Linear Systems[END_REF], it is ν e (t) = Φ 11 (t -t 1 )ν(t 1 ) + Φ 12 (t -t 1 ) ν(t 1 ), ∀t ≥ t 1 . We note that (13) is governed, component-wise, by the same dynamics of (12). Therefore, the solution of ( 13) is e * (t) = Φ 11 (t -t 1 )e(t 1 ) + Φ 12 (t -t 1 ) ė(t 1 ), ∀t ≥ t 1 . (18) Assuming e(t 1 ) and ė(t 1 ) are parallel, vector ė(t 1 ) can be expressed as

ė

Fig. 1 :

 1 Fig. 1: Switching function λ(ν) with swithching values νT and νT (dashed lines) such that λ(νT ) ≈ 0 and λ(νT ) ≈ 1.

Fig. 2 :

 2 Fig.2: Behavior of the error norm ν(t) when using: the full strategy of Secs. III-B and III-C and the estimated χ(t) (case 1blue line); the classical controller (9) and the estimated χ(t) (case 2 -red line); the classical controller (9) by employing χ = χ * (case 3 -green line); the classical controller (9) by using the real χ(t) (case 0 -black dashed line)

Fig. 4 :

 4 Fig. 4: Fig. (a): camera 3D trajectory during case 1 with arrows representing the camera optical axis. The three phases of Sec. III-C are indicated by different colors: blue -phase 1), red -phase 2), green -phase 3). Fig. (b): trajectory of the four point features in the image plane during case 1 with the same color code.

Fig. 5 :

 5 Fig. 5: Behavior of the error norm ν(t) (Fig. (a)) and of δ , the measure of misalignment between vectors e and ė (Fig. (b)).In both plots, the blue lines represent the behavior of case 1 (full implementation of the switching strategy of Sec. III-C), while magenta lines represent the direct switch from phase 1) to phase 3) without the action of vector r in (15).

  (t 1 ) = ė(t 1 )

			e(t 1 ) e(t 1 )	= ė(t 1 )	e(t 1 ) ν(t 1 )	.	(19)
	Therefore, (18) becomes
	e (20)
	Now, being ν = e and using (19), it is
	ν(t 1 ) =	e T (t 1 ) ė(t 1 ) ν(t 1 )	= ė(t 1 )	e

* (t) = Φ 11 (t -t 1 ) + Φ 12 (t -t 1 ) ė(t 1 ) ν(t 1 ) e(t 1 ), ∀t ≥ t 1 , resulting in e * (t) = ν * (t) = Φ 11 (t -t 1 ) + Φ 12 (t -t 1 ) ė(t 1 ) ν(t 1 ) e(t 1 ) = Φ 11 (t -t 1 ) + Φ 12 (t -t 1 ) ė(t 1 ) ν(t 1 ) ν(t 1 ) = Φ 11 (t -t 1 )ν(t 1 ) + Φ 12 (t -t 1 ) ė(t 1 ) , ∀t ≥ t 1 . T (t 1 )e(t 1 ) ν 2 (t 1 ) = ė(t 1 ) . Plugging ė(t 1 ) = η(t 1 ) in (20) finally yields ν * (t) = Φ 11 (t-t 1 )ν(t 1 )+Φ 12 (t-t 1 ) ν(t 1 ), ∀t ≥ t 1 , thus showing that ν * (t) ≡ ν e (t), i.e. fulfilment of condition (14).

Note that also in the case m < n the use of (10) would have increased the redundancy degree from nm to n -1.

However, we also note that similar theoretical difficulties affect most of the robotics applications in which an estimation step is plugged into the loop (e.g., whenever exploiting an Extended Kalman Filter for feeding a motion controller with the reconstructed state).