Interval-based cooperative UAVs pose domain characterization from images and ranges

Ide-Flore Kenmogne, Vincent Drevelle and Eric Marchand

Abstract-An interval-based approach to cooperative localization for a group of unmanned aerial vehicles (UAVs) is proposed. It computes a pose uncertainty domain for each robot, i.e., a set that contains the true robot pose, assuming bounded error measurements. The algorithm combines distances measurements to the ground station and between UAVs, with the tracking of known landmarks in camera images, and provides a guaranteed enclosure of the robots pose domains. Pose uncertainty domains are computed using interval constraint propagation techniques, thanks to a branch and bound algorithm. We show that the proposed method also provides a good point estimate, that can be further refined using nonlinear iterative weighted least squares. Results are presented for simulated two-robots configurations, for experimental data, and compared with a classical Extended Kalman Filter.

I. INTRODUCTION

In the last few years, unmanned aerial vehicles (UAVs) have significantly attracted attention and interest in several fields of applications involving multi-robot tasks, like searching and coverage missions [START_REF] Maza | Multiple UAV cooperative searching operation using polygon area decomposition and efficient coverage algorithms[END_REF], cooperative manipulation [START_REF] Michael | Cooperative manipulation and transportation with aerial robots[END_REF], and camera based formation control [START_REF] Schiano | Bearing rigidity maintenance for formations of quadrotor UAVs[END_REF]. It appears that a team of robots can perform more efficiently tedious tasks like exploration [START_REF] Fox | Distributed multirobot exploration and mapping[END_REF].

One of the problems to address with group of robots is cooperative localization [START_REF] Roumeliotis | Distributed multirobot localization[END_REF], i.e., improvement of each robot positioning ability thanks to information exchange (e.g sensor measurements) with other robots of the group. Several methods are available for Cooperative Localization (CL), like the Extended Kalman Filter (EKF) for a centralized system [START_REF] Huang | Observability-based consistent EKF estimators for multi-robot cooperative localization[END_REF], or, if computation is decentralized and communication is unreliable, other techniques like Covariance Intersection [START_REF] Julier | A non-divergent estimation algorithm in the presence of unknown correlations[END_REF] or Interleaved Update [START_REF] Bahr | Cooperative localization for autonomous underwater vehicles[END_REF]. Bounded error approaches using polytopes and linear programming have also been proposed [START_REF] Spletzer | A bounded uncertainty approach to multirobot localization[END_REF], [START_REF] Camillo | A bounded uncertainty approach to cooperative localization using relative bearing constraints[END_REF].

In a bounded error framework, interval methods [START_REF] Moore | Interval analysis[END_REF] enable rigorous propagation of errors in non-linear problems. Applications are found in localization of mobile robots [START_REF] Colle | Mobile robot localization by multiangulation using set inversion[END_REF] or cars [START_REF] Drevelle | Localization confidence domains via set inversion on short-term trajectory[END_REF], and in the context of cooperative localization for underwater robots with sonar ranging [START_REF] Bethencourt | Cooperative localization of underwater robots with unsynchronized clocks[END_REF], and vehicle networks with GNSS [START_REF] Lassoued | Cooperative localization with reliable confidence domains between vehicles sharing GNSS pseudoranges errors with no base station[END_REF]. The main characteristic of these approaches is to provide a set a feasible positions, instead of a point estimate.

In a previous work [START_REF] Kenmogne | Image-based UAV localization using interval methods[END_REF], we proposed an interval-based set-membership approach to single robot pose domain characterization. In this paper, we extend this method to multirobots cooperative localization. We consider a group of Ide-Flore Kenmogne, Vincent Drevelle and Eric Marchand are with Univ Rennes, Inria, CNRS, IRISA {ide-flore.kenmogne, vincent.drevelle, eric.marchand}@irisa.fr.

UAVs equipped with cameras able to track landmarks and a communication system with ranging capabilities. We aim at computing an enclosure for the pose of each robot based on set-inversion and constraints propagation techniques. The method computes a set that contains all the feasible poses of each robot, by taking image measurements, distance measurements and landmark positions error bounds into account. The true pose is guaranteed to be inside the computed solution set, as long as the actual measurement errors stay within their bounds. The main contribution of this paper resides in:

• using interval methods for cooperative fusion of camera and range measurements for the localization of two UAVs • combining the interval approach with non-linear weighted least squares for accurate initial localization • providing a comparison with other existing classical approaches The paper is organized as follows. Section II presents the localization problem with camera and ranges. Interval analysis and contractors are introduced in Section III. The proposed approach to compute pose domains is presented in Section IV. Finally, the approach is validated in Section V.

II. PROBLEM STATEMENT

This paper addresses the problem of localizing two UAVs in the world reference frame (Fig. 1), in a cooperative way, and with a focus on characterizing pose uncertainty domains. Given measurement uncertainties, we want to determine an enclosing domain that contains the pose of the robots.

Both robots are equipped with an altimeter, an inertial measurement unit (IMU) and a camera, so that each robot R k , k ∈ {1, 2} can measure:

• its altitude z k

• its accurate roll ψ k and pitch θ k angles • a rough estimate of the heading φ k angle • image points {x i,k , ..., x N,k } corresponding to observable landmarks { w X i , ..., w X N } in the environment In addition to these embedded sensors measurements, each robot is capable of:

• measuring its distance d k,j to the other robot R j • measuring its distance d k to the base station (the origin of the world frame in which the robots move) • communicating its measurements to the other robot. This can be done using an IEEE 802.15.4.a ultra-wide band (UWB) communication system [START_REF] Sahinoglu | Ranging in the IEEE 802.15.4a standard[END_REF] which enables ranging via time of flight measurements, or others external sensors. We aim at computing the domains of the feasible 6-DOF poses of each robot in the world reference frame F w , i.e., positions p k = (x k , y k , z k ) and attitudes (roll, pitch and yaw)

q k = (φ k , θ k , ψ k ).
As in [START_REF] Schiano | Bearing rigidity maintenance for formations of quadrotor uavs[END_REF], since we are operating in nearhovering regime, we assume that the roll and pitch (φ k , θ k ) are measured with an onboard IMU. Moreover, it is possible to retrieve the altitude of each robot through an altimeter. Thus, most of the effort in this article will be focused on estimating the horizontal position x k , y k and the heading ψ k of each robot.

A. Camera based pose estimation

Let F w the world frame attached to the origin of a local coordinate system, F r k the robot frame attached to the UAV R k center of gravity, and F c the camera frame attached to the camera optical center. We aim at determining the pose r k = (x k , y k , z k , φ k , θ k , ψ k ) of each robot in the world frame. Considering a single robot, the indices k are omitted in the sequel of this subsection for clarity. Pose estimation amounts to estimating the transformation matrix r T w between the world reference frame F w and each robot frame F r .

r T w = r R w r t w 0 3×1 1 (1)
where r R w and r t w are respectively the rotation matrix depending on the UAV's orientation and the translation vector that represents the position of the UAV in the world reference frame.

Knowing that c T w = c T r r T w , if we suppose that the rigid transformation c T r between the robot and the camera frame is known from calibration [START_REF] Tsai | A versatile camera calibration technique for high-accuracy 3D machine vision metrology using off-the-shelf tv cameras and lenses[END_REF] (for each UAV), the pose can be obtained indirectly by estimating the transformation between the camera and the world frames.

To determine the pose of a camera in the world (as depicted in Fig. 2), the perspective projection equation of a 3-D point Fig. 2: Perspective projection of an object in the image plane (in the world frame) on the image frame (a 2-D point) is used. This is the pinhole camera model, expressed by:

x = K Π c T w w X (2)
where:

