Maxime Meilland
email: maxime.meilland@inria.fr

Andrew Ian Comport
email: comport@i3s.unice.fr

Patrick Rives
email: patrick.rives@inria.fr

Real-time Dense Visual Tracking under Large Lighting Variations

This paper proposes a model for large illumination variations to improve direct 3D tracking techniques since they are highly prone to illumination changes. Within this context dense monocular and multi-camera tracking techniques are presented which each perform in real-time (45Hz). The proposed approach exploits the relative advantages of both model-based and visual odometry techniques for tracking. In the case of direct model-based tracking, photometric models are usually acquired under significantly greater lighting differences than those observed by the current camera view, however, model-based approaches avoid drift. Incremental visual odometry, on the other hand, has relatively less lighting variation but integrates drift. To solve this problem a hybrid approach is proposed to simultaneously minimise drift via a 3D model whilst using locally consistent illumination to correct large photometric differences. Direct 6 dof tracking is performed by an accurate method, which directly minimizes dense image measurements iteratively, using non-linear optimisation. A stereo technique for automatically acquiring the 3D photometric model has also been optimised for the purpose of this paper. Real experiments are shown on complex 3D scenes for a hand-held camera undergoing fast 3D movement and various illumination changes including daylight, artificial-lights, significant shadows, non-Lambertian reflections, occlusions and saturations.

Introduction

This work is supported through a project for automatic visual navigation in structured complex 3D environments, however, as will be shown, the application domain is much wider and also includes robot localisation and augmented reality. This paper focuses on the real-time tracking part of visual navigation in the presence of significant illumination change using an appearance-based method. Direct tracking methods [START_REF] Baker | Equivalence and efficiency of image alignment algorithms[END_REF] use an estimation model that minimises an error directly in the sensor space. This allows them to be robust to modeling error and to provide a single simplified model that does not require propagating uncertainty between different stages (such as feature based approaches). Whilst direct tracking approaches are incremental and local, full-image stereo techniques have been shown to efficiently track over a large convergence domain, especially when coupled with a multi-resolution pyramid [START_REF] Comport | Real-time quadrifocal visual odometry[END_REF]. The main drawback and criticism with this type of approach is therefore that of being able to handle significant illumination variation.

To handle illumination for direct methods, several approaches have been proposed. In [START_REF] Bartoli | Groupwise geometric and photometric direct image registration[END_REF] an affine model is used but only global changes are modeled. In [START_REF] Silveira | Real-time visual tracking under arbitrary illumination changes[END_REF] a more complete model is employed, including diffuse and specular reflections. Very good results are obtained, but a lot of unknowns are introduced in the minimization process which makes the algorithm more complex and robustness to occlusions is not handled. In [START_REF] Hager | Efficient region tracking with parametric models of geometry and illumination[END_REF] a complete robust registration technique is presented. Illumination changes are handled by modeling image variation using low dimensional linear subspaces. A learning step is carried out to generate invariant image representations from a set of images taken under different illuminations. Despite this learning step, the model construction deals only with Lambertian surfaces and is sensitive to occlusions and self-shadowing.

In the multi-view reconstruction and SFM literature [START_REF] Steven | A comparison and evaluation of multi-view stereo reconstruction algorithms[END_REF], powerful models have been used to perform 3D reconstruction. Illumination is generally modeled using surface normals and albedo [START_REF] Basri | Photometric stereo with general, unknown lighting[END_REF][START_REF] Zhang | Shape and motion under varying illumination: Unifying structure from motion, photometric stereo, and multi-view stereo[END_REF] but most of that work only considers Lambertian surfaces and distant lights, which are clearly not valid in uncontrolled environments. Furthermore those methods are often complex and used offline and are not well suited for online camera tracking.

Instead of using direct Sum of Squared Differences (SSD) [START_REF] Baker | Equivalence and efficiency of image alignment algorithms[END_REF], other error functions have been used. In [START_REF] Irani | Robust multi-sensor image alignment[END_REF] normalized cross correlation (NCC) is employed jointly with gradient oriented pyramids. This allows to register different images, however, NCC is a local measure and is not well suited for 3D scenes where perspective effects are significant. Other authors have proposed to maximize the information shared between images using Mutual Information [START_REF] Dame | Accurate real-time tracking using mutual information[END_REF][START_REF] Panin | Mutual information-based 3d object tracking[END_REF]. This is currently, one of the best ways to register highly different multimodal images (i.e. between a map and a satellite image). Nevertheless, these methods rely on smoothing an uncontinuous cost function in order to obtain an analytic gradient which degrades precision and leads to uncertain performance and convergence properties. Furthermore these methods rely on maximizing an information metric which is not sensor-based (and therefore they are less robust to model error).

To the authors' knowledge, the existing real-time direct tracking techniques which consider light change only tackle simple geometric scene models: planar homographies [START_REF] Hager | Efficient region tracking with parametric models of geometry and illumination[END_REF][START_REF] Silveira | Real-time visual tracking under arbitrary illumination changes[END_REF] or cylinders [START_REF] Marco | Fast, reliable head tracking under varying illumination[END_REF] and only small regions (patches) are tracked. Alternatively, the method proposed here handles general 3D scenes, and many additional undesirable effects are present: occlusions, self-shadowing, objects interactions, specular reflections, camera saturation and also uncertainties on the scene geometry (e.g. due to reconstruction). The central purpose of this paper is to address these illumination problems whilst maintaining a dense direct approach and considering real-time aspects.

Proposed approach

The novelty of this paper is to propose a hybrid model-based/visual-odometry method which is robust to a large set of illumination changes whilst minimising drift with respect to a global reference frame (3D model). Firstly, a model-based (MB) approach is considered here to be one that minimises the error between a known model (3D+photometric) and the warped current image. In terms of illumination, large changes can be expected for the MB approach (see Fig. 1(b)) since there is a large temporal difference between the moment the model was acquired and the current image. Secondly, a visual odometry (VO) approach is defined as minimising the error between the image acquired at time t -1 and the warped current image at time t (see Fig. 1(a)). In this case relatively small changes can be expected (even if they must still be modeled). The following four types of illumination changes are identified:

