Patent Publication Number: US-10762643-B2

Title: Method for evaluating image data of a vehicle camera

Description:
FIELD OF THE INVENTION 
     The invention relates to a method and a device for evaluating image data of a vehicle camera and can, in particular, be used to reconstruct the motion of the vehicle camera within the framework of a camera-based driver assistance system. 
     BACKGROUND INFORMATION 
     Visual odometry makes it possible to calculate the motion of a camera system from a sequence of images. In the case of a stereo system, the rotation about all three axes, as well as the translation along the axes and, consequently, the motion in all six degrees of freedom can be reconstructed. This information can be used as an estimate of the current driving states for different driver assistance systems such as e.g. anti-lock braking system (ABS), electronic stability control (ESC), cruise control or roll stability control (RSC). 
     Nowadays, a combination of different sensors is used for the self-localization of vehicles. These include, for example, rotational speed sensors, acceleration sensors, odometry and GPS systems. 
     In order to increase the accuracy of the determination of the vehicle&#39;s own position—but in particular also the availability—in critical scenarios such as street canyons or tunnel scenarios, camera-based egomotion estimates are very important as an alternative sensor concept, in order to extend the previous sensor concept with an additional and independent sensor source. 
     WO 2013/037840 A1 discloses a method for determining position data of a vehicle. Driving dynamics data of the vehicle are measured. Position data of the vehicle are measured with an environment sensor which acquires the position data on the basis of at least one distance from a stationary object arranged with respect to the vehicle. Finally, position data of the vehicle are determined on the basis of the driving dynamics data and the position data. Here, a vehicle camera can be used as an environment sensor. 
     WO 2010/099789 A1 discloses a method for automatically detecting a driving maneuver of a motor vehicle, in particular a passing maneuver or an evasive maneuver. In this case, data from the vehicle sensor technology and lane marker detection based on data from the video sensor technology can be fused in the odometry. The odometry makes it possible to estimate the position, speed and orientation of the vehicle on the lane as well as additional state variables. These estimated variables can be made available to maneuver detection, other situation analysis algorithms or for control tasks. 
     In addition, the information from a camera-based egomotion estimate can be used as input for any dead reckoning approach to estimate the trajectory and location of the egovehicle. With such approaches, the own location can be continuously estimated, taking account of a starting position and the subsequent course of the motion and speed (translation and rotation). 
     Due to the high level of accuracy that can be achieved, the information can furthermore be combined for each time step and can be used to reconstruct the trajectory or respectively for localizing the vehicle—for example as a source of information for risk assessments, trajectory planning, car2X exchange of information right up to autonomous driving. These data can be used, for example, to supplement an existing GPS system and to support it in critical situations. The need for this is demonstrated, for example, by investigations, according to which GPS availability is only 30% in Hong Kong or Calgary, for example. 
     A value of only 10% availability has even been established for construction site scenarios, which are very similar to street canyons. 
     Visual odometry can provide considerable support in precisely these scenarios since, in contrast to GPS receivers, it is not reliant on external signals. A distinction can be made between visual odometry systems, for example, based on the time window of their calculation. In addition to calculating motions between two consecutive time steps, additional information can be used from less recent time steps. Here, a distinction can be made between Simultaneous Localization and Mapping (SLAM) and Bundle Adjustment. Whereas in the case of SLAM, mapping is also carried out in addition to estimating egomotion, the aim of Bundle Adjustment is to improve the egomotion estimate by subsequently optimizing the triangulated spatial points. 
     Challenging situations for camera-based systems include, in particular, an insufficient number of correspondences of static scene points, heavily changing illumination and low brightness, an unstructured environment with homogeneous, non-textured surfaces, or an improperly low frame rate. High-speed scenarios such as, for example, along freeways or country roads combine several of these problems, making them one of the most challenging situations. Especially, the loss of or respectively the significantly lower number of suitable near features (correspondences) complicates the estimation of the egomotion. 
     The essential part of any visual odometry system is the detection of outliers. A variety of methods for detecting outliers are known. Purely flow-based approaches are based on the assumption that the optical flow follows patterns which are induced by the egomotion of the vehicle. Furthermore, model-based approaches exist, which explicitly constrain the flow using a certain motion model. Many known methods use reprojection error-based approaches. 
     B. Kitt et al. demonstrate, for example, in the publication  Visual odometry based on stereo image sequences with ransac - based outlier rejection scheme , IEEE Intelligent Vehicles Symposium, 2010, the calculation of a feature-based reprojection error for removing outliers, which is compared with a constant threshold. 
     In the case of RANSAC methods, a minimum number of correspondences selected at random is used, in each iteration, to create a motion hypothesis. Then, a score for each feature is calculated that describes whether the feature supports the motion hypothesis. If the motion hypothesis achieves sufficient support from the features, the non-supporting features are rejected as outliers. Otherwise, a minimum number of correspondences is selected at random again. 
     Alternative methods can be bracketed together as “MASOR” methods (MAximum Subset Outlier Removal). Here, the maximum number of features is taken to calculate a motion hypothesis. This motion hypothesis and a subsequent outlier removal step are repeated in an iterative scheme. Then a support score is calculated for every feature. Instead of judging the motion hypothesis, the support score is interpreted as a measure for the quality of a feature, as the hypothesis is considered to be a good estimate. Non-supporting features are rejected and the next iteration starts with the remaining features. This process is repeated until a termination criterion is met. 
     SUMMARY OF THE INVENTION 
     It is an object of the present invention to indicate an improved outlier detection for a motion estimate by a vehicle camera. 
     A first aspect of the invention relates to the reconstruction of the motion in all six degrees of freedom (6-DOF motion) of vehicles by means of a vehicle camera as a standalone system, or in order to extend the sensor cluster. Since the vehicle camera for driver assistance applications is, as a general rule, integrated into the vehicle, the motion of the vehicle camera corresponds to the motion of the vehicle. A stereo camera represents a preferred embodiment of a vehicle camera. 
     The fundamental methodology of the visual odometry originates from mobile robotics. Significantly lower translation speeds are generally reached in this field than in the automotive sector. Due to the significantly higher speeds in the vehicle, significant errors occur in high-speed scenarios (country road, freeway) with the previous systems. A second aspect of the invention therefore relates to presenting a suitable method for high-speed scenarios. 
     Temporal and spatial image correspondences, which contain errors (e.g. moving objects, errors due to aliasing . . . ), are required to estimate the motion. The correspondence of a feature in a first image with the same feature in a second image is called image correspondence. A feature can, in particular, be an image detail (or respectively patch), an edge, a corner or an object. The first and the second image can be recorded by the same mono camera at different times or by two camera modules of a stereo camera at the same time. As a general rule, the respective image position (image coordinates) is of interest in the case of the correspondence of a feature in two images, whilst the time which has elapsed between the two recordings is also of interest in the case of images recorded consecutively in time. 
     Image correspondences can be judged by a so-called reprojection error. The reprojection error indicates the difference between the measured flow and the flow calculated from the motion hypothesis. If the reprojection error of a feature exceeds a boundary value, this feature is removed as an “outlier” from the final measurement, which is used to estimate the vehicle&#39;s egomotion. 
     Another aspect of the invention relates to the realization that a constant threshold for the reprojection error is not expedient for removing the outliers. 
     Another aspect of the invention is the realization that the reprojection error depends on the image coordinate of the observed correspondence relative to the image focal point as well as the depth of the observed measuring point. These dependencies can, in addition to a deterioration in the estimate, result in a complete breakdown of the estimate, since the influence due to the coordinates/depth results in a higher reprojection error than in the case of an erroneous correspondence with a “more favorable” coordinate/depth. 
     Another aspect of the invention is based on a careful analysis of the causes (see section I.), which then makes it possible to introduce anew criterion, which makes the estimate significantly more robust and increases the accuracy during a pure translation. 
     According to another aspect of the invention, this criterion can furthermore also be used in scenarios with rotation if knowledge of this is available. To this end, the flow induced by the rotation is simply compensated in the measured flow. However, this is not necessary in overland and freeway scenarios because of the low rotation in the cases investigated. 
     Consequently, an entire camera-based concept is created, which carries out the self-localization of vehicles with a high degree of accuracy and leads to better results in a significantly broader speed range. 
     A method according to the invention for evaluating image data of a vehicle camera comprises the following steps: 
     a) Recording of a first image by means of the vehicle camera, 
     b) Recording of a subsequent second image by means of the vehicle camera, 
     c) Extraction of multiple correspondent features in the first and the second image, 
     d) Assumption of a motion of the vehicle camera (motion hypothesis) between the recording of the first and the second image, 
     e) Determination of a reprojection error of an extracted feature, wherein the reprojection error indicates the difference between the flow measured from the feature extracted in the first and the second image and the flow calculated from the motion hypothesis, 
     f) Establishment of outliers, wherein an extracted feature is established as an outlier if the reprojection error of this feature reaches or exceeds a threshold, 
     g) wherein the threshold is not constant within an iteration step. 
     Here, at least steps b) to g) are part of an iteration step or, in other words, an iteration step comprises steps b) to g). In step c), the extraction of a (complete) set of correspondent features is, in particular, envisaged and in step e) the associated extraction error is then preferably determined for each extracted feature. Steps f) and g) are, in particular, to be understood to mean that the threshold is not the same for all of the features extracted in an iteration step. In the first iteration step, a translation with a predefined speed can, for example, be used as an assumption of the motion of the vehicle camera or respectively “initial motion hypothesis”. Alternatively, data from other sensors in the vehicle can supply an initial motion hypothesis. 
     The method according to the invention offers an improved possibility of detecting outliers, which is known for schemes such as RANSAC and MASOR. The new way of creating a threshold can be quickly applied in the known methods, which establish the reprojection error, and it increases the robustness of the outlier detection. 
     According to one advantageous configuration of the invention, a threshold for an extracted feature is used in step f), which depends on a parameter (i.e. a quantifiable property) of the extracted feature. 
     The threshold preferably depends on the flow measured from the feature extracted in the first and the second image (i.e. in practice the displacement of the same feature in the first image compared with the second image). A case distinction can e.g. be advantageously made, wherein a constant threshold is applied to the reprojection error for very small flows which are below a minimum value, in order to guarantee the numerical robustness, whereas a different, in particular varying, threshold is applied in the case of larger flows. 
     The threshold is particularly preferably proportional to the measured flow of the extracted feature. That is the same as if the reprojection error usually used is normalized with the measured optical flow and a constant threshold is used. This change clearly allows correspondences with a large optical flow a larger error as well, which forces the use of close correspondences more strongly than the reprojection error. Mathematically, both the dependency of the error on the coordinate relative to the image focal point is removed, and the dependency of the depth in the relevant region is virtually completely compensated. This prevents the breakdown into more distant correspondences, which significantly lowers or respectively even prevents the resolution of the translation estimate. Advantageously, a rotational component of the motion can be compensated, in that the threshold is proportional to the flow following the compensation of the rotational component. 
     The threshold advantageously comprises a constant factor and a variable proportion, wherein the constant factor can be pre-defined, taking account of physical properties of the vehicle camera. 
     According to one preferred embodiment, the motion of the vehicle camera is reconstructed in an additional step h), taking account of the features, which are not removed as outliers in the current iteration step. In other words, only the inliers are used to reconstruct motion. Here, reconstructing can in particular mean that a new motion hypothesis is then calculated or the previous motion hypothesis is refined. 
     The motion of the vehicle camera reconstructed in step h) is preferably adopted for the following iteration step as a motion hypothesis for step d). 
     During a following iteration step the previous second image can be used as the new first image. Alternatively, step a) can also be carried out in the following iteration step. 
     The threshold or respectively the constant factor thereof is preferably lowered as the iteration progresses until such time as the desired accuracy is achieved. 
     The method can advantageously be used at high translational motion speeds of the vehicle camera or respectively of the vehicle, wherein high speeds are preferably larger than or equal to 30 km/h, in particular larger than or equal to 50 km/h. 
     The reconstructed vehicle camera motion is preferably enlisted for self-localization. 
     The reconstructed vehicle camera motion is advantageously used to determine the side slip angle of the vehicle in which the vehicle camera is arranged. 
     According to one preferred embodiment, the reconstructed vehicle camera motion is used to determine the wheel diameter of the vehicle in which the vehicle camera is arranged. Here, the distance covered results from the reconstruction, and the wheel diameter can be approximately determined by means of a wheel speed value (e.g. from the vehicle sensor cluster). 
     The vehicle camera can be advantageously calibrated by a comparison between the reconstructed vehicle camera motion and the motion of the vehicle established from other sensors. 
     The vehicle camera is preferably a stereo camera. Alternatively, the vehicle camera can also be a different camera, provided that the data thereof allow the reconstruction of a depth image, in particular TOF (time of flight) cameras such as e.g. HiRes flash lidar, but a 3D scene reconstruction is also possible from mono image data—at least approximately, by means of the assumption of a level subsurface, see e.g. Kitt et al. Monocular Visual Odometry using a Planar Road Model to Solve Scale Ambiguity, in Proc. of the European Conference on Mobile Robots, Orebro, Sweden, September 2011. 
     The invention furthermore relates to a device which is configured to carry out a method according to the invention, for example in the form of a control device for the vehicle camera. 
     By means of the application of a method according to the invention, we were able to exceed the quality of the results of the competing camera-based algorithms as of a speed of approximately 65 km/h. Even the results of laser scanners succumb to our method. The improvement in the accuracy achieved for very low additional computational cost results in improved products for vehicle localization and estimating the driving state. 
    
    
     
       BRIEF DESCRIPTION OF THE DRAWINGS 
       Embodiment examples of the invention are explained in greater detail below and partially illustrated with reference to Figures, wherein: 
         FIG. 1  schematically shows how a feature in the real space ξ i  is imaged by a moving vehicle camera, 
         FIG. 2  shows the dependency of the reprojection error on the error of the speed estimate for different error-free feature depths, 
         FIG. 3  shows a comparison between the reprojection error and the ratio of the reprojection error to the optical flow for features having different depths, 
         FIG. 4  shows a comparison between the measuring results of an embodiment example of the invention and the two best camera-based algorithms in the KITTI benchmark (status as at 11 Jan. 2016), and 
         FIG. 5  shows a comparison between the measuring results of the three best stereo camera-based and laser-scanner based systems in the KITTI benchmark with an embodiment example of the invention. 
     
    
    
     DETAILED DESCRIPTION OF EXAMPLE EMBODIMENTS OF THE INVENTION 
     An outlier can be determined as follows. 
     The main problem of visual odometry is distinguishing the suitable features—the inliers—from the non-suitable features—the outliers—from all of the extracted features. This is usually done by selecting only features with reliable measurements and defining some criterion to evaluate how well these measurements fit to some hypothesis of an estimated motion. 
     The reliability of measurements has two aspects: 
     First, the reliability of the determination of feature correspondences for calculating the disparity. Unambiguousness of a correspondence of features in the right and the left image. 
     Second, the accuracy of these correspondences is limited by the resolution of the images. So even if the correspondence of a feature in the right and in the left image is unambiguous the smaller the distance of the correspondent features, i.e. their disparity, the less accurate the pose change can be determined. Ultimately, the resolution of the reconstructed depth reduces quadratically with the distance. 
     Consequently, near features with highly confident correspondence values could be selected as suitable features. 
     The pose change of the camera from time t−1 to t is given by the translation vector T and the rotation matrix R (of the camera motion). 
     Since the camera for driver assistance applications is, as a general rule, integrated into the vehicle, the motion of the camera corresponds to the motion of the vehicle. 
     I. How to Define an Outlier? 
     The starting point is the classical least squares estimator: 
     
       
         
           
             
               
                 
                   
                     
                       ( 
                       
                         
                           R 
                           ^ 
                         
                         , 
                         
                           T 
                           ^ 
                         
                       
                       ) 
                     
                     = 
                     
                       arg 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         
                           min 
                           
                             R 
                             , 
                             T 
                           
                         
                         ⁢ 
                         
                           
                             ∑ 
                             
                               i 
                               = 
                               1 
                             
                             N 
                           
                           ⁢ 
                           
                             
                               ( 
                               
                                 ɛ 
                                 i 
                                 t 
                               
                               ) 
                             
                             2 
                           
                         
                       
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   1 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       ɛ 
                       i 
                       t 
                     
                     = 
                     
                       
                          
                         
                           
                             x 
                             i 
                             
                               t 
                               - 
                               1 
                             
                           
                           - 
                           
                             π 
                             ⁡ 
                             
                               ( 
                               
                                 
                                   R 
                                   ⁢ 
                                   
                                       
                                   
                                   ⁢ 
                                   
                                     λ 
                                     i 
                                     t 
                                   
                                   ⁢ 
                                   
                                     x 
                                     i 
                                     t 
                                   
                                 
                                 + 
                                 T 
                               
                               ) 
                             
                           
                         
                          
                       
                       2 
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   2 
                   ) 
                 
               
             
           
         
       
     
     where the norms ε i   t  are called the reprojection errors for each feature indexed by i at time t within the feature set    t ={f i   t } i=1   N . Here, {x i   t-1 ,x i   t }∈   3  is the correspondent pair of coordinates denoted in homogeneous normalized image coordinates x i   t =[x i   t , y i   t , 1] T  for all 3D points p i  ∈   3  with camera coordinates X i   t =[X i   t , Y i   t , Z i   t ]T=λ i   t x i   t . The pose change of the camera from time t−1 to t is given by the 3D translation vector T=[t x ,t y ,t z ] T ∈   3  and the rotation matrix R∈SO(3), wherein the space of the rotation matrices is indicated by SO(3):={R∈R 3×3 |R T R=I, det(R)=1}, and π denotes the standard planar projection [X,Y,Z] T    [X/Z,Y/Z,1] T . 
       FIG. 1  schematically illustrates these relationships. A feature ξ i  in the real space is imaged at time t−1 by a vehicle camera, wherein the image coordinate x i   t-1  is produced for this feature. 
     In  FIG. 1 , the vehicle camera is a stereo camera (shown as two adjacent parallelograms which each symbolize one camera module), which in each case generates a pair of images consisting of a left and a right image at a recording time. This represents one possible way of establishing the depth of the feature by means of triangulation or respectively by determining the disparity. The disparity is given by the displacement of the correspondent feature x i   t-1  in the left and the right image. 
     A method according to the invention is, however, not restricted to the stereo camera as the vehicle camera, since other cameras also make it possible to reconstruct a depth image, in particular TOF cameras such as e.g. HiRes flash lidar, but a 3D scene reconstruction is also possible, at least approximately, from mono image data. 
     The projection, for example, of the spatial point ξ i  relative to the left camera onto the left image x i   t-1  is indicated in  FIG. 1  by the dashed line. 
     The camera subsequently changes its pose by means of translation and rotation (camera motion). At time t, the camera images the feature at the image coordinate x i   t . The camera can now be moved virtually in the space by means of the hypothesis of the motion of the camera and, taking account of the imaging property of the camera, it can be calculated where the feature ξ i  is imaged in the next image (projection). The deviation of this position from the actual image coordinate x i   t  (continuous line) corresponds to the reprojection error ε i   t . 
     Following the classical visual odometry procedure, it is assumed that for each point p i  the depth λ i   t ∈  is measured by some stereo vision algorithm, the image coordinates x i   t-1  are extracted by some feature detector and the correspondent image coordinates in the next frame x i   t  are measured by some optical flow algorithm. 
     To find the optimal estimate of the pose change ({circumflex over (R)},{circumflex over (T)}) by means of minimizing the objective in accordance with equation (1) with an iterative gradient descent method (e.g. Gauss-Newton or Levenberg-Marquardt method), some initial estimate of the pose change has to be given. 
     Now, we are faced with the main problem of visual odometry: Given the set of all of the extracted features, we need to find suitable features—the inliers—and reject all other features from the set—the outliers. This is usually done by selecting only features with reliable measurements {λ i   t , x i   t-1 , x i   t } and defining some criterion to evaluate how well these measurements fit to some hypothesis of the estimated pose change ({tilde over (R)},{tilde over (T)}). 
     The reliability of a measurement has two aspects. First, since, in stereo vision, depth λ i   t =b/d i   t  is reconstructed from disparity d i   t  using a stereo camera with a fixed known baseline b and both the disparity d i   t  and the pairs {x i   t-1 ,x i   t } are based on a correspondence search, only unambiguous correspondences, e.g. not facing the aperture problem, should be taken into account. Second, the accuracy of these correspondences is limited by the resolution of the images. So even if the correspondences are unambiguous the smaller their distances ∥x i   t −x i   t-1 ∥ and d i   t , the less accurately the pose change can be estimated. This is because the ratios ∥x i   t −x i   t-1 ∥/Δp and d i   t /Δp between distances ∥x i   t −x i   t-1 ∥, d i   t  and the limited image resolution Δp become smaller with smaller image-distances and thus the signal-to-resolution ratio decreases. Especially for the accuracy of the reconstructed depth λ i   t =b/d i   t ; this is crucial because the depth ∂λ i   t ∞∂d i   t (λ i   t ) 2  reduces quadratically with distance. 
     Considering these facts, it seems to be easy to figure out good features. Near features with large optical flow that are based on highly confident correspondence estimates should be chosen. Additionally, each feature has to fulfill the epipolar constraint for one optimal estimate ({circumflex over (R)},{circumflex over (T)}), thus the features only have to be projections of static points in the scene. 
     Since we cannot guarantee that the measurements are all confident and we do not have the optimal pose change at hand, we need to find a good hypothesis ({tilde over (R)},{tilde over (T)}) and a proper criterion to keep as many suitable features as possible. 
     This is a chicken-and-egg problem. To resolve it, the reprojection error in accordance with equation (2) is investigated in greater detail in two ways: On the one hand, it should be used as the criterion to remove outliers and, on the other hand, the reproduction error should improve the hypothesis of the pose change given the inliers. In order to combine both subproblems in an alternating scheme, it is figured out how to use the reprojection error for both subtasks such that as many inliers as possible can be kept, which also leads to a more accurate estimate of the pose change. 
     To find a good criterion for the outlier removal, we examine the dependency of the reprojection error on the values of the measurements assuming error-free measurements {{circumflex over (λ)},{circumflex over (x)} i   t-1 ,{circumflex over (x)} i   t } and an imprecise pose change hypothesis ({tilde over (R)},{tilde over (T)}). For improving the pose change hypothesis, we assume error-prone measurements {{circumflex over (λ)},{circumflex over (x)} i   t-1 ,{circumflex over (x)} i   t } and try to maximize the improvement of the iterative pose change estimation ({circumflex over (R)},{circumflex over (T)}) by using the knowledge about the dependency of the reprojection error again. 
     As is known from Shi and Tomasi, Good features to track, in IEEE Conference on Computer Vision and Pattern Recognition, Proceedings, 1994, high translation estimate errors occur at large longitudinal pose changes along the optical axis. The translation estimates become especially poor for long distance features, see Persson et al., Robust stereo visual odometry from monocular techniques, in IEEE Intelligent Vehicles Symposium, 2015. To obtain a first impression on the consequences for this sensitivity in such driving scenarios  FIG. 2  shows the dependency of the reprojection error ε i   t  or respectively RE in pixels [px] for an error-prone translation estimate {tilde over (t)} z ={circumflex over (t)} z +Δt z  in the z-direction with {circumflex over (t)} z =100 km/h applied via the relative error of the speed estimate Δt z /{circumflex over (t)} z  at an interval of [−10%, 10%] for three different (error-free) feature depths {circumflex over (λ)} i   t  of 10, 50 and 100 meters.  FIG. 2  clearly shows that the reprojection error (RE) linearly increases with increasing translation error but the sensitivity of the reprojection error (the slope of the lines) decreases with increasing distance (or respectively depth) of the features. 
     With a constant threshold for removing outliers, close features (with low depth) would be very quickly rejected. However, this produces a poorer estimate of the translation overall, since in the case of more distant features which are not rejected as quickly as outliers, the signal-to-noise ratio worsens and these features cannot be usefully exploited any more. 
     A. The Reprojection Error in High-Speed Scenarios 
     For error-free measurements {{circumflex over (λ)} i   t ,{circumflex over (x)} i   t-1 ,{circumflex over (x)} i   t } and the optimal motion estimate ({circumflex over (R)},{circumflex over (T)}), the reprojection error (2) becomes zero because the following applies:
 
 {circumflex over (x)}   i   t-1 =π( {circumflex over (R)}{circumflex over (λ)}   i   t   {circumflex over (x)}   i   t   +{circumflex over (T)} ),∀ i,t.   (3)
 
     In order to find a proper threshold for removing outliers, some motion error range (ΔR,ΔT) has to be defined in addition to the optimal motion estimate ({circumflex over (R)},{circumflex over (T)}) ({tilde over (R)},{tilde over (T)})=({circumflex over (R)},{circumflex over (T)})+(ΔR,ΔT) to find the reprojection error range given the error-prone motion and error-free measurements. This results in the following dependency of the reprojection error
 