• w X = (X, Y, Z, 1) are the homogeneous coordinates of a 3-D point in the world frame; • x = (u, v, 1) T are the homogeneous coordinates (in pixel) of the projection of w X in the image; • K is the camera intrinsic parameters matrix expressed using p x and p y , the meter/pixel scale factor and u 0 and v 0 the image principal point's coordinates [START_REF] Hartley | Multiple View Geometry in Computer Vision[END_REF]; • Π is the perspective projection matrix. The intrinsic parameters can be easily obtained through an off-line calibration step; we therefore consider them as known. If we have N points w X i , i = 1..N whose coordinates expressed in F w are given by w X i = (X i , Y i , Z i , 1) , the projection xi of these points in the image plane is then given by: xi = KΠ c T w w X i . Knowing 2D-3D point correspondences, xi and w X i , pose estimation consists in solving the system of equations for c T w . This is an inverse problem that is known as the Perspective from N Points problem or PnP. Many solutions exist to solve the PnP problem [START_REF] Lepetit | EPnP: An accurate O(n) solution to the PnP problem[END_REF], [START_REF] Marchand | Pose estimation for augmented reality: a hands-on survey[END_REF]. The "gold-standard" consists in estimating the six parameters of the transformation c T w by minimizing the norm of the reprojection error, by using a non-linear minimization approach such as the Levenberg Marquardt technique. Without prior knowledge of the pose, 3 unaligned points lead to four ambiguous solutions. A minimum number N ≥ 4 of points is thus needed to determine the pose.

B. Range measurements

Assuming the antenna coincides with the center of the UAV, range measurements are independent of the robot attitude. They only provide information about the robot position p k = (x k , y k , z k ). Indeed, range measurement of robot R k to the base station B is defined by The inter-distance measurement d 1,2 constrains the feasible configurations such that for a given position of R 1 , there are only two admissible positions of R 2 . These corresponds to the intersections of the R 2 position circle with the sphere S 2 (p 1 , d 1,2 ). In addition to this ambiguity, there is still one degree of freedom left for the two-robot position configuration, corresponding to rotations around the vertical axis of the base station.

d k = p k -b 2 with k ∈ {1,

C. Cooperative pose-domain characterization

As previously explained, in the proposed configuration (two robots and a fixed base station), ranging alone does not enable to do perform a position fix. It has to be combined with camera measurements. A robot is able to compute its pose by combining pitch and roll knowledge from the IMU, altitude measurement, distance measured to the base station, and vision of at least two landmarks. Seeing more landmarks enables to refine the pose thanks to data redundancy.

Cooperative positioning is enabled by inter-UAV ranging, and measurements exchange. It provides a means for position refining by using additional measurements. Cooperation also allows to relax the landmark visibility constraint, enabling one of the robots to see only one landmark and still estimate its pose (up to an ambiguity that can be resolved with a rough heading estimate).

In the sequel, we consider that measurements are uncertain, and are represented by intervals (bounded-error model). The solution to the localization problem is not a point pose estimate anymore, but a pose domain, i.e., a set. We now show how to compute a guaranteed enclosure of the two robots pose domains, by using interval analysis and constraint propagation methods.

III. INTERVAL ANALYSIS

This section presents a short overview of Interval Analysis as a non-probabilistic tool for bounded error estimation. It puts an emphasis on the use of Contractor Programming [START_REF] Chabert | Contractor programming[END_REF] and Set Inversion via Interval Analysis (SIVIA) [START_REF] Jaulin | Set inversion via interval analysis for nonlinear bounded-error estimation[END_REF] to perform set-membership estimation, i.e., computing the set of solutions that are compatible with the measurements and their error bounds.

A. Set Inversion

Let us denote IR the set of real intervals, and IR n the set of n-dimensional boxes (interval vectors). Intervals and boxes will be denoted between brackets. The interval [x] = [x, x] is defined as the set {x ∈ R, x ≤ x ≤ x}, where x and x are respectively the lower and upper bounds of [x]. The width of an interval is w([x]) = x -x, and its midpoint (or center) is mid(

[x]) = x+x 2 .
The width of a box is the largest of the widths of its components, and the center of a box is the vector of the midpoints of its components.

With f a function from R n to R m and Y a subset of R m , set-inversion consists in characterizing the preimage of Y, i.e., the set

X = {x ∈ R n |f (x) ∈ Y} = f -1 (Y).
Since computing with arbitrary sets is intractable in the general case, interval analysis treats problems by operating on interval and boxes. Inclusion functions are an important tool for interval computations. An inclusion function for f is a function from IR n to IR m such that

∀[x] ∈ IR n , f ([x]) ⊂ [f ]([x]).
If f admits a convergent inclusion function [f ], then two subpavings (sets of non-overlapping boxes) X -and X + that characterize the set X such that X -⊂ X ⊂ X + can be obtained with the SIVIA branch and bound algorithm.

B. Contractors

To characterize a set X, SIVIA needs to bisect in all dimensions of the search space, which make the complexity grow exponentially with the dimension of the problem. Problems in higher dimensions can however still be addressed, by using contractors to reduce the search space.

A contractor is a mapping from IR n to IR n such that If the box resulting from the contraction operation is equal to the smallest interval that contains the solution, the contractor is said to be minimal. The forward-backward contractor, also known as HC4 [START_REF] Benhamou | Revising hull and box consistency[END_REF], is the classical constraint propagation loop found in the literature. It allows to contract with respect to a system of constraints, by propagating constraints in an optimal order, as shown in the example above. In this article, we use the contractor to refine our domain by rejecting the part that is non consistent with the measurements.

• ∀[x] ∈ IR n , C([x]) ⊆ [x] (contraction) • (x ∈ [x], C(x) = x) ⇒ x ∈ C([x]) (consistency) • C(x) = ∅ ⇔ (∃ε > 0, ∀[x] ⊆ B(x, ε), C([x]) = ∅) (

C. Set Inversion with Contractor

An outer subpaving of the solution set X contained in the initial domain [x 0 ] can be computed with the SIVIA algorithm (Alg. 1). Taking the initial box [x 0 ], SIVIA successively refines the box by applying contractions and bisections. The list L, which is a stack (LIFO) for depth first space exploration, is used as a working list in which bisected boxes are temporary stored. For each box, the contractor C f is used to discard all parts of the box that are not consistent with the measurements (this is equivalent to applying the constraint f (x) ∈ [y]. The parameter ε is used to control the sharpness of X + . If the width of a contracted box is greater than , then the box is bisected and the two resulting boxes are enqueued in L.

Result: X + X + := ∅ outer subpaving of pose domain

L ← {[x 0 ]} while L = ∅ do [x] ← get the first box of L [x] ← C f ([x]) contract the box if width([x]) < then X + ← X + ∪ [x] else if [x] = ∅ then ([x 1 ], [x 2 ]) ← bisect([x]) put [x 1 ] in L put [x 2 ] in L end end Algorithm 1: SIVIA( [x 0 ], C f , )
IV. PROPOSED APPROACH To solve the problem described in Section II, we define our problem as a Constraint Satisfaction Problem (CSP), and then use SIVIA with contractors (Alg. 1) to characterize the feasible poses domain for each robot. So in this section, we present:

• the constraints related to the projection of points in the camera frame • the constraints related to the measured ranges (UAV to UAV and UAV to base ranges) • the proposed strategy used to seek the domain of all the feasible poses. The width of this domain will quantify the uncertainty on the pose with respect to that of the measurements.

A. Camera constraints

For each landmark seen by the camera of a robot, the projection equation (Eq. 2) and the fact that the camera cannot see from the back give the following constraints:

• Two constraints C proj w.r.t. the 2D-3D correspondences.

With each landmark point represented in homogeneous coordinates, the landmarks coordinates in the camera frame are given by

( c X, c Y c Z, 1) T = c T r r T w (r)(X, Y, Z, 1) T
and we have their pixel coordinates in the image plane

(u, v, 1) T = K Π( c X, c Y c Z, 1) T .
The image projection constraint C proj is given by:

C proj = {u = p x x i + u 0 , v = p y y i + v 0 } with x i = c X i c Z i , y i = c Y i c Z i .
• C f ront expresses the constraint w.r.t. the fact that we have a front-looking camera. For all points in camera frame, c Z i > 0, i.e., observed points are in the front half-space:

C f ront = { c Z i > 0}
Therefore C cam,i = {C proj,i , C f ront,i } is the set of all these constraints for all 2D-3D pairs (x i , w X i ), with i ∈ 1...N .

B. Range constraints

Considering the three known ranges, we have 3 constraints derived from this.

• C base expresses the constraint w.r.t. the distance to the base station for each UAV. Considering the base b as defined in Section II-B in the world frame, we define d k the distance constraint from b to UAV k (with k ∈ 1, 2) by

C r k ,base : {d k = p k -b 2 }
• C r1,r2 expresses the constraint w.r.t. the inter-distance between UAVs. Let d 12 be the measured distance between both UAVs. The constraint can be expressed as follow

C r1,r2 = {d 12 = r 1 -r 2 2 }

C. Approach

Taking into account the fact that all UAV exchange their image measurements, their distance to the base station, and the distance between each other, we can formulate the problem as a centralized CSP H defined by:

H :     r 1 ∈ [r 1 ], r 2 ∈ [r 2 ], xi,k ∈ [x i,k ] , w X i ∈ [ w X i ] , {C cam,i,k , i ∈ 1...N, k ∈ {1, 2}, C r1,base , C r2,base , C r1,r2 }     (3)
where the boxes [r 1 ] and [r 2 ] represent prior knowledge of the UAV poses, [x i,k ] represents bounded-error measurements of the landmarks in the image seen by UAV number k, and the width of [ w X i ] represents the uncertainty about world coordinates of the landmarks.

To characterize the pose solution-set of each UAV, we use the SIVIA algorithm presented in section III-C to compute the subpaving of all feasible poses in [r 1 ], [r 2 ] that are consistent with the measurements using H (CSP in Eq. 3).

As the components φ 1 , θ 1 , φ 2 , θ 2 , z 1 and z 2 of the poses r 1 and r 2 are directly measured, the initial domains of theses components of the state are known with a small uncertainty related to the sensors precision. Let φ , θ and z , these measurements uncertainties. The domains are therefore set to intervals [φ meas.

1 ± φ ], [θ meas. 1 ± θ ], [φ meas. 2 ± φ ], [θ meas. 2 ± θ ], [z meas. 1 ± z ] and [z meas. 2 ± z ].
For the unknown parameters (x 1 , y 2 , ψ 2 , x 2 , y 2 and ψ 2 ), their initial domains in [r 1 ], [r 2 ] are set w.r.t. the prior knowledge of the environment (in this article, the experimental room dimensions)

After setting the initial domain for each robot, a forwardbackward contractor (HC4) is built and applied in the SIVIA algorithm for all unknown parameters (x, y, ψ). This enables to recursively reduce the initial domain by rejecting the parts that are not consistent with the measurements when applying the constraints of H).

At convergence of SIVIA, bounded domains for the poses r 1 and r 2 are obtained in the form of subpavings X + r1 and Fig. 3: Position domains for a single robot, at 50 different positions, with range measurement to the base station (±5 cm) and 3 landmarks in view (±0.5 px). Top view.

X + r2 . Assuming the measurements error bounds have not been violated (i.e., the bounded error model is respected), the computed domains are guaranteed to contain the true robot poses r * 1 and r * 2 , i.e., r * 1 ∈ X + r1 and r * 2 ∈ X + r2 .

V. COOPERATIVE LOCALIZATION

In this section, we present results obtained from simulations, and an experimental trial using cameras onboard UAVs.

A. Simulation results

The simulation environment consists in a room, with the base station at coordinates b = (0, 0, 0), and three landmarks of known positions on the other side of the room (see Fig. 3 for a top view). A pattern of 50 test positions has been generated to validate our approach. This leads to a set of 2450 two-robot positions combinations (for every fixed position i ∈ 1..50 of robot R 1 , all positions j = i, j ∈ 1..50 of R 2 are considered).

Altitude, pitch angle and roll angle are assumed to be known. Assuming unreliable compass measurements, the robot heading is assumed to be known only very coarsely (within a ±20 degrees interval). Ranging errors (from base station and inter-UAV) are assumed to be within ±5 cm. Landmarks tracking in the image is assumed to be ±0.5 px accurate. I. We clearly observe in the figure that pose domains get larger as the robot gets further from the landmarks. It is also interesting to notice bigger uncertainties near the x = 0 axis. This is due to the poorly conditioned intersection of the sphere from the base station distance and the landmarks projection constraints.

2) Cooperative localization with full visibility: Position domains have been computed using the proposed cooperative localization method for the two-robot combinations of the simulated positions patterns, assuming both robots are identical and able to see the three landmarks. Overall improvement on the position uncertainty (w.r.t. the single robot case) is mainly visible on the x coordinate (better min and mean values in Table I). This improvement is not very large because the position uncertainty domains of both robots without measurements exchange are often of comparable sizes, and the geometric configuration does not always allow domain contraction. Moreover, some of the computed domains are larger that in the single robot case, due to the parameter of the set inversion process which prevents perfect approximation of the domain (this is visible in the "max" column of Table I, and also affects the means).

Figure 4 shows a configuration where cooperative localization sensibly reduces position uncertainty, thanks to a good geometric configuration (compare the x = 0 column with Fig. 3).

On a 2014 2.8GHz Core i7, with a single threaded implementation, the average computation time is of 200 ms for = 30 mm (presented results), and of 32 ms for = 60 mm. Increasing reduces the computation time, but provides a rougher estimate of the feasible poses domain. When using a fixed value, the computation time varies with the size of the computed domain and is linear with the number of landmarks seen by a robot. Real-time implementation is possible using, either a fixed number of SIVIA iterations, or an any-time 3) Cooperative localization with reduced visibility: To emphasize the benefits of the proposed cooperative localization, we now suppose that the robot R 1 sees all 3 landmarks, but the robot R 2 only sees one landmark. This situation could arise when an operator wants to rotate an UAV in order to observe another part of the environment. Localization with a single landmark is impossible for a single UAV (horizontal position is only constrained by the distance to the base station, which, given uncertainties and badly known heading, leads to an arc-shaped solution set). Knowing the distance to the other (well localized) UAV enables to compute an usable position domain. Simulations have been done for the 2450 possible configurations. Obtained uncertainty domains are, of course, larger than in the previous case, due to the lack of camera measurements (see Table II). The very large maximum values obtained in Table II correspond to a few cases where the heading measurement uncertainty prevents from resolving the position ambiguity of R 2 , leaving a set of two disjoint pose domains. Figure 5 shows the pose domain subpavings obtained for the robot R 2 (in red), for 4 different positions of the robot R 1 (whose position domain is painted in blue and circled). It clearly shows the influence of the uncertainty