• Global-Step. Abrupt lighting changes across the entire image -an ambient light being switched on or a large difference when initialising the pose between a model and a camera. The MB approach can be considered to be in a permanent Step configuration for local and global variation (although some local gradient must be preserved). Nevertheless, it is this approach which allows to avoid drift. The VO technique, on the other hand, undergoes more incremental lighting change (Ramp + Step) and is therefore much more robust to changes but it accumulates drift over time. As in [START_REF] Gonçalves | Real-time direct tracking of color images in the presence of illumination variation[END_REF], robustness to Global variation is handled by a robust centering measure and Local-step variations are rejected along with outliers using robust M-estimation. This paper, however, only dealt with planar VO tracking. Moreover, a full six degrees of freedom tracking approach is proposed which is based on a 3D model of the environment that contains both photometric and geometric information. The 3D model is automatically reconstructed in near real-time from a learning sequence that has been acquired by a stereo acquisition system. Online real-time tracking is then performed at 45Hz using a monocular or multi-camera system. The proposed approach will be shown to work in real-time with many different illumination conditions and with no drift.

Robust 3D direct image registration

Let I denote a set (considered also as a vector) of brightness image measurements, where for each image pixel p ∈ I a corresponding 3D point P ∈ R 3 is available (e.g. extracted offline by dense stereo matching). It is assumed that the scene geometry does not vary in time. The set of the measurements S = {I * , P * , W}, denoted with a ' * ', will be referred throughout that paper as the augmented reference image, where P * = {P 1 , . . . , P n }, n is the number of pixels in the reference image and W ∈ R 1×n is a saliency map indicating which pixels condition each of the 6 dof best.

Multiple augmented reference images are then joined together to form a global 3D photometric model. The N augmented reference images are related via a graph G = {S 1 , . . . , S N , T 1 , . . . , T M }, of M poses, which have been estimated off-line using the approach in [START_REF] Comport | Real-time quadrifocal visual odometry[END_REF] which will be re-introduced in Section 2.2. It can be noted that very few works have appeared in the literature which perform direct dense tracking from arbitrary geometry due to the difficulty in acquiring such models. Here this stereo technique has been made to run in near real time for fast augmented model acquisition (see Sub-section 3.2).

In the following subsections, first a model-based tracking approach will be presented and then a visual odometry based approach will be presented before combining both within a hybrid tracking framework.

P I t I t-1 T t-1
T(x)