Δε i   t   =∥{circumflex over (x)}   i   t-1 −π( {tilde over (R)}{circumflex over (λ)}   i   t   +{tilde over (T)} )∥ 2   ,∀i,t.   (4)
 
     Now, we can assume very small rotations for high-speed scenarios. 
     1. High-Speed Approximation
 
 R≈I.   (5)
 
     thus the rotation matrix approximately equals the identity matrix and longitudinal movements are much larger than horizontal movements, thus the components of the translation are approximately equal to zero: 
     2. High-Speed Approximation
 
 t   x   ,t   y ≈0.  (6)
 
     Applying approximations (5) and (6) produces an approximation of the sensitivity of the reprojection error (4) at high speeds for an error-prone motion hypothesis, which reads 
     
       
         
           
             
               
                 
                   
                     Δ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       ɛ 
                       i 
                       t 
                     
                   
                   ≈ 
                     
                   ⁢ 
                   
                     
                        
                       
                         ( 
                         
                           
                             
                               
                                 
                                   
                                     
                                       
                                         λ 
                                         ^ 
                                       
                                       i 
                                       t 
                                     
                                     ⁢ 
                                     
                                       
                                         x 
                                         ^ 
                                       
                                       i 
                                       t 
                                     
                                   
                                   
                                     
                                       
                                         λ 
                                         ^ 
                                       
                                       i 
                                       t 
                                     
                                     + 
                                     
                                       
                                         t 
                                         ^ 
                                       
                                       z 
                                     
                                   
                                 
                                 - 
                                 
                                   
                                     
                                       
                                         λ 
                                         ^ 
                                       
                                       i 
                                       t 
                                     
                                     ⁢ 
                                     
                                       
                                         x 
                                         ^ 
                                       
                                       i 
                                       t 
                                     
                                   
                                   
                                     
                                       
                                         λ 
                                         ^ 
                                       
                                       i 
                                       t 
                                     
                                     + 
                                     
                                       
                                         t 
                                         ^ 
                                       
                                       z 
                                     
                                     + 
                                     
                                       Δ 
                                       ⁢ 
                                       
                                           
                                       
                                       ⁢ 
                                       
                                         t 
                                         z 
                                       
                                     
                                   
                                 
                               
                             
                           
                           
                             
                               
                                 
                                   
                                     
                                       
                                         λ 
                                         ^ 
                                       
                                       i 
                                       t 
                                     
                                     ⁢ 
                                     
                                       
                                         y 
                                         ^ 
                                       
                                       i 
                                       t 
                                     
                                   
                                   
                                     
                                       
                                         λ 
                                         ^ 
                                       
                                       i 
                                       t 
                                     
                                     + 
                                     
                                       
                                         t 
                                         ^ 
                                       
                                       z 
                                     
                                   
                                 
                                 - 
                                 
                                   
                                     
                                       
                                         λ 
                                         ^ 
                                       
                                       i 
                                       t 
                                     
                                     ⁢ 
                                     
                                       
                                         y 
                                         ^ 
                                       
                                       i 
                                       t 
                                     
                                   
                                   
                                     
                                       
                                         λ 
                                         ^ 
                                       
                                       i 
                                       t 
                                     
                                     + 
                                     
                                       
                                         t 
                                         ^ 
                                       
                                       z 
                                     
                                     + 
                                     
                                       Δ 
                                       ⁢ 
                                       
                                           
                                       
                                       ⁢ 
                                       
                                         t 
                                         z 
                                       
                                     
                                   
                                 
                               
                             
                           
                         
                         ) 
                       
                        
                     
                     2 
                   
                 
               
               
                 
                                                                                               
                   ⁢ 
                   
                     ( 
                     7 
                     ) 
                   
                 
               
             
             
               
                 
                   = 
                     
                   ⁢ 
                   
                     
                        
                       
                         
                           
                             
                               λ 
                               ^ 
                             
                             i 
                             t 
                           
                           ⁢ 
                           Δ 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             t 
                             z 
                           
                         
                         
                           
                             ( 
                             
                               
                                 
                                   λ 
                                   ^ 
                                 
                                 i 
                                 t 
                               
                               + 
                               
                                 
                                   t 
                                   ^ 
                                 
                                 z 
                               
                             
                             ) 
                           
                           ⁢ 
                           
                             ( 
                             
                               
                                 
                                   λ 
                                   ^ 
                                 
                                 i 
                                 t 
                               
                               + 
                               
                                 
                                   t 
                                   ^ 
                                 
                                 z 
                               
                               + 
                               
                                 Δ 
                                 ⁢ 
                                 
                                     
                                 
                                 ⁢ 
                                 
                                   t 
                                   z 
                                 
                               
                             
                             ) 
                           
                         
                       
                        
                     
                     ⁢ 
                     
                       
                         
                            
                           
                             
                               x 
                               ^ 
                             
                             i 
                             t 
                           
                            
                         
                         2 
                       
                       . 
                     
                   
                 
               
               
                 
                     
                   ⁢ 
                   
                     ( 
                     8 
                     ) 
                   
                 
               
             
           
         
       
     
     The sensitivity of the reprojection error is scaled by the absolute value of the image coordinate ∥{circumflex over (x)} i   t ∥ 2  and damped by the feature&#39;s depth {circumflex over (λ)} i   t . This means that an incorrect motion hypothesis {tilde over (t)} z ={tilde over (t)} z +Δt z  with a fixed error range Δt z  produces a variant sensitivity Δε i   t  dependent on the feature&#39;s position. 
     Thus, methods that are based on outlier removal by means of a constant threshold for the reprojection error remove close features, although the measurements are error-free (or error-prone to the same extent as more distant features). 
     This leads to the breakdown of outlier removal in high-speed scenarios for methods based on a constant threshold for the reprojection error. 
     As close features with high absolute values of their correspondences are lost during the outlier removal process, the sensitivity of the reprojection error against forward translation gets lost, as can be seen in  FIG. 2 . 
     In turn, this results in worse estimates of the translation because the signal-to-resolution ratio is becoming smaller and cannot be exploited anymore. To conclude, a reasonable threshold to judge the quality of a feature cannot be a constant value but must incorporate the depth and the length of the image coordinate in order to make a meaningful statement on the quality of a feature. 
     B. Almost Invariant Criterion for Outlier Removal 
     To reduce the dependency of the reprojection error on the feature position, we can either use a position adaptive threshold for outlier removal or normalize the reprojection error for coordinate ∥{circumflex over (x)} i   t ∥ 2  scaling and depth {circumflex over (λ)} i   t  damping. Since the resolution of the measured depth values decreases with distance, and depth measurements are in general error-prone, we do not want to incorporate them to compensate the depth damping of the reprojection error. Instead, we use the dependency of the absolute value of the optical flow on the depth and use the optical flow measurements to normalize the reprojection error as follows: The error-free absolute value of a feature&#39;s optical flow induced by an error-free straight forward motion {circumflex over (t)} z , again assuming (5) and (6) reads: 
     
       
         
           
             
               
                 
                   
                     
                        
                       
                         
                           
                             x 
                             ^ 
                           
                           i 
                           
                             t 
                             - 
                             1 
                           
                         
                         - 
                         
                           
                             x 
                             ^ 
                           
                           i 
                           t 
                         
                       
                        
                     
                     2 
                   
                   = 
                     
                   ⁢ 
                   
                     
                        
                       
                         
                           π 
                           ⁡ 
                           
                             ( 
                             
                               
                                 
                                   R 
                                   ^ 
                                 
                                 ⁢ 
                                 
                                   
                                     λ 
                                     ^ 
                                   
                                   i 
                                   t 
                                 
                                 ⁢ 
                                 
                                   
                                     x 
                                     ^ 
                                   
                                   i 
                                   t 
                                 
                               
                               + 
                               
                                 T 
                                 ^ 
                               
                             
                             ) 
                           
                         
                         - 
                         
                           
                             x 
                             ^ 
                           
                           i 
                           t 
                         
                       
                        
                     
                     2 
                   
                 
               
               
                 
                                                                                              
                   ⁢ 
                   
                     ( 
                     9 
                     ) 
                   
                 
               
             
             
               
                 
                   ≈ 
                     
                   ⁢ 
                   
                     
                        
                       
                         
                           
                             t 
                             ^ 
                           
                           z 
                         
                         
                           
                             
                               λ 
                               ^ 
                             
                             i 
                             t 
                           
                           + 
                           
                             
                               t 
                               ^ 
                             
                             z 
                           
                         
                       
                        
                     
                     ⁢ 
                     
                       
                         
                            
                           
                             
                               x 
                               ^ 
                             
                             i 
                             t 
                           
                            
                         
                         2 
                       
                       . 
                     
                   
                 
               
               
                 
                     
                   ⁢ 
                   
                     ( 
                     10 
                     ) 
                   
                 
               
             
           
         
       
     
     Using the absolute value of the current optical flow as a normalization of the sensitivity of the reprojection error, we obtain 
     
       
         
           
             
               
                 
                   
                     Δ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       
                         ɛ 
                         _ 
                       
                       i 
                       t 
                     
                   
                   = 
                   
                     
                       
                         Δ 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           ɛ 
                           i 
                           t 
                         
                       
                       
                         
                            
                           
                             
                               
                                 x 
                                 ^ 
                               
                               i 
                               
                                 t 
                                 - 
                                 1 
                               
                             
                             - 
                             
                               
                                 x 
                                 ^ 
                               
                               i 
                               t 
                             
                           
                            
                         
                         2 
                       
                     
                     ≈ 
                     
                        
                       
                         
                           
                             
                               λ 
                               ^ 
                             
                             i 
                             t 
                           
                           ⁢ 
                           Δ 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             t 
                             z 
                           
                         
                         
                           
                             
                               t 
                               ^ 
                             
                             z 
                           
                           ⁡ 
                           
                             ( 
                             
                               
                                 
                                   λ 
                                   ^ 
                                 
                                 i 
                                 t 
                               
                               + 
                               
                                 
                                   t 
                                   ^ 
                                 
                                 z 
                               
                               + 
                               
                                 Δ 
                                 ⁢ 
                                 
                                     
                                 
                                 ⁢ 
                                 
                                   t 
                                   z 
                                 
                               
                             
                             ) 
                           
                         
                       
                        
                     
                     ≈ 
                     
                       
                          
                         
                           
                             Δ 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               t 
                               z 
                             
                           
                           
                             
                               t 
                               ^ 
                             
                             z 
                           
                         
                          
                       
                       . 
                     
                   
                 
               
               
                 
                   ( 
                   11 
                   ) 
                 
               
             
           
         
       
     
     Here, the second approximation assumes the depth being much larger than the longitudinal motion {circumflex over (λ)} i   t &gt;&gt;{circumflex over (t)} z +Δt z . 
     For this reason, the normalized reprojection error is not scaled by the absolute value of the image coordinate anymore and is almost independent of the distance of distant features. 
     We therefore use the following case distinction, using a threshold ε thresh  on the normalized reprojection error to mark each feature as a member of the current feature set: 
     
       
         
           
             
               
                 
                   
                     f 
                     i 
                     t 
                   
                   ⁢ 
                   
                     { 
                     
                       
                         
                           
                             
                               
                                 ∈ 
                               
                               , 
                             
                           
                           
                             
                               
                                 
                                   if 
                                   ⁢ 
                                   
                                       
                                   
                                   ⁢ 
                                   
                                     
                                       ɛ 
                                       _ 
                                     
                                     i 
                                     t 
                                   
                                 
                                 = 
                                 
                                   
                                     
                                       ɛ 
                                       i 
                                       t 
                                     
                                     
                                       
                                          
                                         
                                           
                                             x 
                                             i 
                                             
                                               t 
                                               - 
                                               1 
                                             
                                           
                                           - 
                                           
                                             x 
                                             i 
                                             t 
                                           
                                         
                                          
                                       
                                       2 
                                     
                                   
                                   &lt; 
                                   
                                     ɛ 
                                     thresh 
                                   
                                 
                               
                               , 
                             
                           
                         
                         
                           
                             
                               
                                 ∉ 
                               
                               , 
                             
                           
                           
                             else 
                           
                         
                       
                       . 
                     
                   
                 
               
               
                 
                   ( 
                   12 
                   ) 
                 
               
             
           
         
       
     
     This criterion (as part of an outlier removal scheme as explained below) turns out to be very suitable for outlier removal, especially in high-speed scenarios, because it is almost independent of the features&#39; 3D position. 
     A rotational component of the motion hypothesis can be compensated in that the reprojection error is normalized with the flow following the compensation of the rotational component. In the case of a reprojection error normalized in such a way 
                   ɛ   n   t     ⁡     (         R   ^     t     ,       T   ^     t       )                  x   n     t   -   1       -     π   ⁡     (         R   ^     t     ⁢     x   n   t       )              2       ,         
an external rotation estimate {circumflex over (R)} t  is used to transform the measured image points on the right in the denominator such that the rotation of the vehicle is offset in the correspondence. As a result, only the flow through the translation remains. That means that we transform each scenario to the situation “free of rotation”, which until now was the assumption for the applicability of the measure of the error in accordance with equation (12).
 
     We use the same {circumflex over (R)} t  constantly for calculating the reprojection error (we therefore only optimize according to {circumflex over (T)} t ). 
     Therefore, the described outlier detection can be used not only for high-speed scenarios (assuming freedom of rotation). This significantly extends the scenarios in which the method leads to improvements. 
     Here, the indices t and t−1 represent the time of the estimate, while n represents the observed feature. 
       FIG. 3  shows a comparison between the reprojection error ε i   t  or respectively RE in pixels (top) and the normalized reprojection error  ε   i   t  or respectively NRE (bottom) for a selection of different features, wherein each tenth feature f i   t  has an error of the estimated depth λ i   t  in meters of 10% (center) for a simulated forward motion t z  of 100 km/h. The depths of the features start at 3 m for the first feature f 1   t  and extend up to 100 m for the 50 th  feature f 50   t . The reprojection error ε i   t  ( FIG. 3 , top) does not allow an expedient separation between outliers and inliers, because it scales with the absolute value of the coordinate of the features. By contrast, the normalized reprojection error  ε   i   t  ( FIG. 3 , bottom) makes possible a clear distinction of the outliers, as the normalized reprojection error of these features is larger by approximately a factor of two than the error-free features. 
     C. Hypothesis Refinement on the Inlier Set 
     One question has not yet been addressed: Since the normalized reprojection error (11) improves the outlier removal, is it also suitable for obtaining a better hypothesis for the least squares problem formulated in equation (1) ? Now, assuming error-prone measurements {{tilde over (λ)} i   t ,{tilde over (x)} i   t-1 ,{tilde over (x)} i   t } and trying to minimize equation (1) to obtain a better pose change estimate ({circumflex over (R)},{circumflex over (T)}) the features with measurements that possess a high resolution should contribute more to the estimate than measurements with low resolution. This can be realized with an extension of (1) to a weighted least squares problem that realizes a decrease of the weights for distant features and an increase of the weights for larger optical flow amplitudes. Looking at the approximation of the reprojection error in equation (8) for high longitudinal speeds, this weighting is intrinsically done by the reprojection error itself, whereas the normalized reprojection error would weight the features more or less equally. Thus, for refinement of the motion hypothesis based on the current set of inliers, the (un-normalized) reprojection error is already most suitable. 
     II. Incremental Alternating Outlier Removal and Pose Refinement Scheme 
     To realize an iterative optimization scheme that carefully alternates between incremental outlier rejection and pose change refinement, we need a suitable set of a reasonable number of features to start with. A suitable feature has unambiguous temporal as well as stereoscopic correspondence measurements to obtain as many reliable optical flow and depth estimates as possible. 
     A. Per Frame Initialization 
     Our initial feature set for every stereo-frame pair is created as follows applying only standard functions (e.g. of the OpenCV library): 
     We start with the feature selection using the Shi and Tomasi method    0   t ={x i   t-1 ,x i   t ,λ i   t } j=1   N     0   . For each feature the disparity at time t−1 is calculated using SAD-based block matching. For optical flow initialization, we triangulate each feature&#39;s position in three-dimensional space at time t−1 and reproject the features to the current frame at time t using a constant turn rate and velocity model based on the last estimated pose change (which is a variant of a motion model with predicted tracking by matching proposed in Persson et al. (see above)). 
     After that the optical flow for the left and the right image between time t−1 and t is refined with the Lucas-Kanade method in accordance with Bouguet, Pyramidal implementation of the affine lucas kanade feature tracker description of the algorithm, 2001, Intel Corp. 5. The final feature set with a starting number for initialization is reached via a left-right consistency check at time t for all remaining optical flow estimates (which is a variant of circular matching proposed in Geiger et al., Stereoscan: Dense 3d reconstruction in real-time, in Intelligent Vehicles Symposium, 2011). 
     B. Alternating Iteration Based on MAximum Subset of Outlier Removal (MASOR) 
     We iterate over p alternating between a) pose refinement keeping the current inlier set fixed and b) outlier removal keeping the current pose ({circumflex over (R)} p , {circumflex over (T)} p ): 
     a) Pose refinement starts with    0   t  at first iteration p=0. The pose estimate is initialized with the estimate of the last frame {circumflex over (R)} 0   t ={circumflex over (R)} t-1  and {circumflex over (T)} 0   t ={circumflex over (T)} t-1 . In the following we omit the time index t for simplicity: 
     
       
         
           
             
               
                 
                   
                     
                       ( 
                       
                         
                           
                             R 
                             ^ 
                           
                           p 
                         
                         , 
                         
                           
                             T 
                             ^ 
                           
                           p 
                         
                       
                       ) 
                     
                     = 
                     
                       arg 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         
                           min 
                           
                             R 
                             , 
                             T 
                           
                         
                         ⁢ 
                         
                           
                             ∑ 
                             i 
                           
                           ⁢ 
                           
                             
                               ( 
                               
                                 ɛ 
                                 i 
                                 t 
                               
                               ) 
                             
                             2 
                           
                         
                       
                     
                   
                   , 
                   
                     ∀ 
                     
                       
                         f 
                         i 
                         t 
                       
                       ∈ 
                     
                   
                 
               
               
                 
                   ( 
                   13 
                   ) 
                 
               
             
           
         
       
     
     b) For outlier removal, we apply our combined criterion for case distinction, which we call Robust Outlier Criterion for Camera-based Odometry (ROCC): 
     
       
         
           
             
               
                 
                   
                     f 
                     i 
                     t 
                   
                   ⁢ 
                   
                     { 
                     
                       
                         
                           
                             
                               
                                 ∈ 
                               
                               , 
                             
                           
                           
                             
                               
                                 if 
                                 ⁢ 
                                 
                                     
                                 
                                 ⁢ 
                                 
                                   
                                     
                                       ɛ 
                                       _ 
                                     
                                     i 
                                     t 
                                   
                                   ⁡ 
                                   
                                     ( 
                                     
                                       
                                         
                                           R 
                                           ^ 
                                         
                                         p 
                                       
                                       , 
                                       
                                         
                                           T 
                                           ^ 
                                         
                                         p 
                                       
                                     
                                     ) 
                                   
                                 
                               
                               &lt; 
                               
                                 
                                   ɛ 
                                   _ 
                                 
                                 p 
                                 thresh 
                               
                             
                           
                         
                         
                           
                             
                                 
                             
                           
                           
                             
                               
                                 
                                   or 
                                   ⁢ 
                                   
                                       
                                   
                                   ⁢ 
                                   
                                     
                                       ɛ 
                                       i 
                                       t 
                                     
                                     ⁡ 
                                     
                                       ( 
                                       
                                         
                                           
                                             R 
                                             ^ 
                                           
                                           p 
                                         
                                         , 
                                         
                                           
                                             T 
                                             ^ 
                                           
                                           p 
                                         
                                       
                                       ) 
                                     
                                   
                                 
                                 &lt; 
                                 
                                   ɛ 
                                   p 
                                   thresh 
                                 
                               
                               , 
                             
                           
                         
                         
                           
                             
                               
                                 ∉ 
                               
                               , 
                             
                           
                           
                             else 
                           
                         
                       
                       . 
                     
                   
                 
               
               
                 
                   ( 
                   14 
                   ) 
                 
               
             
           
         
       
     
     The threshold  ε   p   thresh  therefore applies to the normalized reprojection error and is an example of a constant factor of the threshold, which can be predefined, taking account of physical properties of the vehicle camera. The first condition requires a sufficiently large optical flow, otherwise a normalization of the reprojection error is not possible (unstable). A small optical flow occurs, in particular, with more distant features. Alternatively, the second criterion intervenes, which then requires that the non-normalized reprojection error is below a low constant threshold ε p   thresh . Therefore, the feature is considered to be an inlier for as long as at least one criterion is met. 
     Here, we increase the thresholds  ε   p   thresh  and ε p   thresh  in a coarse to fine manner during the iterations. 
     When the number N p  of the feature sets does not change anymore, a minimum number of features N min  or a maximum number of iterations p max  is reached. We terminate our robust pose estimation scheme and perform one last optimization run with the remaining features. This run is initialized with the rotation and translation direction from OpenCV&#39;s standard least median of squares 2D-2D five-point method. 
     III. Evaluation and Results from Comparison Measurements 
     The evaluation is based on the KITTI benchmark (http://www.cvlibs.net/datasets/KITTI or respectively Geiger et al. Are we ready for Autonomous Driving? The KITTI Vision Benchmark Suite, Conference on Computer Vision and Pattern Recognition (CVPR), (2012), which provides city, overland and freeway scenarios. More detailed information regarding the methods indicated below as comparison measurements (SOFT, cv4xvl-sc, V-LOAM, LOAM and DEMO) can be found at http://www.cvlibs.net/datasets/kitti/eval_odometry.php or respectively under ‘Odometry’ on the KITTI website. 
     In order to evaluate the performance of an embodiment example of the invention in high-speed scenarios, we first compare it with known visual odometry algorithms without additional sensor data for different speeds v in km/h: Our method was the first-placed purely camera-based method with an overall error of 0.98% on 11 Jan. 2016. The SOFT method (“Multi Frame Tracking, SOFT” in  FIG. 4 ) achieved the second best overall result of the KITTI benchmark with a translation error TE of 1.03%. The authors use feature tracking on the basis of many images. 
     With an error of 1.09%, the algorithm cv4xvl-sc (“BA, cv4xvl-sc” in  FIG. 4 ) shows a slightly worse quality. Here, the authors apply Bundle Adjustment to improve the motion estimation. 
     By contrast with these two algorithms, we do not use the feature&#39;s history, but only rely on the frame-to-frame information of the features. As shown in  FIG. 4 , this leads to an almost constant additional error in comparison to the two known methods. Despite the loss of precision due to not using the feature&#39;s history, our new outlier removal (ROCC) scheme leads to a lowered error as of a speed of approximately 65 km/h. 
     This shows that the application of our new error measure enables even a comparatively imprecise system to outperform the conventional methods. An extension by means of Bundle Adjustment in order to further increase the quality of our method is therefore possible. 
     In order to underline the performance in high-speed scenarios, we compare our results to methods that additionally incorporate the information from a high-precision laser scanner: With an error of 0.88% and 1.14%, the LOAM and DEMO methods reach a very high reconstruction quality. The top-ranked V-LOAM method even achieves an overall-error of 0.75%. 
       FIG. 5  shows the results of a comparison between the measuring results of laser scanner systems and an embodiment example of the invention. It is true that the measuring accuracy of the camera-based system is inferior to that of the laser scanners overall. However, our careful outlier removal provides the best measuring results at speeds higher than 70 km/h. 
     At speeds of 70 km/h and more, our configuration consequently provides the best results in the KITTI benchmark of all the camera and laser scanner systems according to the prior art.