B. Point estimate and comparison with weighted least squares

By computing the set of all feasible poses, our approach is interesting for solving the initial localization problem. When point estimate is needed, a straightforward solution consists in taking the center of the bounding-box (CoB) of the solution subpaving.

The horizontal position and heading errors of the CoB estimate are reported in the first row of Table III (full visibilty case) and of Table IV (robot R 2 in the reduced visibility case). Cumulative distributions of the errors for the reduced visibility case are shown in Fig. 6 (dotted blue lines).

These estimates are compared to non-linear weighted least squares (WLS) estimates computed with the Levenberg-Marquardt (LM) algorithm, using all observations (images, distances, altitude and IMU angles). The measurements weights are initially set w.r.t. the standard deviations corresponding to the widths of the interval error bounds and then fine-tuned. The ground truth is first used as an initial guess (LM GT guess in the tables, dashed red lines in Fig. 6). It shows that the center of the bounding box is a good point estimate, with errors figures very close to the WLS estimate when the domains are not too large. In the full visibility case, the CoB average errors are 1.51 cm horizontal and 0.16 degrees in heading, vs 1.51 cm and 0.14 degrees with the WLS. However, when the computed domains gets bigger, the CoB becomes less precise. This can be seen with robot R 2 in the reduced visibility case (Table IV).

An idea is to combine the domain computed with the interval method (as an uncertainty indicator) with LM to provide a precise point estimate. When initialized far from the solution (here, the center of the simulation area), LM We thus use the CoB as initial guess for LM, in order to get a better point estimate (Interval+LM). In the full visibility case, starting at the CoB make the LM result similar to the one initialized by the ground truth (second row of Table IV). This shows the CoB is a good initial point for efficient convergence to the solution and helps to avoid local minimums.

In the reduced visibility test case, the Interval+LM method significantly improves point estimates, especially in the case of robot R 2 (see how plain black curves in Fig. 6 are almost superimposed with ground-truth-initialized-LM curves).

C. Experimental validation