I * I w t-1 (

Model-based (MB) tracking under illumination variation

Assuming that an augmented reference model G has already been reconstructed, the aim of model-based tracking estimate the 6dof. motion increment x ∈ R 6 which associates the current image I t acquired at time t, with the reference image I * . x is related to the homogeneous camera pose matrix T defined as:

T(x) = e [x] ∧ = R t 0 1 ∈ SE(3), x = 1 0 (ω ω ω, υ υ υ)dt ∈ se(3), [x] ∧ = [ω ω ω] × υ υ υ 0 0 , (1)
where ω ω ω and υ υ υ are the angular and linear motion components which are integrated to obtain a pose T and where [.] × represents the skew symmetric matrix operator, R ∈ SO(3) is a rotation matrix and t ∈ R 3 is a translation vector.

The warping function p = w(P, K; T(x)) = K [R t] P transfers 3D Euclidean points P onto another camera plane, via perspective projection according to a camera motion x, where K ∈ R 3×3 is the upper triangular intrinsic parameter matrix of the current sensor, which is calibrated and where p is the homogeneous pixel vector.

In the case of the true motion x, the current image intensities I t , at time t, are related to the reference image I * by:

I * (P * ) = α α α MB I t w P * ; T(x) -β MB , ∀P * ∈ R 3 , (2)
where the corresponding intensities are interpolated bi-linearly at points p to create novel view [START_REF] Avidan | Novel view synthesis in tensor space[END_REF]. α α α MB = diag(α 1 , . . . , α i ) ∈ R n×n is the local affine pixel-by-pixel diagonal gain matrix and β MB ∈ R is the global bias shift in intensity due to global illumination changes. The current camera pose can be estimated by minimizing the intensity error between the warped current image and the reference.

e MB = ρ α α α MB I t w(P * ; TT(x)) -β MB -I * (P * ) , ( 3
)
where T is the estimated pose up to time t -1 and T(x) is the incremental pose at time t.

Since this is a non-linear function in the unknown pose parameters, a robust iteratively re-weighted least squares minimization procedure can be used [START_REF] Comport | Real-time quadrifocal visual odometry[END_REF] to minimise the error function ( 3) O(x) = arg min x (e . (x)). An inverse compositional algorithm is used [START_REF] Baker | Equivalence and efficiency of image alignment algorithms[END_REF], which allows to precompute most of the minimization parts directly in the augmented reference image. In this case the unknown x is iteratively updated using Gauss Newton optimisation:

x = -(J T MB D MB J MB ) -1 J T MB D MB e MB , (4)
where J MB is the Jacobian of (3) wrt. x.

As mentioned in the introduction, the MB approach is considered to be continually in a global step configuration since it was acquired under significantly different illumination conditions than the current camera. The global illumination change is determined robustly by performing a global shift of the error with respect to a robust estimate of the center of the distribution (its median): β MB = Median(e MB ). This efficient and robust technique was originally proposed in [START_REF] Gonçalves | Real-time direct tracking of color images in the presence of illumination variation[END_REF] which the interested reader may consult for more detail.

On the other hand, the pixel-by-pixel gain α α α MB (Local illumination variation) is, unfortunately, not locally observable and it is difficult to separate from local occlusions and interpolation error. For the purpose of model-based tracking, the local gain α α α MB will be considered to be absorbed by a robust diagonal weighting matrix α α α MB ⊂ D MB = diag(w 1 , . . . , w i ). The weights w i , which reflect the confidence of each pixel, are given by [START_REF] Huber | Robust Statistics[END_REF]:

w i = ψ(δ i /σ ) δ i /σ , ψ(u) = u , if |u| ≤ a a u |u| , if |u| > a, (5)
where δ i is the normalized residue given by δ i = ∆ i -Med(∆) and ψ is the influence function.Huber's function has been chosen where the proportionality factor is a = 1.2107 and represents 95% efficiency in the case of Gaussian noise.

It will be seen in the results that since the MB (3) images differ greatly from the current image, convergence is slow, there is a greater potential for local minima and tracking is not robust and easily fails.

Visual odometry(VO) tracking under illumination variation

This section describes a non-classic visual odometry function that takes advantage of the precomputed depth map from a known 3D model G. This allows to avoid computing a dense depth map online which is very costly for real-time applications. The modified VO technique is therefore based on the augmented reference image S = {I w t-1 , P * , W} which combines the 3D model reference points P * with the image I w t-1 from time t -1. This image is warped, in conjunction with the current image, to the reference image position (see Fig. 1(a)):

I w t-1 (P * ) = I t-1 (w(P * ; T t-1 )) (6)
This leads to an optimised VO error function (corresponding to the temporal inter-frame intensity variation) that is expressed in the reference frame:

e VO = ρ α α α VO I t (w(P * ; TT(x))) -β VO -I w t-1 (P * ) , (7)
where T -1 t-1 T(x) is the classic inter-frame VO camera motion. The illumination changes have been modeled again by a global bias β VO and local gain α α α VO ⊂ D VO . It is assumed that the pose of the camera T 0 at time t = 0 has been adequately initialised (either by an initialisation procedure or initial conditions that are within the convergence domain). ( 7) is minimized using a robust least square optimisation as in [START_REF] Basri | Photometric stereo with general, unknown lighting[END_REF].

Considering that images are acquired at video framerate (e.g. ≥ 25 Hz), the illumination changes for VO between two consecutive frames I t-1 and I t are much smaller than for the MB technique (even if some step variation is allowed) : α α α VO << α α α MB and β VO << β MB meaning that the error e VO << e MB . Since the global step change is easily modeled, both VO and MB approaches handle nicely global variation. On the other hand, the temporally local illumination change can be more efficiently handled by the VO approach and in many cases the MB approach will simply fail due to heavy local variation (see Fig. 3). Once again this approach still allows to account for outliers.

Hybrid model-based and visual odometry (H) tracking

This section presents a hybrid approach to take advantage of both MB and VO approaches. In the first case MB tracking ensures no drift, however, it is prone to large illumination differences and subsequently slow to failed convergence. On the other hand, VO provides temporally close illumination measurements, however, it is prone to drift. In a sensor-based approach it is important to maintain raw sensor measurement as reference in the minimisation process to ensure precision, avoid drift and remain robust to modeling error. In that case, if an illumination model was to incrementally adjust the reference image then drift would creep into the system. It is for this reason that the local illumination changes are not estimated as part of the state vector.

In this hybrid case a global error function is defined by stacking e MB and e VO as:

Since the minimized error distributions e MB and e VO may have different modalities, two independent robust and centering functions are used, to ensure not rejecting the nondominant modality. ( 8) is minimized using a robust least square optimisation as in [START_REF] Basri | Photometric stereo with general, unknown lighting[END_REF]. The hybrid Jacobian is efficient to compute since the geometric part of the VO is the same as for MB.

Fig. 2 shows an example convergence obtained with MB-VO minimization (8) compared to MB (3) and VO [START_REF] Dame | Accurate real-time tracking using mutual information[END_REF]. Since the VO inter-frame error is close to zero, a fast exponential convergence to the minimum is guaranteed, but small errors are accumulated. The MB approach convergence is slower due to large illumination differences whilst the hybrid method (H) converges faster and without drift.

Results

Experimental validation

To prove the efficiency and the robustness of the method, a set of augmented reference images are acquired and reconstructed from a stereo image pair (see Section 3.2). The current image sequence was acquired using a hand held camera and extreme light variations are created in several ways including using a spotlight to generate local step and ramp variations, whilst creating shadows due to occluding objects and sensor saturations in the current image. At time t = 0 the current image pose w.r.t the reference pose is initialised at a known starting point so that T(x) = I. Movements of the camera are generated manually thought the full six dof and the camera is returned to the origin (a camera tripod) so as to provide a ground truth on the final camera pose (i.e. the identity matrix).

One of the test sequences presented here contains a set of 1243 images. The sequence was tracked using 3 different methods. First only the MB (3) approach was performed, next only the VO (7) approach and finally using the proposed hybrid approach (H) [START_REF] Gonçalves | Real-time direct tracking of color images in the presence of illumination variation[END_REF]. Each tracking phase was computed using the same tuning parameters (e.g. maximum number of iterations, convergence threshold).

Fig. 3 shows the six estimated dof obtained using each method. It can be seen that for the MB method, the tracking fails at image 761. The VO method tracked throughout the image sequence but errors are accumulated along the sequence leading to drift. The hybrid method, tracked well throughout the entire image sequence. No drift is accumulated since the true reference image is still involved non-linearly in the minimization process.

Automatic 3D Model acquisition

To build the 3D model G it is necessary to perform dense localisation and mapping during a learning phase. In practice a classic rectified stereo rig is used. Dense matching is then computed along epipolar scan-lines using a standard algorithm [START_REF] Hirschmuller | Stereo processing by semiglobal matching and mutual information[END_REF] and triangulation is performed to obtain a set of 3D points P * corresponding to the image pixels.

Since the 3D model is reconstructed offline it is possible to use a more accurate and robust parametrisation of the localisation method. In particular, dense optimisation is performed using all the available data, estimation is allowed more iterations to converge and loop closing is performed where possible. Direct minimization is performed between the last constructed augmented reference image and the current image in the sequence. A new reference image is constructed and added to the model when the robust estimate of the scale of the minimized error distribution (e.g. the median absolute deviation (MAD)), is greater than a certain threshold or when the number of overlapping pixels between the current and the last reference view is too small.

Real-time Experiments

In each experiment, a 3D model is first acquired as in Section 3.2. In the experiment presented here, the 3D model is built and 20 reference images were necessary to cover the zone which was mapped. This database is later used to track a hand-held monocular camera (or stereo camera pair) in real-time. Using stereo online is more robust, however, it requires slightly more computation than monocular tracking. For the online tracking, a real-time implementation has been realized in C++. The algorithm runs at 45 Hz on an Intel Core 2 Duo laptop at (2.2 Gz) for stereo images of dimension 800 × 600. To achieve this frame rate optimisation was first performed by creating a saliency map of the reference images' pixels [START_REF] Meilland | A spherical robot-centered representation for urban navigation[END_REF]. The saliency map was then used to select a small number of the pixels necessary to accurately perform the tracking (about 3.10 4 pixels/image).

To initialise tracking (or recover from tracking failure), the closest image from the database is obtained by performing direct low-resolution tracking between the first image acquired from the live camera and each image from the database. The best score in terms of "Zero Normalised Correlation" is then chosen as the initial image. The initial pose T 0 is then estimated at full resolution. To guarantee convergence and precision at initialisation, all pixels are used in the minimization and a lot of iterations are allowed. Since this is an initialization step, more computation time is allowed and the camera is held static for a few seconds. A technique which scales well with large models would merit further investigation.

The main set of results captured from the real-time tracking experiments are shown in Fig. 4. The figure shows some aligned images from different tracking experiments. In the figure a first experiment was performed in the afternoon ((a), (b)) with direct beams of sunlight modifying illumination conditions. Another experiment was performed at night ((c), (d), (e)) and only artificial lighting was used. The camera exposure time was intentionally set high in order to provoke camera saturations. To highlight tracking results, a synthetic view of the reference image is rendered in real-time according to the current computed pose and displayed beside the current image. It can be seen that the proposed tracker is able to track all the 6 dof of the camera in various extreme perturbations cases: camera shaking and motion blur, defocus and aperture changes, occlusions, saturation, shadows and even with 180 degrees rotation. The video accompanying this paper better illustrates the results and is directly captured from the visual output of the tracking implementation, as are the images of Fig. 4).

