Olivier Kermorgant

Franc ¸ois Chaumette

Combining IBVS and PBVS to ensure the visibility constraint

Keywords:

In this paper we address the issue of hybrid 2D/3D visual servoing. Contrary to popular approaches, we consider the position-based visual servo as the core of our scheme. 2D information is only added when necessary, that is when the observed object is about to leave from the field of view, even partially. The injection of 2D information is tuned by only one parameter, that simply defines the area where a point is considered to be near to the image border. Simulations allow the comparison of the proposed scheme with both positionbased and hybrid schemes, showing nice properties. Finally, experiments are performed on a real object that is tracked by a pose estimation algorithm. Results validate the approach by showing the object stays entirely in the image field of view.

I. INTRODUCTION

A main issue in visual servoing is the choice of the set of visual features. Indeed, contrary to classical sensors such as laser range sensors or robot encoders, cameras deliver highdimensional data that need to be processed in order to obtain information about the robot environment. Visual servo are classically divided in two main approaches [START_REF] Chaumette | Visual servo control. i. basic approaches[END_REF]. Image-based visual servoing (IBVS) focus on using geometric features that can be directly obtained from the image. When used with a teaching-by-showing to specify the desired robot pose, these schemes are known to be robust to calibration and model errors. The main drawback is that the induced 3D trajectory is not predictable and may be unsatisfactory. Also, jacobian singularities or local minima may exist [START_REF] Chaumette | Potential problems of stability and convergence in image-based and position-based visual servoing[END_REF]. On the opposite, position-based visual servoing (PBVS) uses the 3D pose of the camera as visual features and is globally asymptotically stable when the pose is assumed to be perfectly estimated. Additional information such as true camera parameters and a 3D model of the observed object are necessary to estimate the camera pose from the acquired images. For some PBVS schemes, the induced 3D trajectory is a 3D straight line, but there is no control in the image space and the object may get out of the field of view (FoV). Another design of PBVS does not lead to a straight line but implicitly ensures that the object reference point stays in the image [START_REF] Thuilot | Position based visual servoing: keeping the object in the field of vision[END_REF]. A similar approach is called 2 1/2D VS, where some of the 3D features are replaced by 2D information, leading to a set of 6 features that allow analytical proof of convergence and study the sensibility to calibration errors. Popular choices are to use 2D coordinates of an image point together with 3D translation (resp. rotation) along the z-axis and the whole vector for 3D rotation (resp. translation) [START_REF] Malis | 2-1/2d visual servoing[END_REF]. This strategy ensures the reference point stays in the image, yet the object may partially leave the FoV. In [START_REF] Morel | Explicit incorporation of 2d constraints in vision based control of robot manipulators[END_REF] another 6-feature-set is designed to cope the visibility issue: 2D coordinates and 3D rotation together with the radius of the circumcircle of the object projection. Yet, this shape may not be suited for all 3D objects, and a planning strategy must be done in the general case. When using more than 6 features, merging 2D and 3D information leads to various hybrid approaches. In [START_REF] Chesi | Keeping features in the field of view in eye-in-hand visual servoing: A switching approach[END_REF], PBVS translational and rotational motion are switched when the image points are near to the border. The goal is to perform only the 3D motion that will keep the points in the image. Isolating the z-axis is also considered in [START_REF] Corke | A new partitioned approach to imagebased visual servo control[END_REF] with a partitioned strategy. In practice, this leads to backward translational motions which is not an optimal 3D trajectory. In [START_REF] Gans | Stable visual servoing through hybrid switched-system control[END_REF], a switching between IBVS and PBVS is proposed. A maximum error is defined for each scheme, that makes the system switch to IBVS (resp. PBVS) if the 2D (resp. 3D) error norm is too high. However the maximum acceptable 2D error may be difficult to define: if too high, a point may be able to reach the image border. If too small the scheme may stay in IBVS mode. Finally, a hybrid control law has been proposed in [START_REF]Visual servoing by optimization of a 2D/3D hybrid objective function[END_REF] with a 5D-objective function that allows determining the best weighting between IBVS and PBVS. Once again, the tuning may be difficult in practice and does not ensure the visibility because the 2D weights are bounded. To cope with partial visibility lost, a scheme has been proposed in [START_REF] García-Aracil | Continuous visual servoing despite the changes of visibility in image features[END_REF] where the features can be continuously added and removed from the task when they enter or get out of the field of view. As a point is approaching the image border, its participation in the control law is decreasing and becomes null before it is no more visible. Still, the number of observed points must be enough to perform the pose estimation. Our work is part of the hybrid approaches that let PBVS keep the points in sight. The main idea is to modify the PBVS as little as possible since this scheme provides nice 3D trajectories. To do so, we choose the opposite approach of [START_REF] García-Aracil | Continuous visual servoing despite the changes of visibility in image features[END_REF]: instead of not taking into account the points that are about to leave the field of view, we inject them into the control law so that they stay in the image. Contrary to previous hybrid schemes, all point coordinates are treated separately depending on their distance to the image border. First, visual servoing classical control laws are recalled. In Section III we expose the proposed hybrid scheme and we proof the local stability. We also consider particular issues due to discretization. Finally, we compare our approach to other popular schemes in both simulations and experiments.

II. VISUAL SERVOING SCHEMES

In this section we recall the modeling of classical visual servoing control laws. VS schemes consist in defining a robot task by an error function to be minimized:

e = s -s * (1)
where s is a vector of m visual features with s * being their desired values. In the following, we assume m ≥ 6. Once the visual features have been chosen, the time variation of s is directly related to the camera velocity screw v s by:

ṡ = ė = L s v s (2)
where L s is the interaction matrix related to s and can usually be computed analytically [START_REF] Chaumette | Visual servo control. i. basic approaches[END_REF]. Assuming the robot can be controlled with the camera velocity, (2) leads to the following control law:

v s = -λ L s + e (3
) where L s + is an estimation of the Moore-Penrose pseudoinverse of L s , that ensures at best that the error e is exponentially minimized in terms of euclidean norm. We now recall the VS schemes that are considered in our hybrid approach.

A. Position-based visual servoing

In PBVS [START_REF] Wilson | Relative end-effector control using cartesian position based visual servoing[END_REF], a computer vision method retrieves the 3D pose (position and orientation) of the camera, which is then used as the visual features. If a CAD model of the object is known, tracking the edges of the object [START_REF] Comport | Efficient model-based tracking for robot vision[END_REF], [START_REF] Drummond | Real-time visual tracking of complex structures[END_REF] is a popular pose estimation approach. On the other hand, a model-free method has been presented in [START_REF] Malis | 2-1/2d visual servoing[END_REF], allowing for the homography estimation from a set of corresponding points. The pose matrix that transforms points from object frame to camera frame is an element of the group of rigid body transformations SE(3) and can be written:

c M o = c R o c t o 0 1 (4)
where c R o ∈ SO(3) is a rotation matrix and c t o ∈ R 3 is a translation vector. Classically, 3D features are s = (t, c * θu c ) where c * θu c expresses the angle and the axis of the 3D rotation that the robot has to achieve. Two popular choices exist for the translation, determining the behavior of the robot [START_REF] Chaumette | Visual servo control. i. basic approaches[END_REF]. 1) 3D straight line: t can be chosen to express the transformation between the current camera frame F c and the desired one F c * , that is s = ( c * t c , c * θu c ). In this case the corresponding desired feature is a null vector, and the interaction matrix is known to be bloc-diagonal, inducing decoupled translational and rotational motions. The corresponding camera trajectory is a straight 3D line. Although this scheme, that is denoted in this paper as c * t c -PBVS, is popular for its very nice behavior in the 3D space, no control at all is done in the image and the visual features that allow for the pose estimation may be lost.

2) 2D straight line: Another choice is to use the translational vector relative to the observed object frame, that can be retrieved from the pose estimation algorithm. In this case, s = ( c t o , c * θu c ) and the interaction matrix L s is bloctriangular. The corresponding control law, denoted c t o -PBVS, implicitly ensures that the reference point of the object frame F o draws a straight line trajectory in the image. On the other hand, the camera trajectory does not follow a 3D straight line as soon as a rotational motion has to be performed. When the reference point is in the center of the object this scheme is very similar to 2 1/2D VS [START_REF] Malis | 2-1/2d visual servoing[END_REF]. However, even if one point is ensured to stay in the image, nothing can be said for the other points or visual elements used in the pose estimation. Visibility is improved in [START_REF] Morel | Explicit incorporation of 2d constraints in vision based control of robot manipulators[END_REF] but problems still remain depending on the object shape and the camera pose. In the following the interaction matrix of the PBVS scheme is denoted L 3d .