The experimental environment is made of a Vicon motion capture system that tracks the pose of quadcopter UAVs (MikroKopter MK-Quadro equipped with a camera) to provide ground truth and the landmarks positions. The landmarks are cubes for which the positions are known in the reference frame and have a very visible face for the purpose of image tracking (Fig. 7).

In addition to the uncertainties described in the previous simulation section, this experiment also considers small uncertainties on the roll and pitch angles (±0.6 deg), on the altitude measurements (±1 cm) and on the landmark positions (±1 mm).

The experiment mainly consists in moving the UAVs along the x axis. The bounds of the computed pose domains for the two robots are reported in Fig. 8. Similarly to the simulation, uncertainty is larger on the x component of the position (36 cm mean width) than on y (17 cm mean width), which is due to the geometry of the problem. This can be observed on Fig. 9, where the position domains computed for the two The first row of Table V reports the position and heading errors obtained during the experiment, when using the center of the computed domain as a point estimate. The mean horizontal error is of 3.03 cm, and the mean heading error is less than 0.6 degree. An Extended Kalman Filter (EKF) has been applied on the same data, with the observations gains set identically to the WLS of the previous subsection. The results of the EKF (second row of Table V) shows the proposed interval method outperforms the EKF in terms of position precision. The EKF initial state has been set to the true pose, to avoid initial localization problem and keep the comparison fair. The poor results of the EKF on position estimate are mostly due to the assumption of centered, white and independent noise on measurements, which is not the case in practice due to correlation in landmark tracking errors (e.g., due to motion blur) and biases. The EKF, however, yields a better heading estimate than the interval method. An explanation comes from the setting of the interval method, which limits the precision of the pose domain approximation.

Camera tracking of the landmarks has failed at several epochs of the trial, leading to measurements error bounds violation. At times t ≈ 16 s and around t = 20 s, the result of the interval method is the empty set (blanks in Fig. 8). It is the consequence of measurements inconsistency, i.e., there is no pose that satisfies all the constraints of Eq. This inconsistency enables to prove that the bounded error assumption is violated for these epochs. Around time t ≈ 2 s, we observe that the computed pose domains are very small and do not contain the true robot poses. This still correspond to error bounds violation, but not enough to yield empty set.

It can be noted that, during these tracking failure epochs, the EKF precision is also negatively impacted, with average errors of 6.4 cm in position and 1.0 deg in heading (vs 4.1 cm and 0.2 deg average over the other epochs).

VI. CONCLUSION

This paper has presented a set-membership cooperative localization approach for two UAVs. Assuming that measurement errors are bounded and can be represented by intervals, a set-inversion method based on constraint propagation techniques is employed to compute a guaranteed outer approximation of the set of feasible poses for each robot. Both robots measure distances to a base station, the distance between them, and can see known landmarks with their cameras. They share their measurements to enable cooperative localization. We show that cooperation enables to tighten the pose uncertainty domain, especially when the geometrical configuration is favorable to inter-distance measurement. The center of the computed pose domain is a good point estimate that can be refined using the Levenberg-Marquardt algorithm. Another strong aspect of cooperative localization is that it gives positioning ability to a robot that otherwise would have not been able to compute a position fix due to the lack of measurements. An example is given, where one of the robot can only see a single landmark, while the other can see all of them. These aspects have been demonstrated by simulating multiple two-robot configurations. Validation with experimental data shows the usability of the method with real image measurements, and better position accuracy than an Extended Kalman Filter.

Fig. 1 :

 1 Fig. 1: Cooperative localization of two UAVs R 1 and R 2 , seeing landmarks and measuring ranges to the base station B.

  2}, where b = (x B , y B , z B ) is the base station position in the world reference frame. Inter-robot range measurements are defined by d k,j = p k -p j 2 with (k, j) ∈ {1, 2} 2 and k = j. Since the base station position b is known, the knowledge of d k constrains the position of R k on the sphere S 2 (b, d k ). The additional knowledge of d 1,2 restricts the feasible configurations: for a given candidate position of R 1 , the admissible positions of R 2 are restricted to a circle (intersection of the two spheres S 2 (b, d 2 ) and S 2 (p 1 , d 1,2 )). Range measurements d 1 , d 2 and d 1,2 provide constraints on the positions of the two robots, but there are 3 degrees of freedom left (out of 6) on the position components of the two-robot group (corresponding to all rotations of the formation around the base station). Now let us consider that the altitude z k of each robot is known (provided by the altimeter). This provides two additional constraints. Distances d k to the ground station now restrict the position of each robot on an horizontal circle (intersection of S 2 (b, d k ) and the z k = z meas k plane).

  continuity), where B(x, ε) is the ball centered on x with radius ε. A contraction operation returns a sub-domain of the input domain [x], without losing any solution. For example if we have the equation a = 2c + b. Let a ∈ [-2, 3], b ∈ [1, 1] and c ∈ [0, 2]. The most straightforward way to contract with respect to numerical constraints is by using the forwardbackward algorithm. The forward operation will give a = [-2, 3] ∩ 2 * [0, 2] + [1, 1] = [1, 3]. The backward operation, based on applying the derived constraints c = (a -b)/2 and b = a -2c, gives c = [0, 1] and b = [1, 1].

Fig. 4 :

 4 Fig. 4: Position domains for cooperative localization. R 1 is fixed (in blue, circled), R 2 at 49 different positions (in red).

Fig. 5 :

 5 Fig. 5: Cooperative localization results when R 1 sees 3 landmarks and R 2 sees only one. Each figure corresponds to a different fixed position of R 1 (circled blue domain). The (x 2 , y 2 ) domains at 49 positions of R 2 are shown in red.

of R 1

 1 , but also of the geometric configuration: large domains obtained in the center of the two right figures are due to the fact that distance to the base and distance to the robot R 1 constrain the position of R 2 at the intersection of almost tangent circles. Bottom right figure of Fig. 5 even shows a few ambiguous cases, where the solution set is made of two disjoint subsets.

2 Fig. 6 :

 26 Fig. 6: Cumulative distribution of horizontal position error (left) and absolute heading error (right), for robot R 1 seeing 3 landmarks and robot R 2 seeing only one.

Fig. 7 :

 7 Fig. 7: Onboard camera view of the two UAVs

Fig. 8 :

 8 Fig. 8: Errors bounds and punctual estimate in the experimental cooperative case. Zero corresponds to ground truth.

Fig. 9 :

 9 Fig. 9: Position domains of the two UAVs at the beginning of the experiment: R 1 in blue (right), R 2 in red (left).

TABLE I :

 I Pose domains width in the single robot case and in the full visibility two-robot cooperative case.

TABLE II :

 II Pose domain width of R 2 in the reduced visibility cooperative case. R 1 sees 3 landmarks and R 2 only sees one.

	Method	Horiz. pos. error (cm) mean 95 % max	Heading error (deg) mean 95 % max
	Interval (CoB)	1.51	3.52	17.5	0.16	0.41	1.99
	Interval + LM	1.43	3.36	10.1	0.14	0.36	1.06
	LM (GT guess)	1.43	3.36	10.1	0.14	0.36	1.06
	LM (bad guess) 351.7	645.2	721.9	39.1	105.7 179.3

TABLE III :

 III Position (2-D horizontal) and heading error statistics in the 2-UAV cooperative case with full visibility

	Method	Horiz. pos. error (cm) mean 95 % max	Heading error (deg) mean 95 % max
	Interval (CoB)	5.39	13.61	167.7	0.80	2.59	19.4
	Interval + LM	4.11	7.00	328.4	0.56	1.19	39.4
	LM (GT guess)	2.90	6.68	34.1	0.36	1.08	5.79
	LM (bad guess)	391.9 760.8	1074	140.5	179.0 180.0

TABLE IV :

 IV Position and heading error of R 2 in the cooperative case with reduced visibility. R 1 sees all landmarks and R 2 only sees one.

  3.

	Method	Horiz. pos. error (cm) mean 95 % max	Heading error (deg) mean 95 % max
	Interval	3.03	6.01	7.22	0.58	1.63	3.02
	EKF	4.2	9.47	20.6	0.26	0.66	5.25

TABLE V :

 V Position and heading error in the cooperative experiment. Statistics of EKF and interval errors