Conclusions

The real-time hybrid tracking method presented here is shown to track robustly in complex 3D scenes at 45Hz on very challenging image sequences. It efficiently combines a model- Since this model is non-linear and direct it also leads to accurate tracking. The 3D model was also acquired automatically in near real-time using a stereo learning phase. Salient pixel selection has allowed this direct algorithm to run at high frequency on a standard laptop.

An important aspect which has not been considered in this paper are the geometric changes in the scene over time. A possible future direction will be to extract depth online from a stereo camera pair and jointly minimizing the inter-frame displacement with an augmented reference image from the database.

•

  Local-Step. Abrupt lighting changes locally in the image -a spotlight being switched on, specular reflections, shadows or occlusions induced by a change of camera viewpoint. • Global-Ramp. Slow lighting changes across the entire image -a moving light or a change of light source intensities (e.g. clouds, position of the sun,...). • Local-Ramp. Slow lighting changes locally in the image -a moving spotlight, shadows or local reflections.

  a) Cameras position in the scene. (b) MB tracking: registration results.

Figure 1 :

 1 Figure 1: (a). In red, the augmented reference image I * . In blue, the image I w t-1 from time t -1 warped onto the reference. The current camera image I t at time t. For the MB configuration the error I t -I * is minimised and for the VO configuration I t -I w t-1 is minimised. (b). Top left, robust outliers weights. Top center, augmented reference image I * . Top right, reference depthmap. Bottom left, intensity error I w t -I * after alignment. Bottom center, warped current image I w t . Bottom right, original current image I t .

  e H = ρ I t (w(P * ; T(x)) -I * (P * )β MB ρ I t (w(P * ; T(x)) -I w t-1 (P * )β VO .

Figure 2 :

 2 Figure 2: Convergence speed is compared between MB (red), VO (green) and the proposed hybrid H (blue).

Figure 3 :

 3 Figure 3: Norms of the 6 estimated dof of a loop trajectory containing 1243 images. Top, translation norm. Bottom, rotation norm. The MB tracking fails after image 761. Error was accumulated using only VO. The proposed hybrid approach (H) tracked well the camera, giving an identity pose at the end of the sequence.

  (a) Full 6 dof motion with local saturations. (b) 180 degree rotation with local saturations. (c) Night illumination with global illumination variation. (d) Spot illumination with shadows, and local reflections. (e) Spot illumination with occlusions.

Figure 4 :

 4 Figure 4: Different tracking results. For each row of the image from left to right: Current image; Synthetic 3D view rendered at the estimated pose (corresponding to the current image pose); Original reference image

This work has been supported by ANR (French National Agency) CityVIP project under grant ANR-07_TSFA-013-01.


Arnaud Tanguy

Daniele De Simone

Andrew I Comport

Giuseppe Oriolo

Abderrahmane Kheddar

Closed-loop MPC with Dense Visual SLAM -Stability through Reactive Stepping

Walking gaits generated using Model Predictive Control (MPC) is widely used due to its capability to handle several constraints that characterize humanoid locomotion. The use of simplified models such as the Linear Inverted Pendulum allows to perform computations in real-time, giving the robot the fundamental capacity to replan its motion to follow external inputs (e.g. reference velocity, footstep plans). However, usually the MPC does not take into account the current state of the robot when computing the reference motion, losing the ability to react to external disturbances. In this paper a closedloop MPC scheme is proposed to estimate the robot's real state through Simultaneous Localization and Mapping (SLAM) and proprioceptive sensors (force/torque). With the proposed control scheme it is shown that the robot is able to react to external disturbances (push), by stepping to recover from the loss of balance. Moreover the localization allows the robot to navigate to target positions in the environment without being affected by the drift generated by imperfect open-loop control execution. We validate the proposed scheme through two different experiments with a HRP-4 humanoid robot.

I. INTRODUCTION

In recent years, humanoid robots have been given increasingly more attention due to their ability to perform complex tasks, thanks to their highly redundant kinematic structure. However, those systems are also challenging to control because they have very complex dynamics. Therefore researchers tend to use simplified models to approximate the humanoid robot dynamics. When dealing with humanoid locomotion, the most commonly used model to approximate the dynamics of walking is the Linear Inverted Pendulum (LIP) [START_REF] Kajita | Introduction to Humanoid Robotics[END_REF], and the use of Model Predictive Control (MPC) has become predominant [START_REF] Herdt | Walking without thinking about it[END_REF][START_REF] Naveau | A reactive walking pattern generator based on nonlinear model predictive control[END_REF]. In traditional MPC-based gait generation techniques, a reference motion is generated without taking into account the current robot state, however, planning a motion starting from the current robot state is often necessary to react to unexpected situations and to obtain more robust motions. In this paper the problem of closedloop MPC is tackled [START_REF] Villa | Model predictive control of biped walking with bounded uncertainties[END_REF][START_REF] Feng | Robust dynamic walking using online foot step optimization[END_REF], and the robot state is estimated using SLAM and force/torque sensor measurements.

The use of SLAM for humanoid locomotion planning has been widely used, since the introduction of dense RGB-D approches, such as [START_REF] Whelan | Kintinuous: Spatially extended kinectfusion[END_REF][START_REF] Meilland | On unifying keyframe and voxel-based dense visual SLAM at large scales[END_REF] because they both provide a dense 3D map of the explored environment and 6D localization at sensor framerate with centimeteric precision. With this information, it becomes possible to plan robot actions w.r.t. the environment. In [START_REF] Fallon | Continuous humanoid locomotion over uneven terrain using stereo fusion[END_REF], a pair of Multisense stereo cameras Fig. 1. HRP-4 walking with the proposed closed-loop MPC with dense visual SLAM feedback. The estimated LIP pendulum state obtained from SLAM and force-sensor measurements is shown as a red line going from the ZMP estimate to the robot's CoM. The green line represents the desired pendulum state. Blue and green squares are respectively the desired optimal footstep, and a non-optimal footstep that minimizes changes of the footstep trajectory.

are used to generate RGB-D inputs for use with Kintinuous SLAM [START_REF] Whelan | Kintinuous: Spatially extended kinectfusion[END_REF]. A footstep plan for walking over a rough brick field is generated from its map, while the robot state is estimated with a kinematic-inertial estimator [START_REF] Fallon | Drift-free humanoid state estimation fusing kinematic, inertial and lidar sensing[END_REF]. Footsteps are selected within the map, but no attempt is made to change the next robot's footstep should its state differ from a feasible one. In [START_REF] Scona | Direct visual SLAM fusing proprioception for a humanoid robot[END_REF], kinematic-inertial measurements are used to improve the robustness of ElasticFusion [START_REF] Whelan | ElasticFusion: Dense SLAM without a pose graph[END_REF], and are applied to a cartesian walking controller to repeatedly walk towards goal positions in the environment. The proposed approach is depicted in Figure 1.

II. MPC-BASED GAIT GENERATION

When dealing with a complex system like a humanoid robot, it is a common practice to rely on a simplified model, the Linear Inverted Pendulum (LIP) [START_REF] Kajita | Introduction to Humanoid Robotics[END_REF] to describe the humanoid behavior during locomotion. The reason why the LIP is widely used when dealing with humanoid locomotion is because its dynamics approximate well the motion of the Center of Mass (CoM) of a biped robot, and the differential equations governing the two directions of motion x and y are linear, identical and decoupled. So, from now on, only the sagittal component of motion will be referred to.

Consider, without loss of generality, the evolution of the sagittal component of motion x

  ẋc ẍc ẋz   =   0 1 0 η 2 0 -η 2 0 0 0     x c ẋc x z   +   0 0 1   u z (1)
where x c is the position of the CoM, x z the position of the Zero-Moment Point (ZMP), η = g/h and h the constant height of the CoM. In the motion model (1) we assume controls u z are piece-wise constant over time intervals of duration δ.

The gait generation is based on the intrinsically stable MPC [START_REF] Scianca | Intrinsically stable MPC for humanoid gait generation[END_REF], where the decision variables are the ZMP velocities ( ẋi z , ẏi z ), i = 1, . . . , N and the footstep positions and orientations (x j f , y j f , θ j f ), j = 1, . . . , M , over a prediction horizon T h = N δ.

The choice of a MPC-based gait generation allows us to give the robot a high-level task, through an appropriate cost function to be minimized, and also to impose constraints to enforce stability, to maintain balance and to guarantee the kinematic feasibility of the robot motion.

A. Cost Function

In the formulation proposed here, the aim is to have the robot track high-level reference sagittal and coronal velocities (v x , v y ), and an angular velocity ω, by an appropriate selection of ZMP velocities ( ẋi z , ẏi z ) and footstep positions and orientations (x j f , y j f , θ j f ). However to maintain linearity in the MPC formulation, the footstep orientations must be chosen before the computation of their positions [START_REF] Herdt | Walking without thinking about it[END_REF].

Therefore, a first optimization problem is solved, minimizing a function that takes into account the reference angular velocity ω to determine the footstep orientations

θ j f M j=1 θ j f -θ j-1 f T s -ω 2 , (2)
where T s is the constant duration of a step, subject to the linear constraint |θ j f -θ j-1 f | ≤ θ max , that limits to θ max the maximum difference in orientation between two consecutive footsteps.

Once the foot orientations are decided, the footstep locations and the ZMP velocities can be computed via the minimization of a second cost function

N i=1 ( ẋk+i z ) 2 + ( ẏk+i z ) 2 + k x ( ẋk+i c -v x cos(iωδ) + v y sin(iωδ)) 2 + k y ( ẏk+i c -v x sin(iωδ) -v y cos(iωδ)) 2 , (3)
where the first two terms penalize the control effort, while the last two terms are meant to minimize the deviation from the reference velocities (v x ,v y ).

B. Constraints

In the following the three constraints enforced in the proposed MPC scheme will be briefly presented. The first, The pendulum state is estimated from HRP-4 sensors: its CoM position and velocity is obtained with dense visual slam from RGB-D measurements, and its ZMP from force-sensor measurements. This state is used to compute the MPC predictions, i.e. the future ZMP velocities and associated footsteps. Through integration, these references converted to reference position are then tracked by a quadratic programming controller that sends a desired whole-body configuration to the position-controlled robot.

introduced in [START_REF] Scianca | Intrinsically stable MPC for humanoid gait generation[END_REF], is the stability constraint, which takes the form

1 η 1 -exp(δη) 1 -exp(N δη) N i=1 exp(iδη) ẋk+i z = x k c + ẋk c η -x k z . (4)
This constraint will guarantee the boundedness of the computed CoM trajectory.

In order to guarantee balance during locomotion, we must guarantee that the ZMP is at any time instant inside the robot current support polygon. To do so we define a rectangle with sides d z

x , d z y , and therefore the balance constraint takes the form

R T j   δ k+i-1 l=k ẋl z -x j f δ k+i-1 l=k ẏl z -y j f   ≤ 1 2 d z x d z y -R T j x k z y k z , (5)
where R j is the rotation matrix associated to the angle θ j f . The last constraint is to guarantee that the choice of the next footstep is in a location that avoids self collisions and is inside the robot kinematic limits:

R T j-1 x j f -x j-1 f y j f -y j-1 f ≤ ± 0 + 1 2 d f x d f y , (6)
where d f x and d f y are the sides of a rectangle defining the feasibility zone, and l is a reference distance between two consecutive footsteps.

III. CLOSED-LOOP MPC

Decision variables (future ZMP velocities and next footstep locations) are computed by solving the QP formulation of the MPC described in Sect. II, which requires an initial state of the pendulum, defined by its CoM position and velocity x c , ẋc and its ZMP position x z . Traditional MPC gait generation typically envolves this state from an initial value (supposed known) by forward integration of its decision variables through a LIP motion model. This assumes that the

Push Prediction

Step Recover underlying system represented by the reduced LIP model is behaving perfectly as expected. Unfortunately, real physical systems are subject to interactions with their environment, which can easily perturbate the state of the system (imperfect contacts, external forces, poor tracking of the reference pendulum by the underlying system). It is proposed here to estimate the state of the system, and periodically use it to compute an updated MPC solution. An overview of the proposed closed-loop MPC is given in Figure 2. The next sections will show how this estimation is achieved, and used to obtain a closed-loop MPC formulation.

A. Estimation of the floating base

The robot's floating base pose B X 0 and velocity B V 0 is a prerequisite to estimating its CoM and ZMP state, see [START_REF] Nori | Sensor Fusion and State Estimation of the Robot[END_REF]. In this work, two simple estimators are considered. The estimation model is depicted in Figure 4.

The first provides ground-truth measurement obtained from a high-precision VICON mocap system composed of 8 infrared cameras. Reflective markers are placed on the robot's floating base link, forming a frame V , and tracked by the system with high accuracy at a rate of 100Hz.

B X 0 = B X V V X 0 . ( 7
)
where B X V is the transformation between the VICON markers frame and the floating base and S X H is the transformation between the camera optical frame and the robot's head link.

The second is obtained from the dense visual SLAM method of [START_REF] Meilland | On unifying keyframe and voxel-based dense visual SLAM at large scales[END_REF], where RGB-D images from the robot's Asus Xtion sensor are used to estimate the pose of the sensor's optical frame S. Contrary to IMU-based estimators usually used in walking scenarios, visual SLAM provides the full position and orientation in a global world-frame.

B X 0 = B X H H X S S X 0 . (8)
Fig. 4. This HRP-4 is fitted with a passive mechanism between its ankles and feet soles, which provides a spring-damper compliant system. Its state is taken into account by estimating the robot's floating base with dense visual SLAM and validated with a VICON tracking system, with reflective IR markers attached to the floating base link. The ZMP is estimated from ankle force-torque sensors. Note that here it is depicted outside of the robot's support polygon and triggers a step from the MPC.

Note that B X V and S X H are calibrated by Hand-Robot calibration [START_REF] Tanguy | Online Eye-Robot Self-Calibration[END_REF] with RobCalib software 1 . The transformation between the head link and the floating base B X H depends on an accurate kinematic tree calibration, and joint-encoder measurements. HRP-4 is fitted with high-accuracy optical encoders, providing this information at control rate (200Hz).

Pose estimates obtained from both VICON and SLAM are noisy, and its velocity is not directly estimated. The use of the Savitzky-Golay filter [START_REF] Savitzky | Smoothing and differentiation of data by simplified least squares procedures[END_REF] is proposed, for which an open-source implementation 2 is provided based on Gram-Polynomials [START_REF] Gorry | General Least-Squares Smoothing and Differentiation by the Convolution (Savitzky-Golay) Method[END_REF]. This filter has two main advantages: first, when applied at the end-point of a filtering-window, no additional delays are introduced, and the n th order derivative can be readily obtained. Second, it can be efficiently implemented as a convolution, whose weights depend on the size of the time window, the order of Gram-polynomials, and its derivative order.

Position can be directly smoothened by the filter. Rotation smoothing for a time window consists of solving the following maximum likelihood

R = argmin R∈SO(3) k d geo (R, R k ), (9)
where

d geo (R 1 , R 2 ) 2 = 1 2 ||logm(R T 1 R 2 )|| frobenius .
Many methods exist for solving this problem. One of them consists in applying a temporal convolution, followed by an orthogonalization, which can be shown to be a 2 nd order Taylor approximation of the geodesic distance [START_REF] Gramkow | On averaging rotations[END_REF].

R = k w k R k UDV T = svd( R) R = UV T (10)
This implies that rotation matrices can be directly smoothened at any point in a time window by the Gram-Savitzky-Golay convolution coefficients. Consequently, the filtered floating base position and velocity can be readily obtained.

B. CoM State

Knowing the floating base position and velocity, and the kinematic and inertial model of the robot, the CoM position is trivially obtained as C t 0 . However, our MPC formulation expects its state to be expressed w.r.t. the current support foot. HRP-4 feet are fitted with a passive mechanical flexibility between its ankles and the feet soles that act as a springdamper to protect the force sensors by reducing impacts, and provide some mechanical compliance while walking. The state of this mechanism is not measured but needs to be taken into account in order to correctly express the CoM in the support foot frame.

In the proposed MPC, the support foot is assumed to be flat on the floor. Let's denote by p f 0 = (x f , y f , 0) the position of the support foot in the inertial frame, and θ f z its rotation around the floor normal axis n z = [0, 0, 1] T . This frame assumes that the support foot stays in perfect contact with the floor, and that discrepancies in the CoM state are coming from the robot's passive ankle mechanism. In this frame, the CoM is expressed as

xc = R(θ f z ) p f 0 0 1 C t 0 . ( 11
)
2 https://github.com/i3s/gram_savitzky_golay

Its velocity is also expressed in the flat support foot frame. This implicitly embeds the state of the flexibility in the measurement of the CoM state, as long as the real robot's foot sole remains in perfect contact with the floor.

C. ZMP State

The ZMP can be computed w.r.t. to the CoM as

x z = x c - ẍc η 2 . ( 12
)
In practice ẍc can be obtained from accelerometer measurements, or by differentiating the CoM position obtained from the above estimate twice, which proves unreliable.

Since HRP-4 is fitted with force-torque sensors under its ankles providing measurements at control-rate (200Hz), a more reliable expression of the ZMP can be obtained, without the need for differentiation. The main interest is in the ZMP expressed in the ground plane, passing through a point p 0 and orthogonal to the unit vector n z . Let (f 0 , τ 0 ) be the total contact-wrench measured by the left and right foot force-torque sensors, expressed at the point p 0 . The ZMP is defined as a point Z where the moment of the contact wrench aligns with n z [START_REF] Caron | Zmp support areas for multicontact mobility under frictional constraints[END_REF], that is

n z × τ Z = 0. Consequently -n z × (p z × f 0 ) + n z × τ 0 = 0 (13) -(n z • f 0 )p Z + (n z • p Z )f 0 + n z × τ 0 = 0 (14)
With the additional constraint of the ZMP being obtained in the ground plane n z • p Z = 0, we obtain

p Z = n z × τ 0 n z • f 0 (15)

D. Computing the MPC from its estimated state

Once the estimated state x = (x c , xc , xz ) is computed, the MPC is initialized with it. This means that the prediction of the evolution of the system via the LIP model (1) over the prediction horizon T h is performed using the estimation as the initial condition. Moreover the cost function (3) and the constraints [START_REF] Villa | Model predictive control of biped walking with bounded uncertainties[END_REF][START_REF] Feng | Robust dynamic walking using online foot step optimization[END_REF][START_REF] Whelan | Kintinuous: Spatially extended kinectfusion[END_REF] are built using the estimated robot state.

However, the robot state estimation x is only available at the frequency of the slowest sensor, here SLAM (30Hz) or VICON (100Hz), both lower than the control rate (200Hz). Hence the state is not available at every control iteration. When no measured state is available, the initial LIP state is computed by integration of (1). The controls ẋi z , i = 1, . . . , N , are those computed by the optimization performed by the MPC in the previous iteration. The integration leads to  

x k+1 c ẋk+1 c x k+1 z   = A   x k c ẋk c x k z   + B ẋk z ( 16
)
where the matrices A and B have the form

A =     cosh(ηδ) sinh(ηδ) η 1 -cosh(ηδ) η sinh(ηδ) cosh(ηδ) -η sinh(ηδ) 0 0 1     , (17)
B =     δ - sinh(ηδ) η 1 -cosh(ηδ) δ     . (18)
In this way, a reference whole-body control can be provided at every iteration, by propagating the MPC solution, computed either from the estimated robot state, or its integrated internal state.

E. Choice of footstep

When the loop on the MPC is closed, its solution is computed based on the estimated state (x c , xc , xz ). Hence, at any time instant the optimal solution found by the optimization, will in general be different from the previous one, even in the absence of a real perturbation in the state (due to measurement noise and uncertainties). This leads to different optimal footstep target solutions at every MPC iteration. When tracked with a whole-body QP controller, these abrupt changes will be smoothened to some extent depending on the task gains, reducing tracking precision of the swing foot trajectory, and leading to shaky motions. To overcome this issue, two possible solutions are considered.

The first consists in adding to the cost function (3) a term of the form

k f x j f -xj f 2 + y j f -ȳj f 2 , (19)
that penalizes the difference between the predicted footstep (x j f ,y j f ) and the previously chosen footstep (x j f ,ȳ j f ). However this also affects the other high-level tasks assigned to the robot, e.g. the reference velocity tracking, because the robot minimizes difference between two footsteps therefore making steps as short as possible.

The second option relies on the fact that the solution found by the MPC is the optimal one, but it's not the unique solution that satisfies the constraints. Therefore, if the constraints are still satisfied by keeping the previously found footsteps with the new ZMP trajectory, it is chosen to keep it, even if it is not the optimal in terms of cost function. In this way we somewhat filter the continuous change of predicted footsteps, and only the latest optimal solution of the MPC is chosen if the previous one becomes invalid because it violates the constraints, which guarantees robot balance.

F. Quadratic Programming Controller

The desired MPC solution is then tracked by a wholebody quadratic controller (QP), which generates whole-body motion for the real model. A detailed explanation of the QP controller used in this work can be found in [START_REF] Bouyarmane | Humanoid robot locomotion and manipulation step planning[END_REF][START_REF] Escande | Planning contact points for humanoid robots[END_REF]. Its decision vector is z = (q, λ C ), where q gathers the linear and angular acceleration of floating-base coordinates, and the generalized joint velocities. λ C denotes the vector of conic coordinates of linearized Coulomb friction cones, such that the contact forces f are equal to S f λ C with S f the span matrix of cone generators. The cost function includes various tasks T that drive the whole-body state towards a desired configuration. We consider here

• T CoM tracks the desired CoM position x c and velocity ẋc .

• T Trajectory tracks the desired swing foot target x f .

• T CoP tracks the desired ZMP through admittance control, achieved by the embedded stabilizer. • T Posture is a reference posture that keeps the robot upright. The overall QP can be summed up as follows:

z = argmin z N i=1
w i T i (q, q, q) 2 + w λ λ C 2 subject to: 1) dynamic constraints 2) sustained contact positions 3) joint limits 4) non-desired collision avoidance constraints 5) self-collision avoidance constraints [START_REF] Escande | Planning contact points for humanoid robots[END_REF] Here, w i and w λ are task weights, and T i (q, q, q) denotes the residual of the i th task. The reader is referred to [START_REF] Vaillant | Multi-contact vertical ladder climbing with an HRP-2 humanoid[END_REF] for details on the formulation of all these constraints. The obtained reference acceleration q is integrated twice to obtain a desired joint configuration q which is then executed by the actuators proportional-derivative (PD) controllers.