B. Image-based visual servoing

IBVS uses features that can be measured in the image. Although many choices exist for 2D visual features, most hybrid strategies use the cartesian coordinates of image points. These features are known to induce large 3D motion in particular cases [START_REF] Chaumette | Potential problems of stability and convergence in image-based and position-based visual servoing[END_REF], however they are well-suited for the problem of visibility lost: indeed, as images are rectangular the distance to the border expresses naturally. The analytical expression of the interaction matrix of an image point, denoted L 2d , depends both on its image coordinates (x, y) and on its depth Z. While x and y can be retrieved from the image, a common issue in IBVS is that the depth has to be estimated. However in hybrid control the pose is assumed to be estimated hence point depths can be easily retrieved. The IBVS control law induces straight line trajectories of the selected points in the image. The main drawback is a lack of control in 3D space. Actually, in the case of a task controlling all the 6-DOFs, at least 4 points have to be used in order to avoid potential singularities of L 2d [START_REF] Chaumette | Potential problems of stability and convergence in image-based and position-based visual servoing[END_REF]. As L 2d is of rank 6 at most, it is overdetermined, which makes it possible to reach a local minimum, and prevents from ensuring perfect straight line trajectories for each point in some cases.

Finally, 2D virtual points can be computed from any 3D point when the camera pose c M o is known. Denoting o x the coordinates of a 3D point in the object frame, the corresponding coordinates c x = (X, Y, Z) in the camera frame yield:

c x 1 = c M o o x 1 (5)
2D coordinates can then be expressed with:

x = X/Z y = Y /Z (6)
Hence, the interaction matrix of the image projection of a 3D point can be computed even if the point is not actually extracted from the image. Similarly, the corresponding desired features s * = (x * , y * ) can be computed from the desired camera position c * M o if this matrix is known. We now show how a set of 2D points can improve PBVS.

III. 2D-AUGMENTED PBVS

A. Hybrid control law

We assume a PBVS scheme is performed with the corresponding task denoted e 3d = s 3ds * 3d . Considering a set of p 3D points ( o x 1 , . . . , o x p ) attached to the observed object frame, a 2D task e 2d = s 2ds * 2d can be defined as described in Section II-B. We define the global weighted task of dimension (6 + 2p) by:

e H = H e 3d e 2d = He (7)
where H is a positive semi-definite diagonal activation matrix that allows continuous adding and removing of some features [START_REF] Mansard | Continuity of varying-feature-set control laws[END_REF]. The global minimum of e corresponds to the desired pose of the sole PBVS scheme, that is s 3d = s * 3d . Indeed, the latter is equivalent to having c M o = c * M o , hence s 2d = s * 2d whatever the value of H. The derivative of the task is ėH = H ė + Ḣe. The second term can be neglected in a neighborhood of the desired position [START_REF] Chaumette | Visual servo control. i. basic approaches[END_REF], leading to the following error time derivative:

ėH = H L 3d L 2d v s = HL s v s (8)
which, by analogy with (3), leads to the control law:

v s = -λ(H L s ) + He (9)
In the considered approach, H allows for the continuous injection of 2D features into the PBVS scheme. The weighting matrix is written under a particular form:

H = I 6 0 0 H 2d ( 10
)
where the weights are always equal to 1 for the 3D part, while the 2p weights corresponding to the 2D features are varying. In the following, the resulting control law is denoted 2D-augmented PBVS. Such a control law with varying weights is known to be continuous under three conditions. HL s and He have to be continuous, which is ensured by the computation of the weights that is exposed in Section III-C. The third condition is the pseudo-inverse being continuous, that is also ensured in our case since HL s is always of full rank 6 thanks to the PBVS scheme that has non-null weights.

The nearest approach to the proposed one is found in [START_REF]Visual servoing by optimization of a 2D/3D hybrid objective function[END_REF] and called 5D VS. In this case, the control law yields:

v s = -λ L s + h 3d I 6 0 0 h 2d I 2p e (11)
where scalars h 3d and h 2d allow for balancing the PBVS and the IBVS schemes. However, all 2D features are treated as a whole IBVS. Also, not taking into account the weighting matrix in the pseudo-inverse induces a conservative behavior in the case of small weights. Indeed, the zeroed error components are still taken into account, which is not the case with our approach as we will show in the next section. Finally, the weighting strategy that is proposed for 5D VS does not ensure that the points stay in the image FoV.

B. Stability analysis

In the case of null 2D feature weights H 2d = 0, the proposed control law ( 9) is equivalent to the PBVS part:

v s = -λ L 3d H 2d L 2d + e 3d H 2d e 2d = -λ L 3d 0 + e 3d 0 = -λ L 3d + 0 e 3d 0 = -λL + 3d e 3d ( 12
)
Our scheme is thus different from [START_REF] Malis | 2-1/2d visual servoing[END_REF]. In the following of this section we assume at least one weight is non-null. From ( 8) and [START_REF]Visual servoing by optimization of a 2D/3D hybrid objective function[END_REF], HL s is of dimension ((6 + 2p)×6) and is of full rank 6. Following the classical stability analysis of overdetermined VS schemes [START_REF] Chaumette | Visual servo control. i. basic approaches[END_REF], local stability can be ensured as long as L s is sufficiently well estimated. However, potential local minima may exist in the configurations where H 2 e ∈ Ker L ⊤ s . In practice, we have not encountered any. We modify as little as possible the PBVS scheme and results will show that the domain of stability is very large.

C. Computation of the weights

In this section we first show that a sufficiently high weight ensures that the corresponding feature stays in the image. We then expose our weighting strategy for the 2D features.

1) 2D features convergence:

A classical Lyapunov function associated with the task ( 7) is V (e H ) = 1 2 e ⊤ H e H . Assuming we are in the domain of local stability, the time derivative of V yields:

V = ∂V ∂e ė = 6+2p i=1 h 2 i e i ėi < 0 (13)
Let i > 6 be a 2D feature index. We consider the desired feature s * i is strictly inside the image FoV. A sufficient condition for that feature to stay in the image is e i ėi ≤ 0, which is ensured in three cases:

1) e i = 0: thanks to the decoupled decrease, ėi is very small when e i = 0. Since the feature s i = s * i is strictly inside the image, it does not leave the FoV at the next iteration.

2) ėi = 0: the feature has no motion in the image, hence it stays in the FoV at the next iteration. 3) e i ėi < 0: in that case, ( 13) is equivalent to:

h 2 i > - 1 e i ėi j =i h 2 j e j ėj ( 14
)
where j is the index for the other features.

At any position there exists a sufficiently high weight that prevents an image point from leaving the field of view. However, when several feature points have to be considered simultaneously, the exact set of sufficient values may not be possible to determine: indeed, in ( 14) the minimum value of h i depends on the other weights (h j ) j =i . In the next section, the weights are computed independently from one another. This prevents from computing the smallest weights, but we will see in Section V that the obtained values are still small in practice.

2) Weighting strategy: Classically when dealing with control in the image space, the smaller distance from the 2D points to the image border is computed. This value is then used to weight the IBVS scheme [START_REF] Chesi | Keeping features in the field of view in eye-in-hand visual servoing: A switching approach[END_REF], [START_REF] Hafez | Probabilistic Integration of 2D and 3D Cues for Visual Servoing[END_REF]. In our case, each 2D feature is treated separately . Let (x -, x + , y -, y + ) be the image borders. We define the image safe region S = [x s-, x s+ ] × [y s-, y s+ ] with:

x s-= x -+ρ(x + -x -)

x s+ = x + -ρ(x + -x -) , y s-= y -+ρ(y + -y -) y s+ = y + -ρ(y + -y -) where ρ ∈ [0, 0.5] is a tuning parameter. We have shown that a sufficiently high weight ensures that the feature stays in the FoV. Hence, we propose the following weighting function:

h(x) =      x-x s+ x + -x if x > x s+ x-x s- x --x if x < x s- 0 otherwise (15) h(y) =      y-y s+ y + -y if y > y s+ y-y s- y --y if y < y s- 0 otherwise (16)
h is null in the safe region and increases to ∞ as the feature approaches the image border. The condition ( 14) is thus ensured when any point is close to the border. All weights being continuous, the corresponding control law is continuous. Since very high values are never encountered in practice, they do not endanger the conditioning of HL s for computing the pseudo-inverse. The only parameter for the tuning is ρ, that defines the image safe area.

IV. SIMULATION RESULTS

The considered schemes are evaluated in a simulation environment with ViSP software [START_REF] Marchand | Visp for visual servoing: a generic software platform with a wide class of robot control skills[END_REF]. The object simply consists in four coplanar 3D points forming a square. First, we present the results of exhaustive runs, then the comparison with other schemes is exposed.

A. Exhaustive tests

We have generated a set of 100 random poses such that the 4 points are in the image safe area defined by ρ = 0.05. All combinations of initial and desired poses, that is 9900 runs, are performed with our control law based on c * t c -PBVS. All runs converge without leaving the image FoV with the control gain of λ = 0.1. Furthermore, even if only local stability has been proven, all runs converge to the global minimum. Over all the runs, the maximum weight is less than 20, and the mean of the maximum weight of each run is about 2.4. This reveals 2D features can very easily cope with the visibility constraint without impacting the control law conditioning. Still, as exhibited in [START_REF] Gans | Stable visual servoing through hybrid switched-system control[END_REF] potential issues due to discretization may be encountered in the case of high control gain when some points are very near to the border. An easy way to cope with such problems is to use an adaptative gain that decreases as a point approaches the image border.

B. Comparison with PBVS

Four schemes are compared in this section: c t o -PBVS, c * t c -PBVS, 5D VS [START_REF]Visual servoing by optimization of a 2D/3D hybrid objective function[END_REF] and 2D-augmented PBVS, with an initial pose making it necessary to perform both translational and rotational motions. The two hybrid schemes are performed together with the c * t c -PBVS, that is the one inducing a straight 3D trajectory and the worst image behavior. c t o -PBVS performs well as seen in Fig. 1a. In the considered case, the object reference point is in the middle of the square and c t o -PBVS is similar to 2 1/2D VS. No control is done in the image except for the straight line trajectory of the object center, yet none of the 4 points leave the FoV. We will see in the next section that it can happen for other configurations. On the opposite, Fig. 1b shows that c * t c -PBVS induces a large motion out of the image. The 5D VS is able to reduce the motion, but Fig. 1c shows the points still leave the FoV because of the bounded weights induced by this strategy. Finally, 2D-augmented PBVS is represented in Fig. 1d and is able to keep the points in sight. The trajectory change for the 2D points is particularly visible in the unsafe area, where the image border avoidance is performed. The two successful schemes are compared in terms of velocity in Fig. 2. It is interesting to notice that the camera velocities are very similar, although the control law are based on different schemes. Actually, the behaviors differ at the very beginning in Fig. 2b. Important variations are about v x and ω y , which are the main velocities that control the x-coordinate of the 2D points, that matches to Fig. 1b showing that in c * t c -PBVS the points leave the FoV along the image x-axis.

The global error of 2D-augmented PBVS is represented in

V. EXPERIMENTAL RESULTS

Experiments are carried on a 6 DOFs gantry robot (see the video accompanying this paper). The camera and the robot are calibrated. The camera observes a mechanical object, the CAD model of which being known. During the task, the object lies on a textured surface. The edges are tracked to allow for the pose estimation [START_REF] Comport | Efficient model-based tracking for robot vision[END_REF] at camera rate (30Hz). Three schemes are compared: c t o -PBVS, c * t c -PBVS, and 2D-augmented PBVS. As in Section IV, the latter is performed with c * t c -PBVS. This time, the 3D points are the ones defining the 24 nodes of the CAD model. In order to reduce the number of considered 3D points, one could use the set that defines the model envelope, since the inner points can never leave the FoV if the outer points are in the image.

The image behavior of the 3 schemes are represented in Fig. 4. The safe area is represented in red dotted line. The current projected CAD model is in green, while the desired one is in cyan. This time, c t o -PBVS makes some points leave the image as can be seen in Fig. 4b. This occlusion prevents the successful tracking of the object, which leads to a convergence that does not correspond to the desired camera pose: the edges that are displayed in Fig. 4c are not the actual object edges. c * t c -PBVS induces a trajectory that lacks of control in the image and quickly loses the object (see Fig. 4f). Our control law allows for the convergence of the scheme. The 2D features that are used are represented in orange. They are in the left side in the first images, that is the direction the c * t c -PBVS leaves the FoV. As can be seen in Fig. 4i, during the task several 2D features have been activated and deactivated. Contrary to the simulation case, the final pose corresponds to the unsafe area for some points. This does not prevent the scheme from converging. The 3D behavior of the camera is represented in Fig. 5. A large rotational motion around the optical axis can be seen in Fig. 5a. Although our scheme is based on the c * t c -PBVS where the 3D trajectory is a straight line, Fig. 5c shows that here this is not the case at all. However, the nice convergence to the desired pose is observed in Fig. 5b.

Fig. 6a shows that several 2D features have non-null weights at the end of the task. They correspond to the final points being in the unsafe area in Fig. 4i. More particularly, 4 features have non-null weights at iterations 20-80. They correspond to the object corner being in the unsafe area in terms of x-coordinates in Fig. 4h. In Fig. 6b we can see these 4 features induce a large 2D error, that prevents from leaving the FoV. After iteration 100, very small 2D error is sufficient for keeping the object in the image. As announced in Section III-C, the corresponding weights are not high: the maximum value is about 2.6 and corresponds to a feature that is about in the middle of the unsafe area.

VI. CONCLUSION

A new approach has been proposed to address the classical balance between PBVS and IBVS scheme. Starting with a PBVS that is known to have nice 3D properties, we use only the 2D features that are necessary for keeping the object in the field of view. The corresponding control law is locally stable and has its global minimum at the PBVS desired pose. The parameter for the injection of the 2D features sets the safe distance to the image border. Comparison with classical PBVS schemes show that a few 2D features allow to ensure the visibility constraint.

Fig. 1 .

 1 Fig. 1. Image behavior for the 4 schemes. c to-PBVS (a) and 2D-augmented (d) keep the points in the image while c * tc-PBVS (b) and 5D VS (c) lose visibility.

Fig. 2 .Fig. 3 .

 23 Fig. 2. Camera velocity in c to-PBVS and 2D-augmented PBVS. The velocities are similar although the schemes are different.

Fig. 4 .

 4 Fig.4. Evolution of the acquired image for c to-PBVS (top), c * tc-PBVS (middle) and 2D-augmented PBVS (bottom). In c to-PBVS the object partially leaves the FoV, inducing bad pose estimation. In c * tc-PBVS the object quickly leave the FoV and the task fails. Our scheme converges with the desired position being unsafe for some points Fig.3. 3D convergence to the desired pose is clearly visible in Fig.3a. The weighted 2D error, that is H 2d e 2d , is shown in Fig.3b. As can be seen, only 4 features out of 8 are used in the servo. As expected the first one is a x-coordinate (red curve), inducing the velocity change that can be seen in Fig.2b. Basic correspondences can be done between Fig.3band Fig.1d:• During the task, 2 x-coordinates and 2 y-coordinates have been activated. They correspond to the unsafe trajectories that appear on the left or above the safe area in Fig.1d. • Only point 2 has both coordinates activated. Hence, it is the only point approaching the top left hand corner in Fig.1d.

Fig. 5 .

 5 Fig. 5. 3D behavior of our scheme. Camera velocity (a), pose error (b) and 3D trajectory (c). 2D features slightly modify the trajectory (c).

Fig. 6 .

 6 Fig.6. 2D behavior of our scheme. Several weights are non-null at the end of the task, yet the 2D error is null as the points have converged to their desired position in the image.


Olivier Kermorgant

Franc ¸ois Chaumette

Avoiding joint limits with a low-level fusion scheme

Keywords: Joint limits avoidance, sensor fusion, multisensor, visual servoing

Joint limits avoidance is a crucial issue in sensorbased control. In this paper we propose an avoidance strategy based on a low-level data fusion. The joint positions of a robot arm are considered as features that are continuously added to the control scheme when they approach the joint limits, and removed when the position is safe. We expose an optimal tuning of the avoidance scheme, ensuring the main task is disturbed as little as possible. We propose additional strategies to solve the particular cases of unsafe desired position and local minima. The control scheme is applied to the avoidance of joint limits while performing visual servoing. Both simulation and experimental results illustrate the validity of our approach.