ACKNOWLEDGMENT

This research was supported by the ANR, Project ANR-14-CE27-0007 SenseFly.


Souriya Trinh

Fabien Spindler

Eric Marchand

Franc ¸ois Chaumette

A modular framework for model-based visual tracking using edge, texture and depth features

We present in this paper a modular real-time model-based visual tracker. It is able to fuse different types of measurement, that is, edge points, textured points, and depth map, provided by one or multiple vision sensors. A confidence index is also proposed for determining if the outputs of the tracker are reliable or not. As expected, experimental results show that the more various measurements are combined, the more accurate and robust is the tracker. The corresponding C++ source code is available for the community in the ViSP library.

I. INTRODUCTION

The ability to accurately localize a camera with respect to an object of interest is a crucial step toward bringing dynamic manipulation in robotic vision. With a real-time process, complex tasks such as object grasping or robot positioning can then be performed in closed-loop by visual servoing to take into account perturbations and dynamic environment [START_REF] Chaumette | Visual servo control, Part I: Basic approaches[END_REF]. Augmented reality applications [START_REF] Marchand | Pose estimation for augmented reality: a hands-on survey[END_REF] and indoor navigation of mobile robots [START_REF] Paolillo | Vision-based maze navigation for humanoid robots[END_REF] can also be considered. Main challenges of object tracking concern the ability to not only accurately track the object but also to be able to bring reliability and robustness.

We describe in this paper the main building blocks of the general model-based tracking framework implemented in the latest version of ViSP open-source C++ library 1 . ViSP [START_REF] Marchand | ViSP for visual servoing: a generic software platform with a wide class of robot control skills[END_REF] is a modular cross-platform library that allows prototyping and developing applications related to visual tracking and visual servoing (VS). Although ViSP has been initially developed mainly for visual servoing purposes with basic tracking capabilities [START_REF] Marchand | ViSP for visual servoing: a generic software platform with a wide class of robot control skills[END_REF], such capabilities have been extended over the time. Along with direct image registration or template-based trackers [START_REF] Baker | Lucas-kanade 20 years on: A unifying framework[END_REF], [START_REF] Benhimane | Real-time image-based tracking of planes using efficient second-order minimization[END_REF], a first model-based tracker was introduced in the library using edges only [START_REF] Comport | Realtime markerless tracking for augmented reality: the virtual visual servoing framework[END_REF].

Model-based tracking [START_REF] Lowe | Fitting parameterized three-dimensional models to images[END_REF], [START_REF] Drummond | Real-time visual tracking of complex structures[END_REF], [START_REF] Comport | Realtime markerless tracking for augmented reality: the virtual visual servoing framework[END_REF], [START_REF] Choi | Robust 3d visual tracking using particle filtering on the special euclidean group: A combined approach of keypoint and edge features[END_REF] aims at computing the object pose with respect to the camera from the knowledge of the geometric object model. The basic principle consists in estimating the pose by minimizing the norm of the reprojection error using iterative non-linear minimization techniques, such as a Gauss-Newton or Levenberg-Marquardt. Minimizing this reprojection error provides the Maximum Likelihood estimate when a Gaussian noise is assumed on the measurements.

The earliest methods considered models composed of object edges and distance to contour as basic measurement.

Nevertheless, these methods may fail when the object or background is textured, and the robustness deteriorates when ambiguities between some edges occur. One way to address this issue is to fuse the information coming from edges with information given by keypoints [START_REF] Choi | Robust 3d visual tracking using particle filtering on the special euclidean group: A combined approach of keypoint and edge features[END_REF], [START_REF] Pressigout | Real-time hybrid tracking using edge and texture information[END_REF], [START_REF] Petit | Combining complementary edge, point and color cues in model-based tracking for highly dynamic scenes[END_REF]. Another solution, considered in this paper, is to exploit RGB-D images that provide depth along with photometric data [START_REF] Ireta | Point-to-hyperplane RGB-D Pose Estimation: Fusing Photometric and Geometric Measurements[END_REF].

In this paper, we emphasize on the genericity of the modelbased tracking method that allows combining easily different types of visual features (object contours, textured points of interest, depth features) observed from one or several vision sensors (e.g. RGB-D cameras). In particular, we present how depth information can be integrated for improving tracking robustness and accuracy. A confidence index is also proposed for determining if the outputs of the tracker are reliable or not. Numerous experimental results are finally provided in Section V.

II. RELATED WORK

As stated in the previous section, the new model-based tracker described in this paper and implemented into the latest available version of ViSP has a direct filiation with [START_REF] Comport | Realtime markerless tracking for augmented reality: the virtual visual servoing framework[END_REF]. Using Moving-Edges (ME) algorithm [START_REF] Bouthemy | A maximum likelihood framework for determining moving edges[END_REF] allows tracking efficiently the object contours and provides real-time performance. Integration of robust M-estimators in the estimation process allows handling partial occlusions and outliers. A closely related work [START_REF] Drummond | Real-time visual tracking of complex structures[END_REF] is based on a 1D-search along the object contours and a Lie algebra formulation to update the estimated pose. An extension to multiple cameras was proposed in [START_REF] Dionnet | Stereo tracking and servoing for space applications[END_REF]. Our model-based tracker has then been extended in [START_REF] Pressigout | Real-time hybrid tracking using edge and texture information[END_REF] to include texture information for improving tracking robustness. A similar approach using a GPU to handle object visibility is described in [START_REF] Choi | Real-time 3d model-based tracking using edge and keypoint features for robotic manipulation[END_REF] with a complete framework to initialize the tracker. In [START_REF] Choi | Robust 3d visual tracking using particle filtering on the special euclidean group: A combined approach of keypoint and edge features[END_REF] a particle filter on SE(3) is used to maintain multiple hypotheses for the estimated pose, which allows avoiding the estimation to converge to local minimum. In [START_REF] Azad | 6-dof model-based tracking of arbitrarily shaped 3d objects[END_REF], the GPU capabilities are exploited to render the object and a particle filter is also used to update the pose. [START_REF] Seo | Optimal local searching for fast and robust textureless 3d object tracking in highly cluttered backgrounds[END_REF] focuses on taking into account region appearance to deal with highly cluttered backgrounds.

With the availability of low-cost RGB-D sensors, depth map is a valuable visual cue for improving the tracking accuracy and robustness [START_REF] Ireta | Point-to-hyperplane RGB-D Pose Estimation: Fusing Photometric and Geometric Measurements[END_REF]. Using a Gaussian filter approach, [START_REF] Issac | Depth-based object tracking using a robust gaussian filter[END_REF] considers only the depth map for estimating the pose that best registers the observed and rendered depth maps. Within a particle filter framework, [START_REF] Krull | 6-dof model based tracking via object coordinate regression[END_REF] uses RGB-D data and a proposal distribution to improve the energy-based observation model and reduce the number of particles. A probabilistic model is used in [START_REF] Ren | 3d tracking of multiple objects with identical appearance using rgb-d input[END_REF] to track multiple similar objects with RGB-D images and histograms as appearance models for the registration. Another region-based approach is proposed in [START_REF] Tjaden | Real-time monocular pose estimation of 3d objects using temporally consistent local color histograms[END_REF] with temporally consistent, local color histograms for real-time pose detection and tracking of rigid objects. Multiple cameras are used in [START_REF] Pauwels | Simtrack: A simulation-based framework for scalable real-time object pose detection and tracking[END_REF] to increase the tracking accuracy. Different visual cues such as depth, normals or object appearances are fused with joint state of the robot to track multiple objects in presence of occlusions. The idea of fusing multiple visual cues is also considered in [START_REF] Petit | Combining complementary edge, point and color cues in model-based tracking for highly dynamic scenes[END_REF] with edge, point, and color features [START_REF] Panin | Robust contour-based object tracking integrating color and edge likelihoods[END_REF] for robust tracking in presence of motion blur and challenging illumination conditions. GPU can be used to render directly the object silhouette for edge tracking and allows avoiding the need to pre-process the object CAD model [START_REF] Petit | Combining complementary edge, point and color cues in model-based tracking for highly dynamic scenes[END_REF], [START_REF] Choi | Robust 3d visual tracking using particle filtering on the special euclidean group: A combined approach of keypoint and edge features[END_REF], [START_REF] Wuest | Tracking of industrial objects by using cad models[END_REF].