IV. EXPERIMENTS

The proposed work is based on the MPC formulation presented in [START_REF] Scianca | Intrinsically stable MPC for humanoid gait generation[END_REF]. Including footstep generation into the formulation enables an intuitive and flexible way to control the locomotion of a humanoid robot. Walking is achieved by specifying a reference velocity. Footsteps, CoM and ZMP trajectories are chosen accordingly. This work has previously been demonstrated for open-loop control of a NAO humanoid robot, including pursuit avoidance scenarios in the presence of obstacles [START_REF] Simone | MPC-based humanoid pursuit-evasion in the presence of obstacles[END_REF]. However, it has never been demonstrated applied to large-scale humanoids such as HRP-4. In this section, we show that the MPC can be successfully applied to such a robot while validating its closed-loop implementation.

A first experiment validates the walking performance with a simple cartesian regulator based on SLAM localization that makes the robot walk to specified targets in its environment.

The second experiment validates the closed-loop aspect of the MPC by reacting to perturbations that would otherwise make the robot fall, even in the presence of a stabilizer.

For all experiments, ground-truth floating base position is obtained from a VICON motion capture system, and SLAM estimations are obtained from Asus Xtion's RGB-D frames with D6DSLAM [START_REF] Meilland | On unifying keyframe and voxel-based dense visual SLAM at large scales[END_REF] software. A video is provided for each experiment respectively.