I. INTRODUCTION

Robot control from sensor inputs leads to particular issues, including joint limits avoidance, since a task that is defined in the sensor space may lead to motions that reach the robot joint limits. Different aspects of redundancy are classically used to solve this problem. The gradient projection method (GPM) has been widely used [START_REF] Siciliano | A general framework for managing multiple tasks in highly redundant robotic systems[END_REF], [START_REF] Yoshikawa | Basic optimization methods of redundant manipulators[END_REF], [START_REF] Zghal | Efficient gradient projection optimization for manipulators with multiple degrees of redundancy[END_REF] and consists in modeling the avoidance with a cost function, the gradient of which is projected on to the null space of the sensor-based task. In that case the avoidance scheme cannot disturb the main task, hence the robot joint limits may be reached in some cases. Variations of GPM have been proposed in [START_REF] Antonelli | Prioritized closedloop inverse kinematic algorithms for redundant robotic systems with velocity saturations[END_REF] for the scaling of a velocity saturation scheme, and in [START_REF] Mansard | Directional redundancy: a new approach of the redundancy formalism[END_REF], where a nonlinear projection improves the injection of the secondary task. This approach has been recently extended in [START_REF] Marey | New strategies for avoiding robot joint limits: Application to visual servoing using a large projection operator[END_REF], where full-rank tasks are considered and where the scaling of the avoidance scheme is optimal. However, problems may still occur when avoiding several limits at the same time. The other popular schemes consist in somehow determining the best compromise between performing the sensor-based task, and avoiding the joint limits. This can be expressed with a weighted least norm formulation [START_REF] Chan | A weighted least-norm solution based scheme for avoiding joint limits for redundant joint manipulators[END_REF] or within a LQ scheme [START_REF] Nelson | Strategies for increasing the tracking region of an eye-in-hand system by singularity and joint limit avoidance[END_REF]. The classical drawback of joint limits avoidance methods is that it is difficult to ensure limits avoidance and main task performing at the same time. Also, particular configurations can endanger the strategies, for instance if the main task is full rank or if the robot desired position corresponds to an unsafe joint position. Our work uses the low-level sensor fusion method [START_REF] Kermorgant | Multi-sensor data fusion in sensorbased control: application to multi-camera visual servoing[END_REF] to merge the sensor data with the joint positions into a single task. This allows us to propose an optimal tuning of the avoidance scheme, that disturbs the main task as little as possible and takes into account full-rank main tasks. Additional strategies are proposed to solve issues that may occur in particular Olivier Kermorgant and Franc ¸ois Chaumette are with INRIA Rennes-Bretagne Atlantique, Campus de Beaulieu, Rennes, France. E-mail: Firstname.Name@inria.fr configurations: first, the case of unsafe desired position is considered. We then address the problem of local minima that may appear even the main task is globally stable. Section II introduces the general modeling, then the analytical computation of the optimal tuning is exposed in Section III. Simulations are presented in Section IV to illustrate specific aspects of the proposed scheme. Finally, Section V exposes experimental results.

II. GENERAL MODELING

A. Sensor-based control

We consider a robotic system equipped with k sensors providing data about the robot pose in its environment. Each sensor S i delivers a signal s i of dimension m i with k i=1 m i = m and we assume m ≥ 6. The low-level fusion scheme [START_REF] Kermorgant | Multi-sensor data fusion in sensorbased control: application to multi-camera visual servoing[END_REF] defines the m-dimensional signal of the multisensor set as s s = (s 1 ,. . ., s k ). Considering a reference frame F e in which the robot is controlled, the time derivative of s s is directly related to the robot velocity screw v e : ṡs = L s v e (1) with:

L s = LW e =    L 1 . . . 0 . . . . . . . . . 0 . . . L k       1 W e . . . k W e    (2)
where L ∈ R m×6k contains the interaction matrices of the sensors [START_REF] Chaumette | Visual servo control. i. basic approaches[END_REF] and W e ∈ R 6k×6 contains the screw transformation matrices, making L s ∈ R m×6 the global interaction matrix of the task [START_REF] Kermorgant | Multi-sensor data fusion in sensorbased control: application to multi-camera visual servoing[END_REF]. Denoting e J q ∈ R m×n the robot jacobian, ṡs can be expressed with the articular velocity q: ṡs = L s e J q q = J s q (3) and a classical controller is then:

q = -λJ + s e s (4)
where e s = s s -s * s is the task error, depending on the desired sensor features s * s .

B. Joint limits avoidance

When dealing with joint limits avoidance, we want to perform the defined sensor-based task unless the joints are close to their limits, denoted q -and q + . To do so, a safe interval [q s-, q s+ ] is classically defined for each joint:

q s- i = q - i + ρ(q + i -q - i ) q s+ i = q + i -ρ(q + i -q - i ) (5)
where ρ ∈ [0, 0.5] is a tuning parameter. In the redundancy framework, a projection operator is classically used so that the avoidance task is realized at best under the constraint that the main task is not disturbed [START_REF] Siciliano | A general framework for managing multiple tasks in highly redundant robotic systems[END_REF], [START_REF] Yoshikawa | Basic optimization methods of redundant manipulators[END_REF], [START_REF] Zghal | Efficient gradient projection optimization for manipulators with multiple degrees of redundancy[END_REF]. However, joints may be reached if the avoidance task is in the null space of the projection operator.

C. Joint positions as additional features

Assuming that a subset q u = D q q of p robot joints are in an unsafe position, the low-level fusion scheme (3) can be increased by adding a term corresponding to the unsafe joint position error e q = q uq * u , with:

q * i = q s- i if q i ≤ q s- i q s+ i if q i ≥ q s+ i (6)
The framework of varying-feature-set [START_REF] Mansard | Continuity of varying-feature-set control laws[END_REF] allows continuous adding or removing task components by defining a weighted error e H = He where H is a diagonal activation matrix. The error derivative yields ėH = H ė + Ḣe, where the second term is usually neglected, that is:

ėH = H ṡs qu = H J s D q q = HJ q (7)
where J ∈ R (m+p)×n is the global jacobian. The corresponding control law is computed the same way than in Section II-A and becomes: q = -λ(HJ) + He (8)

Several schemes have been designed with such a weighting matrix within a set of sensor-based features: in [START_REF] Comport | Statistically robust 2-d visual servoing[END_REF] the activation matrix is used to address the problem of outliers in robust visual servoing, while in [START_REF] Cheah | Region-reaching control of robots[END_REF] it defines a task in term of a desired region instead of a desired position. In [START_REF] García-Aracil | Continuous visual servoing despite the changes of visibility in image features[END_REF], the visual features are deactivated in the case of visibility lost.

In the case of joint limits avoidance, we use a particular weighting matrix:

H = Diag(1, . . . , 1, h 1 , . . . , h p ) = I m 0 0 H (9)
where the weights corresponding to the sensor features are always equal to 1, while the weights corresponding to the joints are varying in order to allow smooth activation and deactivation of the unsafe joints. In practice, taking only the unsafe joint features into account is equivalent to adding all joint components with a null weight for the safe ones. As explained in [START_REF] Mansard | Continuity of varying-feature-set control laws[END_REF], the varying-feature-set formalism ensures the continuity of the control law when HJ and He are continuous, with the pseudo-inverse also continuous. A sufficient condition for the latter is the matrix HJ being of constant rank. In the following, this condition is ensured by assuming J s is full rank. Continuity of HJ and He is ensured by the computation of the joint weights (h i ), which is exposed in Section III. In the following, we denote e s as the main task, that is when p = 0 and H = I m .

III. WEIGHTS COMPUTATION

The scheme that is presented in Section II-C is not a sum between joint velocities computed for the main task, and joint velocities computed for the avoidance task. The fusion approach draws a compromise between the main task performing and joint limits avoidance, that depends on the activation matrix H. The tuning of the avoidance scheme is a crucial issue in joint limits avoidance: if activated too slowly joints may be reached, while oscillations may appear if the avoidance scheme has a large magnitude. In the fusion formalism, as there is no projection operator preventing the q lim- q lim+ q a- q s+ q s- q a+ Joint position avoidance task from disturbing the main task, the activation matrix has to be designed with minimal weights in order to disturb the main task as little as possible. We address this problem with the following form of the activation weights:

∀i,

h i = µ i (q i )h opt i (10) where µ i (q i ) ∈ [0, 1
] is an activation function to ensure the continuity of the weights and h opt i ≥ 0 is a scale factor ensuring that the limit is avoided. We now define these two parameters.

A. Activation function

To ensure continuity of HJ and He, weights must be null at feature activation and deactivation, and increasing as the joint values vary from the safe limit to the physical limit. As in [START_REF] Cheah | Region-reaching control of robots[END_REF] we define the positions q a-and q a+ from where the activation is full:

q a- i = q - i + ρ a q (q + i -q - i ) q a+ i = q + i -ρ a q (q + i -q - i ) (11)
where ρ a q < ρ is a tuning parameter. The activation function can then be defined with a sigmoid:

µ i =        1 2 1+tanh( 1 q a+ i -qi -1 qi-q s+ i ) if q s+ i < q i < q a+ i 1 2 1-tanh( 1 q a- i -qi -1 qi-q s- i ) if q a- i < q i < q s- i 1 else ( 12
) µ i is C ∞ and smoothly increases the weight as the joint reaches the limit, with µ i (q a- i ) = µ i (q a+ i ) = 1 and µ i (q s- i ) = µ i (q s+ i ) = 0. The proposed activation function is represented on Fig. 1.

B. Scale factor

Contrary to the classical approach, low-level fusion does not add a secondary velocity value to the main task control law. Hence, scaling the avoidance scheme does not consist in compensating for the main task in the velocity space, but rather in finding a set of minimal weights that ensures joint limits are not reached. We will show that with only one joint limit to be avoided at a time, an optimal weight can be analytically computed to be minimal and sufficient to avoid the limit. On the other hand, determining the scale factor for avoiding several limits at the same time does not have an analytical solution, but the optimal weight for one joint is still a good scale factor for the avoidance scheme. We recall that we assume that J s is full rank, which ensures HJ is also full rank. With the activation matrix from (9), the proposed control law yields:

q = -λ(HJ) + H(s -s * ) = -λ J s H + H(s -s * ) (13)
From the pseudo-inverse form M + = (M ⊤ M) -1 M ⊤ that can be applied since HJ is full rank, the inverse part yields:

(J ⊤ H ⊤ HJ) -1 = (J ⊤ s J s + H2 ) -1 = adj(J ⊤ s J s + G) det (J ⊤ s J s + G) = 1 D(g) N(g) (14)
where g i = h 2 i and G = H2 . D(g) is a strictly positive polynomial of (g i ), being the determinant of a symmetric invertible matrix, and N(g) is the adjugate matrix of J ⊤ s J s + G, that is the matrix of the cofactors. N is a (n×n) full rank symmetric matrix.From the adjugate properties, N jk depends neither on g j , nor on g k . Also, N ii > 0 from the Sylvester's criterion on positive-definite matrices. The control law can then be written:

q = - λ D N J ⊤ s (s s -s * s ) + G(q -q * ) (15)
and each joint velocity yields, up to the scale factor λ D : ∀i, qi ∝ -N i c -N i G(qq * ) (16) where c = J ⊤ s (s ss * s ) is a vector not depending on g and N i is the i th row of N, thus not depending on g i . We denote N s = N(g = 0) = adj(J ⊤ s J s ). We have -N i c = qsi + qci , where qsi = -N si c depends only on the main task and qci depends on (g j ) j =i . For instance, if n = m = p = 2, setting:

J s = a b c d and c = J ⊤ s e 1 e 2 (17)
we have:

N = b 2 + d 2 + g 2 -ab -cd -ab -cd a 2 + c 2 + g 1 (18)
from which we deduce:

qs = - (b 2 + d 2 )(ae 1 + be 2 ) -(ab + cd)(ce 1 + de 2 ) (a 2 + c 2 )(ce 1 + de 2 ) -(ab + cd)(ae 1 + be 2 ) qc = - (ae 1 + be 2 )g 2 (ce 1 + de 2 )g 1 (19)
We can check that N ii > 0 and that qc1 (resp. qc2 ) depends only on g 2 (resp. g 1 ). Coming back to the general case, two cases can be distinguished: ,

• Only joint i is in its unsafe area: in that case, qi depends linearly on

g i : qi ∝ -N i c -N ii g i (q i -q * i ) = q0 i -N ii g i (q i -q * i ) (20

) This leads to two configurations C1 and C2:

the main task is reaching the limit (C1): if qsi and (q i -q * i ) have the same sign, the robot is going towards a joint limit. As N ii > 0 there exists a positive g i such that qi is null.

the main task is avoiding the limit (C2): if qsi and (q i -q * i ) do not have the same sign, the robot is moving away from the limit: self-avoidance occurs. The avoidance scheme can be ignored. From this observation, assuming we want the joint to stop when q i = q a i , the optimal value can be computed analytically from (20):

h opt i = h min i = qsi Nii(q a i -q s i ) if qsi (q a i -q s i ) > 0 (C1) 0 else (C2) (21)
q lim- q lim+ q a- q s+ q s- q a+ Joint position where h opt i = 0 corresponds to the configurations where the main task itself is avoiding the limit.

• p > 1 joints are in their unsafe area: (15) is a system of p polynomials of degree p:

∀i ∈ S = {i 1 , . . . , i p } , qi ∝ -N i c - j∈S N ij g j (q j -q * j ) (22)
Even if each polynomial is actually linear for each g i , such a system cannot be solved analytically. At the joint position q i = q a i , the proposed weight (21) leads to the following joint velocity, once again up to the scale factor λ D : ∀i ∈ S, qi ∝ qc i -

j =i N ij g j (q j -q * j ) (23)
The expression being of unknown sign, it is not possible to ensure the joint limit is avoided in the case of several unsafe joints. However, a set of weights ensuring the limits are not reached always exists, hence two strategies are proposed.

a) Close to avoidance: Increasing the optimal scale factor makes it possible to design a general scaling strategy:

h opt i = (1 + ǫ)h min i if h min i > 0 (C1) ǫ else (C2) ( 24
)
where a high ǫ > 0 makes it more likely to avoid several joint limits in critical positions, but it is never ensured. b) Strict avoidance: In the gradient projection methods, if the secondary task is in the null space of the projection operator then it is possible that a joint limit is reached. On the opposite, the proposed scheme can ensure no limit is ever reached, by having sufficiently high weights when coming close to the limit (q i > q a i ). A strict avoidance scheme can be designed from the previous one:

h opt i = (1 + h ∞ i (q i ))h min i if h min i > 0 (C1) h ∞ i (q i ) else (C2) (25)
where h ∞ i (q i ) is an adaptative gain such that h ∞ i (q a i ) = 0 and lim

qi→q lim i h ∞ i (q i ) = ∞, for example: h ∞ i (q i ) =      q a- i -qi qi-q - i if q i < q a- i qi-q a+ q + -qi if q i > q a+ i 0 else (26)
The drawback of the strict avoidance scheme is that the main task may be neglected if it is the only way to avoid a limit. Yet, the behavior is better than stopping the joint since the fusion scheme always takes into account the coupling with the main task. When avoiding several limits at the same time, configuration C2 for joint i corresponds to the configurations where the main task is avoiding the joint limit, but some Algorithm 1 Escape from local minima Require: α = 1, α + > 1, α -< 1 while performing servoing do measure end-effector velocity v e compute optimal weighting matrix H if v e < v 0 and H = 0 then α ← α + α else α ← 1 + α -(α -1) end if apply control law using weighting matrix α H end while joint velocity may be created by the avoidance of the other joint limits (see ( 23)). A compensation of the main task is thus meaningless and h opt only depends on the adaptative gain h ∞ . Fig. 2 represents the proposed weighting strategies including the activation function and the scale factor.

C. Additional strategies

This section exposes additional strategies that address the cases of unsafe desired position and local minima.

1) Unsafe desired position: If the desired position is unsafe, a full-rank main task cannot be perfectly performed as the non-null joint error will prevent the main task error from reaching zero. In that case, he activation scheme can be progressively ignored wrt. the main task convergence. To do so, we introduce a progress parameter ξ( e s ) smoothly making the activation weights null when the main task gets close to completion. In that case, h i = ξ( e s )µ i (q i )h opt i where: 27) where e 0 and e 1 are defined so that the avoidance scheme is totally ignored when the main task is close to completion, that is e s < e 0 .