III. MODEL-BASED TRACKING A. Problem formulation

Let us denote F c the camera frame and c T o the transformation that defines the pose of the object frame

F o with respect to F c . c T o , is a homogeneous matrix defined such that c T o = c R o c t o 0 1×3 1 (1)
where c R o and c t o are the rotation matrix and the translation vector defining the change of frames.

Pose estimation by model-based tracking is an optimization problem that can be solved using an iterative nonlinear minimization technique, such as Gauss-Newton or Levenberg-Marquardt. Denoting q ∈ SE(3) a minimal representation of c T o (q = ( c t o , θu) where θ and u are the angle and the axis of the rotation c R o ), the objective consists in estimating the six independent parameters involved in q by registering the geometric object model M expressed in the sensor space with respect to visual features x * extracted from the sensor. The problem can be formulated as

q = argmin q i ε 2 i (x * i , x i (M i , q, ξ)) (2)
where M i are parts of the model and x i (M i , q, ξ) expresses the relation between the model and the image space for pose q and calibration parameters ξ. ε(x, x ) is the registration residual, that is typically the Euclidean distance between two visual features x and x that have to be registered. If the set of visual features are well chosen, there exists only one single pose q such that ε 2 i = 0, ∀i. We will see in Section III-C that M i , x i , x * i , and thus the residual ε i can take many forms (distance to contour, distance between points, distance to 3D plane, etc.).

B. Robust minimization

As explained before, the optimization consists in minimizing the norm of the error vector e obtained by stacking the set of residuals

ε i : e = (ε 1 , • • • , ε n ).
The time derivative of this vector can be expressed as

ė = ∂e ∂q q = L e v (3)
where v ∈ se(3) is the velocity screw and L e is the interaction matrix related to x [START_REF] Chaumette | Visual servo control, Part I: Basic approaches[END_REF].

To achieve a decoupled exponential decrease of the residuals, that is, ė = -λe where λ is a positive scalar, one minimization step would be given by v = -λL e + e. However, to deal with outliers, a weight w i is associated to each residual ε i for representing the confidence in the corresponding visual feature. This leads to introduce a diagonal matrix W = diag (w 1 , • • • , w n ) from which we obtain [START_REF] Comport | Realtime markerless tracking for augmented reality: the virtual visual servoing framework[END_REF] v = -λ (WL e )

+ W e.

Note that v can be seen as the velocity of a virtual camera (from which x(M, q, ξ) is computed) at each iteration of the optimization process. Then, the camera pose is updated with

c (k+1) T o = ∆T -1 c k T o ( 5
)
where k denotes the iteration number of the minimization process. The exponential map allows transforming the velocity v to the incremental pose

∆T = c k T c (k+1) = exp (v).
Each element of the diagonal weighting matrix W represents the confidence of the corresponding visual feature

w i = ψ (δ i /σ) δ i /σ (6)
where ψ (u) is the influence function and δ i is the normalized residual given by δ i = ε i -M ed (e) where M ed (e) is the median of e.

Many influence functions exist in the literature. Tukey has been chosen as it rejects completely the detected outliers by assigning them a zero weight. This way, a detected outlier will not influence the tracking. Details about the Tukey influence function are given in [START_REF] Huber | Robust Statistics[END_REF]. It is given by:

ψ (u) = u C 2 -u 2 2 , if |u| ≤ C 0, else (7)
where C = 4.6851 represents the proportionality factor for Tukey's function for 95 percent efficiency in the case of a Gaussian noise. It assumes a variance of 1 for the residuals distribution. So, an online scale estimation of the standard deviation σ is performed using the median absolute deviation of the residuals [START_REF] Comport | Realtime markerless tracking for augmented reality: the virtual visual servoing framework[END_REF].

C. Visual features

As stated now, different types of visual features can be supported. In this paper, we consider the distance between a contour point in the image and the projected object contour (edge-based features), the distance between two keypoints in two successive images (keypoint-based feature), and finally the distance between a 3D point measured by a RGB-D camera and a 3D plane (depth-based feature). a) Edge-based feature: Edge-based tracking relies on the ME algorithm [START_REF] Bouthemy | A maximum likelihood framework for determining moving edges[END_REF] that tracks contour points along the normal of the projected object contours. We consider here as residual ε i = d ⊥ (p i , l i (q)) that corresponds to the distance in the image between a ME p and the projected straight line l (q) (see Figure 1) represented in polar coordinates with x cos θ + y sin θ = ρ, ∀ (x, y) ∈ l (q). For each ME, the residual is then defined by

ε d ⊥ = ρ -(x cos θ + y sin θ) (8)
where (x, y) are the ME coordinates. The corresponding interaction matrix is given in [START_REF] Comport | Realtime markerless tracking for augmented reality: the virtual visual servoing framework[END_REF]]

L d ⊥ =         λ d ⊥ cos θ λ d ⊥ sin θ -λ d ⊥ ρ 1 + ρ 2 sin θ -αρ cos θ -1 + ρ 2 cos θ -αρ sin θ -α         (9)
where

λ d ⊥ = λ ρ + αλ θ , λ ρ = (Aρ cos θ + Bρ sin θ + C) /D, λ θ = (A sin θ -B cos θ) /D, α = x sin θ -y cos θ
and (A, B, C, D) defines the 3D plane coordinates (expressed in the camera frame) to which the line belongs. These 3D parameters are obtained from q and the object model. Any polyhedral object can be considered, as well as cylinders. 3D circles can also be considered. In this later case, a point-to-ellipse distance is used (see [START_REF] Comport | Realtime markerless tracking for augmented reality: the virtual visual servoing framework[END_REF] for more details).

b) Keypoint-based feature: When dealing with keypoint-based features, we consider the relation between point coordinates in two successive images. Such keypoints can be detected thanks to the Harris detector and then tracked using the classical KLT algorithm [START_REF] Baker | Lucas-kanade 20 years on: A unifying framework[END_REF] (alternatively any keypoint detection and matching process could be considered). Let us denote p = (x, y, 1) the homogeneous coordinates of a keypoint in the first image where it has been detected (from a camera located in c(0) T o ) and p * = (x * , y * , 1) its coordinates in the current image. Let us also denote c T c(0) the pose between these two views. Since the keypoints belong to a plane (they are detected on the planar faces of the object), we have p * = c H c(0) p where c H c(0) is a homography that depends of c T c(0) . Assuming that c(0) T o is known from the pose estimation process at view c(0), c H c(0) can of course be expressed as a function of the pose c T o to be estimated. More precisely, for each keypoint, we consider the point-to-point distance in the image between the point p transfered in the current image (according to the estimated pose q) and it correspondence p *

ε x ε y = x(q) -x * y(q) -y * (10)
where (x(q), y(q), 1)

= c H c(0) p with c H c(0) = c R c(0) + c t c(0) d n (11)
where n and d are the normal and the distance to the camera center of the planar face expressed in the camera frame c(0). The corresponding interaction matrix is given by, [START_REF] Chaumette | Visual servo control, Part I: Basic approaches[END_REF], [2]

L p = -1 Z 0 x Z xy -1 + x 2 y 0 -1 Z y Z 1 + y 2 -xy -x (12)
The actual 1/Z value appearing in ( 12) is computed by

1 Z = c R c(0) n p * c t c(0) c R c(0) n -d (13)
In the current implementation, keypoints can be considered for any planar faces of the object, such as circles for instance, and also on the surface of cylinders. c) Depth-based feature: The introduction of the Kinect and derived RGB-D sensors allows getting easily the depth of a point in the image. To exploit these data, the residual ε Z is defined as the point-to-plane distance, in 3D, such that

ε Z = Z(n x x + n y y + n z ) + D ( 14
)
where Z is the depth of the point with image coordinates (x, y) measured in the depth map and n x X + n y Y + n z Z + D = 0 is the planar face equation expressed in the camera frame. The corresponding interaction matrix is given by

L z = n x n y n z (n z yZ -n y Z) (n x Z -n z xZ) (n y xZ -n x yZ) (15)
All terms involved in ( 14) and ( 15) are obtained either directly as measurements (x, y, Z), or from the planar face of the object and the current camera pose (n x , n y , n z ).