A. Stabilization

The desired ZMP computed by the MPC is sent to the embedded stabilizer of HRP-4 [START_REF] Yokoi | Experimental study of humanoid robot hrp-1s[END_REF]. Its role is to compensate deviations from a reference ZMP trajectory by accelerating the torso in the opposite direction. This is achieved by modifying the desired ankle joint reference. In case of a perturbation too large to be recovered without taking a step, the robot would normally fall. Instead, with our proposed closed-loop MPC the robot takes a step forward, and the stabilizer tracks the new reference ZMP (see Figures 3,5, and the video).

B. Drift-free cartesian space control

An additional benefit of the proposed closed-loop control is its ability to robustly reach a target position specified in the environment. To illustrate this, a simple cartesian regulator was implemented to compute MPC reference velocities (v x , v y , ω) according to robot position and orientation relative to a desired world position (x d , y d , θ d ). The floating base pose (x b , ỹb , θb ), estimated with SLAM, is chosen as the robot's reference surface. Ground-truth measurements are obtained as (x v , y v , θ v ) with the VICON tracking system.

  v x v y ω   =   λ x (x d -xb ) λ y (y d -ỹb ) λ θ (θ d -θb )   (21)
Figure 6 shows the result of HRP-4 repeatedly walking from an initial position t 0 = (0, 0, 0) towards a target t1 = (1, 0.5, 0), then returning to its initial position before walking to a second t 2 = (1, -0.5, 0). Without feedback, considerable drift is observed and the targets are not reached accurately. Using the estimated robot state, the regulator leads the MPC to walk accurately towards each target.