ξ( e s ) =      0 if e s ≤ e 0 1 if e s ≥ e 1 1 2 1+tanh( 1 e1-es -1 es -e0 ) else (
2) Avoiding local minima: Task functions may be subject to local minima when the number of features is greater than the available DOFs, which is the case here since we consider a full-rank main task and add unsafe joints in the feature set. A local minimum is easily detected as it is necessarily a configuration where the end-effector velocity is almost null, while at least one joint weight is non-null. Indeed, the global minimum always corresponds to null weights, either because it is in the safe area or thanks to the progress parameter. A popular optimization method to escape from local minima is simulated annealing [START_REF] Brooks | Optimization using simulated annealing[END_REF], [START_REF] Fang | Design and modeling for computer experiments[END_REF] where the minimized criterion is allowed to during the scheme under a probabilistic model. In our case, we take advantage of the particular form of the weighting matrix H (see [START_REF] Kermorgant | Multi-sensor data fusion in sensorbased control: application to multi-camera visual servoing[END_REF]), where the weights for the sensor features are always equal to 1 while the joint weights are varying. The proposed strategy is described in Algorithm 1: as the end-effector velocity is approaching 0 while H = 0, the computed joint weights are artificially (f) Camera velocity for run #3 Fig. 3. Joint positions (a) and corresponding weights (c) in a task with no avoidance (green), one limit avoidance (blue) and two limits avoidance (red). Joint physical limits are in plain lines, q a and q s are in dotted lines.

Visual error (left) and camera velocity (right) are represented for run #1 (middle) and run #3 (bottom). increased by a multiplicative factor α, allowing non-optimal motion in terms of the sensor-based task. During normal convergence, α is slowly set back to 1. However, this strategy may induce oscillations. We will see in the next section that these oscillations are very small in practice.

IV. SIMULATION RESULTS

Simulations are carried for a 6-DOFs Gantry robot, the control law are implemented using ViSP software [START_REF] Marchand | Visp for visual servoing: a generic software platform with a wide class of robot control skills[END_REF]. Main task is a visual servoing (VS) using 4 image points [START_REF] Chaumette | Visual servo control. i. basic approaches[END_REF]. We use the cartesian coordinates of the current image points as visual features. It is well-known that if the initial pose makes it necessary for the camera to perform a large rotation around its optical axis, this choice of visual features induces a large translational backward motion, hence potentially reaching joint limits. First we expose the basic behavior of the avoidance scheme, then the results of exhaustive simulations are presented. In the simulations, the robot desired pose belongs to the safe area for each joint.

A. Behavior of joints and weights

The behavior induced by the proposed control law is exposed in Fig. 3. Joint positions are represented in normalized scale on Fig. 3a, the key positions q a and q s are in dotted lines. The initial and desired pose differ from a 110 deg rotation around the optical axis, inducing a large motion on joint 1. The simulation allows defining the robot joint limits such that only joints 1 and 2 may reach their limits for this task. Close to avoidance scheme is used. Three runs are performed:

1) No avoidance (green curve): Fig. 3d shows that as expected, the camera moves only along v z and ω z , allowing a decoupled decrease of the error as seen in Fig. 3c. However, joint 1 passes the limit q -.

2) Avoiding one limit (blue curve): the joint limits are defined such that only joint 1 may reach its limit. This time, the joint does not reach the limit but stops before q a-. Furthermore, although the joint is still in the unsafe area at iteration 75, one can notice in Fig. 3b that the corresponding weight is null. This reveals the avoidance scheme change from configuration C1 to configuration C2, with the VS task making the joint going towards the safe area.

3) Avoiding two limits (red curves): here the limits are defined such that joint 2 enters the unsafe area. Here, Fig. 3a shows joint 1 nearly reaches q a-while joint 2 passes q a+ , illustrating the coupling between the two. As in run #2, both weights are null at iteration 75, before the joints go back in the safe area. Once more, the avoidance configuration has changed from C1 to C2. Fig. 3e shows the visual features error for run #3. We can see the error is decreasing even during the avoidance (iterations 10-80). The corresponding camera velocity is represented on Fig. 3f. The initial motion is a pure translation and rotation along the z-axis as in Fig. 3d, then other velocity components appear when the avoidance is performed, in order to keep the visual features error decreasing. Finally, we can notice the joint weights are very small (less than 0.02), yet they are sufficient to avoid the limits. A small weight barely disturbs the sensor-based task, as can be observed by comparing Fig. 3c and Fig. 3e.

B. Exhaustive tests

In order to study the strict avoidance behavior, exhaustive runs have been carried from 690 different initial positions assuming the image plane is infinite. The initial positions have been computed using a space-filling design [START_REF] Fang | Design and modeling for computer experiments[END_REF] that ensures all the joint space is explored and the 4 points are in front of the camera. In 636 cases the system converges directly to the global minimum. The other 54 cases make the system converge to a local minimum, that is always escaped from with the strategy exposed in Section III-C.2. Fig. 4 shows such a case: the weight of joint 3 varies following the proposed strategy, inducing very small oscillations on the other joints as seen in Fig. 4a.

V. EXPERIMENTS

Experiments are carried on a 6-DOFs robot arm Adept Viper s850 (see the video accompanying this paper). The desired position is unsafe for some joints, hence the progress parameter defined in (27) is used. First, the initial position corresponds to a pure rotation around the z-axis. A second experiment is done with an initial position necessitating a more complex motion. A general view of both experiments can be found on the video accompanying this paper. Strict avoidance scheme is used.

1) Pure rotation: Here the initial and desired pose only differ from a 150 deg rotation around the z-axis. As in Section IV-A, the expected camera motion is a pure translation and rotation along the z-axis. However, as the initial pose is not safe for joints 2, 5 and 6, the avoidance scheme induces a different motion. Joint 6 (yellow curve on Fig. 5a) being near to the lower limit, the corresponding weight is high (0.8) at the beginning then quickly decreasing as the joint goes away from the limit. The visual error is decreasing, leaving smooth trajectories for the image points as seen on Fig. 5c. The large rotation around the z-axis that occurs at iterations 200-300 draws arcs of a circle in the image, then the points have a straight trajectory towards their desired position. As expected, the 3D trajectory (Fig. 5f) is a backward motion that adapts to the joint limits before converging to the desired position. The progress parameter makes it possible to reach the desired position, although it is unsafe for joints 2 and 3.

2) Complex motion: A new initial position is defined, such that a large rotation around the z-axis is still involved, together with other displacements. Here the initial position is unsafe for all joints except joint 4. Joint 1 starts very near to its upper limit, inducing a very high weight (3.7). As a consequence, and as many joints are in an unsafe position, the visual error increases during the first 100 iterations (Fig. 6b). Once the visual error starts to decrease, joints 3 and 5 stay around q a+ without any oscillation, while joint 2 quickly reaches q a-without approaching the lower limit, with a corresponding weight of about 0.2. After iteration 630, all weights are null and the system converges with a classical IBVS behavior: exponential decoupled decrease of the visual error, straight line trajectories in the image. Once more, the 3D trajectory starts with a large backward motion (Fig. 6f) , before heading to the desired position.

VI. CONCLUSIONS

The framework of multi-sensor data fusion has been applied to joint limits avoidance. The measurement of the robot joint positions are considered as additional features, and treated with a particular varying-feature-set scheme that ensures the continuity of the control law. This leads to an optimal weighting in the case of single joint avoidance, that can be also applied when several joints are in an unsafe position. We have considered the configurations where the desired position is unsafe. A strategy has also been exposed to escape from local minima. Simulations and experiments illustrate the validity of the proposed approach.

Fig. 1 .

 1 Fig. 1. Activation function for lower and upper bounds.

Fig. 2 .

 2 Fig.2. Weighting strategies for C1 and C2. Strict avoidance allows weights to increase instead of keeping equal to the optimal one. Configuration C1 is represented for the upper bound (h opt = 0.5), while C2 is represented for the lower bound. ǫ is set to 0.2.

Fig. 4 .

 4 Fig. 4. Positions and weights for the robot joints while escaping from a local minimum. Oscillations in the weight induce a slight oscillation of the robot end-effector.