D. Generic model-based tracking

Model-based tracking using a single type of visual features has already been proposed in the literature. So does the hybrid tracker combining edges and keypoints in [START_REF] Petit | Combining complementary edge, point and color cues in model-based tracking for highly dynamic scenes[END_REF] although with a different formulation. We propose to go further by proposing a general model-based tracker to let the user combine any types of visual features and multiple cameras. A straightforward example is to consider RGB-D sensor where edges and keypoints can be exploited from the color camera and depths from the depth sensor. Multiple (stereo) cameras are used in [START_REF] Dionnet | Stereo tracking and servoing for space applications[END_REF]. In such cases, it is necessary to know the homogeneous transformation between the different sensor frames which can be known through an a priori extrinsic calibration process.

Concretely, features combination consists in stacking the residuals in a vector set e and similarly for the corresponding interaction matrix. The iterative minimization process then regulates to zero the different feature errors. To deal with visual features extracted from different cameras, a reference sensor frame must be considered. The time variation of the visual features e 2 observed in camera frame F 2 expressed in camera frame F 1 is then

ė2 = L e2 2 V 1 1 v (16)
with2 V 1 the velocity twist matrix

2 V 1 = 2 R 1 2 t 1 × 2 R 1 0 3×3 2 R 1 (17)
which allows computing a velocity 1 v expressed in frame F 1 in frame F 2 . The velocity to be applied in the complete minimization step is given by [16]

1 v = -λ      L e1 L e2 2 V 1 L e3 3 V 1 . . .      +      e 1 e 2 e 3 . . .      (18)
Stacking visual features of different natures and from different viewpoints allows improving the tracking accuracy and robustness by exploiting the redundancy of the available measurements. Robust weights are computed for each type of visual features to maintain the coherence of the feature errors. An advantage of this approach is that it is not required to weight the visual features influence with respect to each other.

E. Covariance matrix

It is possible to estimate the covariance matrix of the estimated pose. Considering the weighting matrix W, it is given by

Γ (q) = σ 2 J e W 2 J e -1 (19)
where J e = L e L -1 q is defined from ė = J e q = L e L -1 q q (with L q given in [START_REF] Chaumette | Visual servo control, Part I: Basic approaches[END_REF]) and where

σ 2 = 1 n (e f -L e v f ) W 2 ( e f -L e v f ) (20)
where e f is the final value of the residual vector after convergence of the estimation process, v f the velocity computed at the last iteration, and n the number of features.

IV. IMPLEMENTATION DETAILS Along with the overview of the general tracker presented in the previous section, practical aspects have to be considered to provide an efficient tracker 2 .

A. Object modeling

From any model of the object built in a CAD software, information about the object geometry (set of lines, circles, cylinders and planar faces) are easily extracted to be used by the model-based tracker.

B. Visibility handling

Different strategies are implemented to handle object parts visibility. The former is based on the angle between a ray from the optical axis and the face normal. This allows very fast processing but is not enough for objects of complex shapes. In this case, a scan-line rendering algorithm is used and allows per-pixel visibility instead of per-primitive only, at the expense of higher computation time. Clipping is also used to restrain the object model to the camera view frustum.

C. Tracking initialization

Automatic initialization3 of the tracker is provided in ViSP by matching keypoints detected in the very first image and those extracted from training images, with an approach similar to [START_REF] Choi | Real-time 3d model-based tracking using edge and keypoint features for robotic manipulation[END_REF]. The correspondences between points in the image and points in the training images (whose 3D coordinates are known) allows estimating the pose using a Perspective-n-Points (PnP) algorithm. RANSAC is considered for rejecting outliers coming from spurious data. This approach is valid mainly for textured objects, and more elaborate techniques exist to handle more complex cases, such as using RGB-D data [START_REF] Brachmann | Learning 6d object pose estimation using 3d object coordinates[END_REF], [START_REF] Hinterstoisser | Model based training, detection and pose estimation of texture-less 3d objects in heavily cluttered scenes[END_REF] or with deep-learning-based approaches [START_REF] Tekin | Real-time seamless single shot 6d object pose prediction[END_REF].

D. Confidence index

The covariance matrix of the estimated pose given in Section III-E is not reliable to detect drift in the tracking (since drifts generally correspond to the convergence of the estimation in a local minimum, where the residuals are small). That is why a confidence index has been implemented to detect when the tracking starts to drift. This is achieved by computing the mean angular error between the normals to the object contours model and the observed gradient orientations in the image. Measure points are sampled at regular locations from the projected model (see Fig. 1) and a derivative kernel is used to compute the gradient orientation. The average angular error (between 0 and 90°) remains low when the estimated pose is correct. When the tracking drifts, the model starts to be projected onto the background, and the confidence index returns a high value. Thresholding the confidence index allows detecting drift in the tracking. Coupled with the initialization process using the pose estimation method described above, this scheme allows automatic reinitialization of the tracking after a drift.

V. EXPERIMENTAL RESULTS

We report in this section the tracking results on three image sequences and compare different combination of visual features. Two sensors are used. The first one is an Intel® RealSense™ SR300 sensor designed for close range (0.2 m to 1.5 m) applications. The second one is an ASUS Xtion PRO LIVE RGB-D sensor for a depth range starting around 0.8 m to approximatively 3.5 m. For each method, the tracking is stopped when the confidence index is higher than 20°. In a real application, the tracker would be reinitialized after a detected tracking drift according to IV-C.

A. Performance measures

Computation time depends on the object complexity and the number of extracted visual features. Table I summarizes the mean tracking time for edge, keypoint, and edge + keypoint + depth trackers on a regular processor (Intel® Core™ i7-4600U) for the three image sequences analyzed in the next section. Edge and keypoint trackers run at video rate. Depth-based tracker involves much more features, which impacts directly the computation time. The depth map is subsampled by a factor 4 to enable real-time tracking (more than 10 Hz). Due to the complex structure of the castle, scanline rendering used to handle visibility tests for the castle sequence increases the computation time.

B. Model-based tracking of a miniature castle

This experiment involves the tracking of a miniature castle using the SR300 sensor. The camera is static and the object is moved by hand. 688 images are acquired at 30 Hz. Fig. 2 shows the tracking results using a single type of visual features. The edge-based tracker is able to track on 265 images before starting to drift when too many ME are badly tracked on the left fortification to be handled by the M-estimators. Keypoint-based tracker drifts similarly and is stopped at frame 243 thanks to the confidence index. Depthbased tracker alone stops at frame 87. Indeed, when the front elements are not visible, this configuration does not allow observing the 6 parameters of the pose (interaction matrix is rank deficient). The tracker starts sliding when the miniature castle is moved perpendicularly to the hidden tower face.

Different visual features combinations (edge + keypoint, edge + depth, edge + keypoint + depth) for tracking are displayed on Fig. 3. Tracking with edge and keypoint features stops at frame 266. Indeed, the model is badly registered and the drift is correctly detected. Adding depth feature to edges allows tracking until frame 447. But then the tracking drifts when the ME are badly tracked. Indeed, depth features cannot prevent the drift since the left tower face is not visible. Only the full combination of features is able to track correctly the whole sequence. We can also note that robust M-estimators allows tracking the castle correctly even with large occlusions made by the hand. In Fig. 4, the estimated pose (translation and θu vectors) is plotted for the different trackers. A divergence in the estimated pose compared to the real one is quickly followed by an increase of the confidence index. This allows to correctly detect the observed tracking drifts. Last row in Fig. 4 shows the covariance in translation and in rotation. As stated in Section IV-D, covariance matrix of the estimated pose cannot be used to reliably detect a tracking drift. Nevertheless, order of magnitude of the covariance values can give an estimate of the accuracy of the pose estimation. Thus, using an identical scale for each covariance plot allows comparing the accuracy of the pose estimation between each experiment. With edge, keypoint and depth features, precision of the pose estimation in translation is around the millimeter, and precision is around one-tenth degree in rotation. These values are coherent with the tracking results and the expected accuracy of the tracker at this distance.

C. Model-based tracking of a printer