C. Push reaction

In the second experiment, the robot is stepping in place with a low-weight cartesian-regulator (λ x = λ y = λ θ = 0.2) when it is pushed from behind, see e.g. [24][25]. The MPC is computed from the estimated robot state (x c , xc , xz ), obtained by the combination of SLAM and force-torque sensors. When perturbed, the MPC decides to change it's desired footstep, and computes a new corresponding ZMP velocity trajectory. This behavior emerges as a result of the optimization: in order to find an optimal solution that satisfies the constraints (bringing the future ZMP trajectory back inside the support foot polygon while respecting kinematic feasibility), the MPC has to move the ZMP forward thus performing a step.

Figures 5 and3 show this behavior on a real perturbation. As can be seen from the first plot, around time 194.7s, the perturbation becomes large enough for the ZMP to exit the balance constraint area (defined as a sub-rectangle of the real foot) with sufficient velocity, and the MPC has no choice but to alter its desired footstep. The shaky behavior of the footstep solution can be seen on the MPC footstep plot, as the experiment was performed without penalizing any change of footstep. Good estimation of the floating base position is achieved with SLAM, although additional delays are introduced, and the velocity estimates remain quite noisy.

V. CONCLUSION

A closed-loop formulation of the intrinsically stable MPC for humanoid gait generation [START_REF] Scianca | Intrinsically stable MPC for humanoid gait generation[END_REF] is proposed, where the robot CoM position and velocity are estimated with visual SLAM, while the ZMP position is computed from force/torque sensing. The closed-loop implementation of the MPC allows the robot to react to external disturbances through stepping, enhancing the overall robustness of the control scheme. Moreover, the visual localization of the robot in the environment, allows it to navigate with precision to specific locations, recovering from the drift generated by uncertainties and unmodeled interactions with the environment (e.g. foot slipping). As extension of this work, the dense SLAM map can be better exploited, in order to take into account obstacles on the robot path to avoid them.