Fig. 5 .

 5 Fig.5. Joint avoidance for a pure rotation. Visual error is smoothly decreasing during the task (b), the resulting trajectory is a backward motion followed by a forward motion (f).

Fig. 6 .

 6 Fig.6. Joint avoidance for a complex motion. Visual error increases before converging to 0 (b), making the points have smooth trajectories in the image (c). Initial position is unsafe for 5 joints (a) and initial weight is very high for joint 1 (d).


Amaury Dame

Eric Marchand

Video Mosaicing using a Mutual Information-based Motion Estimation Process

This paper proposes a method to achieve parametric motion estimation based on mutual information and its use in video mosaicing applications. Sum of Squared Differences (SSD) is widely considered in motion estimation. Here, we consider another metric, Mutual Information (MI), which is far less sensitive to changes in the lighting condition, to occlusions, and to a wide class of non-linear image transformation. Results under various complex conditions are presented.

I. INTRODUCTION

Image mosaics are a collection of overlapping images. The goal of the mosaicing problem is to find the transformations that relate the different image coordinates. Once the transformation between all the images is known, an image of the whole scene can be constructed. This problem requires to find a warping function that maps the coordinates of one image into the coordinate system of another image. When considering a video, one has to warp each new image into the coordinate system of the very first image of the video [START_REF] Szeliski | Image alignment and stitching: a tutorial[END_REF] [3] [8] [6]. This is basically a motion estimation process. One can consider to estimate this motion using matched keypoints as in [START_REF] Brown | Automatic panoramic image stitching using invariant features[END_REF] or using SSD based motion estimation as in [START_REF] Irani | Mosaic based representations of video sequences and their applications[END_REF] [START_REF] Gracias | Underwater video mosaics as visual navigation maps[END_REF]. The latter approach is very efficient when image sequences are considered, that is, when displacements between one frame to an other are small. Indeed, if the motion is small enough motion estimation can be achieved by an energy minimization process. Such approaches are related to the early work of Lucas and Kanade [START_REF] Lucas | An iterative image registration technique with an application to stereo vision[END_REF] that use images difference as energy [START_REF] Baker | Equivalence and efficiency of image alignment algorithms[END_REF].

These classical SSD-based approaches are based on the temporal luminance constancy hypothesis. However, it is well known that this hypothesis can be easily violated leading to important errors in the motion estimation process. Dealing with target tracking illumination variations can be taken into account for example using M-estimator [START_REF] Hager | Efficient region tracking with parametric models of geometry and illumination[END_REF] [START_REF] Odobez | Robust multiresolution estimation of parametric motion models[END_REF] or by the illumination changes as a surface that evolves over time [START_REF] Silveira | Real-time visual tracking under arbitrary illumination changes[END_REF].

To deal with occlusions, and illumination variations we propose to use the mutual information [START_REF] Shannon | A mathematical theory of communication[END_REF], [START_REF] Viola | Alignment by maximization of mutual information[END_REF] as the alignment function, that is, as we will see, robust to all this variations of appearance. Mutual information, especially studied in medical image registration, tolerates such changes. It is another metric well adapted and more robust for the tracking or motion estimation problems. Since its derivative form have been recently studied in many works [START_REF] Thévenaz | Optimization of Mutual Information for Multiresolution Image Registration[END_REF] [START_REF] Maes | Comparative evaluation of multiresolution optimization strategies for multimodality image registration by maximization of mutual information[END_REF] [5] [START_REF] Dame | Accurate real-time tracking using mutual information[END_REF], its application for mosaicing has been considered here.

In the remainder of this paper, we recall the MI principle and present the MI-based motion estimation process that allows to estimate the dominant motion in the image. Finally, results are given on various image sequence.

II. MOTION ESTIMATION

We recall here the basic principle of the differential motion estimation methods that can be used for motion estimation. These methods are based on the optimization of a similarity measure f . The goal is to estimate the displacement p of an image template I * in a sequence of images I 0 ..I t . In the mosaicing application I * = I 0 . In the case of a similarity function, the problem can be written as:

p t = arg max p (f (I * , w(I t , p))) .
(

) 1
where we search the displacement p t that maximizes the similarity between the template I * and the warped current image I t .

To solve the maximization problem, the assumption made is that the global displacement between two consecutive frames is quite small. The previous estimated displacement p t-1 can therefore be used as first estimation of the current displacement to perform the optimization of f and incrementally reach the best estimation p t .

Multiple solutions are possible to perform the iterative process and compute the update of the current displacement parameters. Indeed Baker and Matthews showed that two formulations were equivalent [START_REF] Baker | Equivalence and efficiency of image alignment algorithms[END_REF]. In our case we consider the inverse compositional formulation which considers that the update is modifying the reference image, so that ∆p is chosen to maximize:

∆p k = arg max ∆p f (w(I * , ∆p), w(I t , p k )). (2)
In this case the current parameters will be update using:

w( w -1 (x, ∆p k ), p k ) → w(x, p k+1 ). (3)
In this formulation, since the update parameters is applied to the reference image, the derivatives with respect to the displacement parameters will classically be computed using the gradient of the reference image. Thus, these derivatives can be partially precomputed and the algorithm is far less time consuming. One essential choice remains the one of the alignment function f . One natural solution is to choose the function f as the sum of the squared differences (SSD) of the pixel intensities between the reference image and the transformed current image:

p t = arg min p x∈ROI (I * (x) -I t (w(x, p))) 2 (4)
where the summation is computed on each point x of the reference template, that is, the region of interest (ROI, in our case this is the whole image) of the reference image.

As its definition suggests this similarity function is very sensitive to occlusions and illumination variations. Many solutions have been proposed to make the SSD robust. M-estimators robustifies the least squared problem toward occlusions [START_REF] Hager | Efficient region tracking with parametric models of geometry and illumination[END_REF] [12] and illumination modeling can be used to handle illumination changes [7] [14].

III. MUTUAL INFORMATION-BASED MOTION ESTIMATION A. Mutual information

Rather than comparing intensities, mutual information is the quantity of information shared between two random variables. Mutual information of two random variableI and I * is then given by the following equation:

M I(I, I * ) = h(I) + h(I * ) -h(I, I * ).
(

) 5
where the entropy h(I) is a measure of variability of a random variable I (signal, image...). If r are the possible values of I and p I (r) = P (I = r) is the probability distribution function of r, then the Shannon entropy h(I) of a discrete variable I is given by the following expression:

h(I) = - r p I (r) log (p I (r)) . (6)
The probability distribution function of the gray-level values is then simply given by a the normalized histogram of the image I. The entropy can therefore be considered as a measure of dispersion of the image histogram. Following the same principle, joint entropy h(I, I * ) of two random variables I and I * can be defined as the variability of the couple of variables (I, I * ). The Shannon joint entropy expression is given by:

h(I, I * ) = - r,t p II * (r, t) log (p II * (r, t)) (7)
where r and t are respectively the possible values of the variables I and I * , and p II * (r, t) = P (I = r ∩ I * = t) is the joint probability distribution function. In our problem I and I * are images. Then r and t are the gray-level values of the two images and the joint probability distribution function is a normalized bidimensional histogram of the two images.

As for entropy, joint entropy corresponds to a measure of dispersion of the joint histogram of (I, I * ).

The analytical formulation of a normalized histogram of an image I * is classically written as follows:

p I (r, p) = 1 N x x φ r -I(w(x, p)) (8)
p II * (r, t, p) = 1 N x x φ r -I(w(x, p)) φ t -I * (x)
where

B. Mutual information-based motion estimation

In this section we will see how to use the MI cost function with the motion estimation process presented in section II. Let us remind that the goal is to estimate the displacement parameters p t that maximizes the MI using a first estimation of the parameters p t-1 and an iterative update of the parameters.

a) Derivative function analysis: This problem implies a strong correlation between the elements of the vector p. Therefore, the use of first-order optimization method such as the gradient "descent" is not adapted. Such non-linear optimization are usually performed using a Newton's method that assume the shape of the function to be parabolic.

Newton's method uses a second order Taylor expansion at the current position p k-1 to estimate the update ∆p required to reach the optimum of the function (where the gradient of the function is null). The same estimation and update is performed until the parameter p k effectively reaches the optimum. The update is estimated following the equation:

∆p = -H -1 G (9)
where G and H are respectively the Hessian and gradient matrices of the mutual information with respect to the update ∆p. Following the inverse compositional formulation defined in equation ( 2) those matrices are equal to:

G = ∂M I(w(I * , ∆p), w(I, p)) ∂∆p (10)
H = ∂ 2 M I(w(I * , ∆p), w(I, p)) ∂∆p 2 (11)
Applying the derivative chain rules yields the following gradient and Hessian matrices:

G = r,t ∂pII * ∂∆p 1 + log pII * pI (12)
H = r,t ∂pII * ∂∆p ∂pII * ∂∆p 1 pII * - 1 pI + ∂ 2 pII * ∂∆p 2 1 + log pII * pI (13)
For the purpose of clarity, the marginal probabilities and joint probability that are actually depending on r, t, p * and ∆p are simply denoted as p I , p I * and p II * . The details of the calculation from equation [START_REF] Maes | Multimodality image registration by maximization of mutual information[END_REF] to equation ( 12) can be found in [START_REF] Dowson | A unifying framework for mutual information methods for use in non-linear optimisation[END_REF] for a direct additional formulation.

By analogy with classical Hessian computation in SSD minimization, second order derivatives are usually neglected in the Hessian matrix computation [START_REF] Thévenaz | Optimization of Mutual Information for Multiresolution Image Registration[END_REF] [START_REF] Dowson | A unifying framework for mutual information methods for use in non-linear optimisation[END_REF]. In our approach we compute the Hessian matrix using the second order derivatives that are, in our point of view, required to obtain a precise estimation of the motion.

As we can see in equation ( 12) and equation ( 13), the derivatives of the mutual information depend on the derivatives of the joint probability. Using the previous definition in [START_REF] Irani | Mosaic based representations of video sequences and their applications[END_REF] and passing the derivative operator through the summation yields the following expressions:

∂p II * ∂∆p = 1 N x x φ t -I(w(x, p)) ∂φ ∂∆p r -I * (w(x, ∆p)) ∂ 2 p II * ∂∆p 2 = 1 N x x φ t -I(w(x, p)) ∂ 2 φ ∂∆p 2 r -I * (w(x, ∆p)) .
The remaining expressions to evaluate are the variations of the B-spline function φ with respect to the update. Its derivatives are obtained using the chain rule and gives:

∂φ ∂∆p (r -I * (w(x, ∆p))) = - ∂φ ∂r ∂I * ∂∆p ( 14
)
∂ 2 φ ∂∆p 2 r -I * (w(x, ∆p)) = ∂ 2 φ ∂t 2 ∂I * ∂∆p ∂I * ∂∆p - ∂φ ∂r ∂ 2 I * ∂∆p 2 .
Finally the derivatives of the reference image intensity with respect to the update parameters ∆p is given by the following expressions:

∂I * ∂∆p = ∇I * ∂w(x, p) ∂∆p (15)
∂ 2 I * ∂∆p 2 = ∂w ∂∆p ∇ 2 I * ∂w ∂∆p + ∇I * x ∂ 2 wx ∂∆p 2 + ∇I * y ∂ 2 wy ∂∆p 2 (16)
The motivation for using the inverse compositional formulation is then obvious. The derivatives of the warp function are all computed at ∆p = 0, their values are then constant for each pixels of the template. Moreover, since the reference image is constant, all the expressions from equation ( 14) to equation ( 16) are constants and have to be precomputed only one time.

To improve the optimization process we propose to use an approximation of the Hessian matrix computed in the case of a perfect alignment between the template and the current image that is given for I(w(x, p)) = I * (x) [START_REF] Dame | Accurate real-time tracking using mutual information[END_REF].

This solution has several advantages: first it gives a definite negative Hessian matrix that yields to have a wide convergence domain; second, since the Hessian matrix used in the Newton's method is the Hessian matrix after convergence, the behavior of the optimization near convergence is optimal and the final estimated displacement parameters is very accurate; finally, the Hessian matrix, representing 80 of the computation time in one iteration, is computed only one time.

In this work we focus on mosaicing application. Usually motion is mainly a rotation and a homogoraphy is then well suited. The warp function is thus defined by the group action w : SL(3)×P 2 with x ∈ P 2 and p defines the 8 parameters of the sl(3) lie algebra associated to the SL(3) group. However, affin transformation has also been considered. All details regarding the derivatives of the chosen warp function can be found in [START_REF] Benhimane | Homography-based 2d visual tracking and servoing[END_REF].

IV. EXPERIMENTAL RESULTS

These experiments show the application of the MI -based motion estimation algorithm to the mosaicing problem. In these sequence, since some part of scene completely disappears, it is necessary to define multiple reference images. The approach is build as follows:

• Initialization: the first image is chosen as reference image, i.e. I * 0 = I 0 . • Tracking: for every frame, we compute the displacement p k between I t and I * k . • Reference Update: every 30 images, the reference image I * k is changed and defined as the current image, i.e. I * k = I t for t = 30k. Using the homography from the current image to the current reference image and the homographies between the references, we retrieve the homography between the current image and the first image. Using this homography, we can project all the images of the sequence into the mosaic image and construct the global image of the whole scene.

In the first experiment (Figure 1 and 2), the overlapping images are simply a compressed sequence of 3600 images obtained from Youtube. The aerial scene is acquired from a camera embedded on a flying UAV and shows the ground that is approximately 1 kilometer away from the camera. Since this distance is very large, the scene can be approximated as a plane and tracked using homographies. During the acquisition of the sequence, the camera is moving forward and is rotating around the vertical axis.

In Figure 1 we show some images from the sequence. This sequence has been downloaded on Youtube and is affected by the H264 coding artifacts. We can also note the poor quality of the images. As we can see in the resulting mosaic image. Despite this poor quality, the resulting mosaic presented in Figure 2 shows the accuracy of the MI tracker. Since the camera is making an entire revolution, the first and last images are overlapping. A small shift occurs between the first and last estimated positions: we highlight two corresponding patterns that should have been at the same location on the mosaic. Let us note that nothing has been performed to reduce the drift (such as the bundle adjustment approach proposed by [START_REF] Brown | Automatic panoramic image stitching using invariant features[END_REF]). Considering the template update problem and the planar assumption, the estimated homographies are accurate. The same experiment was performed using the SSD tracker. In this case, due to the noise, blur, and illumination variations, this registration approach diverges after a few iterations.

The second experiment presents a mosaic build from more than 10000 images. Image are extracted from a highly compressed video. The camera was attached to a free flying balloon flying over Paris. Figure 3 shows three steps of the mosaic construction.

In this last experiment (see mosaic in Figure 4), we consider a sequence extracted from the John Ford movie "she wore a yellow ribbon". In that case an affine motion model was consider. The interest of this sequence is that some cavalrymen are moving all along the sequence and, therefore, act as important occlusions as can be seen on Figure 5.

  x are the points of the region of interest in the image, N x is the number of points and t are the possible values of I * (x), i.e. t ∈ [0, N c ]. Let us note that to have a smooth mutual information it is important to maintain the low (N c = 8 in our implemantation) the number of bins of the histogram (and thus to scale image intensity between Ī ∈ [0, N c ]. In the classical formulation φ is a Kronecker's function: φ(x) = 1 for x = 0 and φ(x) = 0 otherwise. So that each time I * (x) = i the ith histogram bin value is incremented. Nevertheless, several solutions have been proposed to simultaneously smooth the mutual information function and keep its accuracy[START_REF] Viola | Alignment by maximization of mutual information[END_REF] [START_REF] Maes | Multimodality image registration by maximization of mutual information[END_REF]. Our approach is based on the use of B-spline functions for φ[START_REF] Maes | Multimodality image registration by maximization of mutual information[END_REF].

Fig. 4 .

 4 Fig.4. Mosaic created from the John Ford movie "she wore a yellow ribbon". A affine motion model was considered. Note that some cavalrymen are moving all along the sequence. Despite these disturbances, motion is correctly estimated.

Fig. 1 .

 1 Fig. 1. Some overlapping key images used for the mosaicing application.

Fig. 2 .Fig. 3 .

 23 Fig. 2. Resulting mosaic image: despite the poor quality of the sequence and the approximation that the scene is planar, the final displacement between the first and last image is accurate. The red and green contours show the position of one physical pattern in the first and last images of the sequence.

Fig. 5 .

 5 Fig. 5. Three images used for the "yellow ribbon" mosaic construction.