In this experiment, the tracking of a printer is done using the ASUS Xtion sensor. The sequence is composed of 1408 images acquired at 30 Hz. Results are presented in Fig. Edge-based tracker correctly tracks the printer until around frame 610 when it starts to slightly drift. The greyish poorly contrasted background causes the ME to be falsely tracked and the estimated pose is thus not well registered with the object. The drift is detected at frame 684 when sufficient portion of the model is projected onto the background. Detection of the keypoint-tracker drift occurs at frame 228. The drift can be explained by the approximation made to model the top printer part by a planar face. Moreover, the printer is mostly textureless, which degrades the quality of this tracker. Once the model is badly registered, depth and keypoint features are badly computed and the tracking drifts until it triggers our drift detector. For the last case, the quality of the depth map is too poor to allow an accurate tracking using solely depth features. Fig. 5 shows the model projected into the color frame using the pose estimated with depth. Since the projected lines do not match with the printer contour, a drift is immediately detected.

Combining keypoints and edges allows avoiding the drift observed with the edge-tracker. This is not the case when combining edges and depths. Indeed, the depth map visible in Fig. 6 is not fully reliable and does not provide enough information to correct the wrong registration of the model. On the other hand, combining all the visual feature types allows successful tracking during the whole sequence.

From Fig. 7, the instants when the different trackers start to drift are visible on the estimated pose curves. As they diverge, the confidence index rapidly increases until reaching the alert threshold. Covariance measures are displayed for both edge + keypoint and edge + keypoint + depth tracking, since they are successful to track the printer for the whole sequence. Good precision of the pose estimation is also obtained on this sequence. Adding depth features allows increasing the tracking accuracy, which is confirmed from the covariance curve. Augmentation of the covariance at the end of the sequence corresponds to the movement when the camera moves away from the printer. This experiment illustrates the complementarity between classical cameras and depth sensors. Indeed, using depth only can be a limiting factor and the possibility to combine visual features of different nature is clearly an advantage of our tracker.

D. Model-based tracking of a circuit breaker

This last experiment involves the tracking of a circuit breaker using the ASUS Xtion sensor. The sequence is composed of 1362 images acquired at 30 Hz by moving manually the sensor.

Using a single type of visual features is not enough to correctly track the circuit breaker, as shown on Fig. 8. For the edge tracker, the drift arises when the camera comes really close to the object and no more vertical anchors are visible. When the camera goes back, this movement cannot be accurately estimated and ME are then tracked on elements in the background. Keypoint tracker is not able to properly track since the object is mainly textureless. Finally, the depth tracker is not efficient. Indeed, it is not possible to correctly estimate the 6 parameters of the pose with only a planar face. Combining edges and keypoints does not prevent the previously mentioned tracking drift. But thanks to the confidence index, it is correctly detected. As expected, combining edges and depths does not allow avoiding the issue. Only the full features combination allows successful tracking during the whole sequence. Indeed, while depth is useless to estimate the motions parallel to the panel, it does help to estimate correctly the distance to the panel and thus allows avoiding the tracking drift observed with edge and edge + keypoint features. From Fig. 10, tracking failures can be clearly seen on the estimated camera poses. Sudden increases in confidence index are directly linked to tracking drifts, while the confidence index always remains at a low value for the successful tracker. As the camera comes closer to the circuit breaker, the covariance values decrease. Indeed, accuracy in the pose estimation is lower when we are far from the object. Sudden peaks in the covariance curve correspond to the instants when the depth map cannot be exploited since the sensor is too close to the object. This is also noticeable by comparing the peaks with the t z translation plot.

In Fig. 11, the camera trajectories are displayed for edge and keypoint and edge, keypoint and depth trackers. Adding depth features allows having much more smoother trajectory, especially when the sensor is far from the object.

E. Availability

The complete model-based tracker presented in this paper is fully available in the ViSP library from https://visp.inria.fr under the GPL licence.

VI. CONCLUSIONS

We presented a general model-based tracker able to combine different types of visual features (edge, keypoint and depth) for improving the tracking robustness and accuracy. Implemented into the ViSP open-source library, the user can easily choose which features to use, depending on the nature of the object to be tracked. In particular, a novel depth feature has been integrated into the tracking framework. Thanks to the versatility of the tracking method, visual features extracted from different viewpoints can be fused as soon as the change of frames between each sensor is known from an a priori calibration step. Experiments have shown that adding depth information to the classical edge or keypoint features improve the tracking robustness. Indeed, the dense nature of the depth map allows having more reliable measurements. Moreover, the model is registered in 3D (point-to-plane distance) rather than in the 2D image space. Even with objects whose full pose is not observable with depth features only, combining depth with edges and/or points of interest reduces tracking jitters.

Fig. 1 .

 1 Fig. 1. Edge-based feature overview

Fig. 2 .

 2 Fig. 2. Top row: edge tracker at frames 87, 172, 265. Middle row: keypoint tracker at frames 87, 172, 243. Bottom row: depth tracker at frames 50, 87 and corresponding color image at frame 87.

Fig. 3 .

 3 Fig. 3. Top row: edge + keypoint tracker at frames 172, 266. Second row: edge + depth tracker at frames 172, 344, 447. Third row: tracking using all features at frames 172, 344, 516, 688. Bottom row: visualization of the depth map at frames 172, 344, 516, 688.

Fig. 4 .

 4 Fig. 4. Comparison of the pose estimated by the six methods. Confidence index is displayed on 4th row, left figure and replicated on the right figure. Square root of the covariance norm is displayed on the last row, in translation (left) and in rotation (right).

5

 5

Fig. 5 .

 5 Fig. 5. Top row: edge tracker at frames 282, 564, 684. Middle row: keypoint tracker at frames 150, 200, 228. Bottom row: depth tracker at frame 1 and corresponding color image at frame 1.

Fig. 6 .

 6 Fig. 6. First and third rows: edge + keypoint and edge + keypoint + depth tracker at frames 282, 564, 682, 1128, 1408. Second row: edge + depth tracker at frames 282, 564, 682. Bottom row: visualization of the depth map at same frames than first and third rows.

Fig. 7 .

 7 Fig. 7. Comparison of the pose estimated by the six methods. Confidence index is displayed on 4th row, left figure and replicated on the right figure. Square root of the covariance norm is displayed on the last row, in translation (left) and in rotation (right).

Fig. 8 .

 8 Fig. 8. Top row: edge tracker at frames 272, 544, 788. Middle row: keypoint tracker at frames 272, 495. Bottom row: depth tracker at frames 50 and 173 and corresponding color image at frame 173.

Fig. 9 .

 9 Fig. 9. Top row: edge + keypoint tracker at frames 272, 544, 816, 853. Second row: edge + depth tracker at frames 272, 544, 774. Third row: edge + keypoint + depth tracker at frames 272, 544, 816, 1088, 1362. Bottom row: visualization of the depth map at the same frames.

Fig. 10 .

 10 Fig. 10. Comparison of the pose estimated by the six methods. Confidence index is displayed on 4th row, left figure and replicated on the right figure. Square root of the covariance norm is displayed on the last row, in translation (left) and in rotation (right).

Fig. 11 .

 11 Fig. 11. Left: camera trajectory estimated with edge + keypoint tracking. Right: camera trajectory estimated with edge + keypoint + depth tracking.

TABLE I MEAN

 I COMPUTATION TIME AND AVERAGE NUMBER OF VISUAL FEATURES. SCAN-LINE VISIBILITY IS USED FOR THE CASTLE SEQUENCE.

			Edge	Keypoint	Edge+ keypoint+depth
	Castle	Mean time Mean nb feat.	36 (ms) 747	8 (ms) 488	87 (ms) 5052
	Printer	Mean time Mean nb feat.	5 (ms) 369	5 (ms) 319	20 (ms) 5970
	Breaker	Mean time Mean nb feat.	7 (ms) 459	5 (ms) 110	23 (ms) 6518

More information in "Markerless generic model-based tracking" tutorial: http://visp-doc.inria.fr/doxygen/visp-daily/ tutorial-tracking-mb-generic.html.

http://visp-doc.inria.fr/doxygen/visp-daily/ tutorial-detection-object.html

This work was supported by the H2020 COMANOID EU project.