Fig. 2 .

 2 Fig. 2. Overview of the proposed closed-loop MPC. A LIP reduced model is used to represent the dynamics of a humanoid robot walking on flat floor.The pendulum state is estimated from HRP-4 sensors: its CoM position and velocity is obtained with dense visual slam from RGB-D measurements, and its ZMP from force-sensor measurements. This state is used to compute the MPC predictions, i.e. the future ZMP velocities and associated footsteps. Through integration, these references converted to reference position are then tracked by a quadratic programming controller that sends a desired whole-body configuration to the position-controlled robot.

Fig. 3 .

 3 Fig.3. Reaction to a push during a real experiment with HRP-4. During the push, the ZMP-CoM pendulum estimated from SLAM and force sensor measurements reaches the edge of the foot (red line) while the robot's CoM is displaced forward. As a result, the MPC computes a new optimal footstep (blue square) and computes a footstep trajectory to bring its next swinging foot there. The cartesian-regulator controls the MPC target velocity to walk back to its initial position.

Fig. 5 .

 5 Fig. 5. A strong push forward causes the MPC to step forward to prevent loss of balance. Left -the estimated ZMP from force-sensor measurements is leaving the stability constraint area at time 194.7, the MPC plans a step forward to compensate. Middle -Estimation of the floating base position from SLAM and VICON measurements along the x and y direction. Right -Estimation of the floating base velocity.

Fig. 6 .

 6 Fig. 6. HRP-4 walks to specific places in the environment. A cartesianregulator continuously computes MPC reference velocities based on the robot's position w.r.t. its target as estimated by SLAM. It's trajectory is validated by both VICON ground-truth data, and visual markers on the floor. The video shows a comparative test of openloop vs closed-loop walking.

https://gitlab.com/i3s/robcalib

This work is supported by grant from the COMANOID H2020 project