Patent Publication Number: US-8120644-B2

Title: Method and system for the dynamic calibration of stereovision cameras

Description:
FIELD 
     This invention relates generally to stereovision camera and imaging systems. More specifically the present invention relates to a method of analyzing stereo vision images to create an X/Y disparity map. 
     BACKGROUND 
     The development of technology related to automotive safety has shifted over the past decade from reactive systems, such as seatbelt, airbags, and occupant detection, to active systems, which include, for example, adaptive cruise control and collision avoidance. The effectiveness of the control algorithms used in these active systems may be enhanced through the use of monocular or stereovision camera systems. Compared to monocular vision systems, stereovision systems provide additional depth information, which can lead to more accurate visual detection and optical measurements. 
     In human vision, one key factor in analyzing 3-dimensional information is the existence of disparity between images received using the left eye and the right eye. Stereovision systems, which use two cameras to increase field of view, thereby, overlapping and improving range resolution, attempt to utilize the known principles associated with human vision. The range (R) resolution can be determined from the disparity between images according to the calculation described in Equation 1, where f x  is the focal length given in pixels of the camera lens in one direction (e.g., the X direction), B is the baseline distance between the two cameras, and d x  is the disparity in the specified X direction. In order to differentiate between objects at various depths, a disparity map must first be determined, which means that, for every pixel location in the left image, the corresponding pixel location in the right image must also be determined. The process for determining such a disparity map can become very computationally time consuming and expensive. 
     
       
         
           
             
               
                 
                   R 
                   = 
                   
                     
                       
                         f 
                         x 
                       
                       · 
                       B 
                     
                     
                       d 
                       x 
                     
                   
                 
               
               
                 
                   ( 
                   
                     Eq 
                     . 
                     
                         
                     
                     ⁢ 
                     1 
                   
                   ) 
                 
               
             
           
         
       
     
     Due to limited image resolution, the disparity calculation will not be completely accurate even in ideal cases where the local texture of the various objects being observed allow for relatively distinct separation between the viewed images. The error in the range (R) determined from Equation 1 is proportional to R 2 . This error will increase significantly if the camera lenses are not parallel or any lens distortion exists. Thus conventional stereovision systems must be calibrated, which can also be both time consuming and expensive. This type of calibration must be done by a trained technician under controlled lighting conditions using a specially designed image template. After calibration, any change in the camera&#39;s position, which can be caused by a number of uncontrollable factors, such as aging, thermal effects, or even accidental physical movement, will result in the reintroduction of significant error in the range (R) calculation. The end result is the need to recalibrate the vision system. Therefore, a need exists to develop efficient and inexpensive methods that can be used to perform automatic dynamic calibration using nothing more than the natural images or scenes observed through the camera. 
     SUMMARY 
     The present invention provides a method and system for analyzing and calibrating the stereovision image of a natural scene in order to produce an X/Y disparity map of the scene for use in the control of an adaptive or active system. One embodiment of a system, constructed in accordance with the teachings of the present invention, generally comprises a first sensor and a second sensor adapted to capture first and second image data of a scene and a processing unit capable of applying a predetermined algorithm to the first and second image data to determine an X/Y disparity map without pre-warping or finding the epipolar directions. The processing unit may also include a calibration module, which is in contact with the first and second sensors, and is capable of dynamically estimating the pitch, range, and yaw errors that are present. The processing unit can correct the X/Y disparity map for the errors estimated by the calibration module. 
     According to yet another aspect of the present invention, a method of determining an X/Y disparity map includes capturing first and second image data of a scene using a plurality of sensors or cameras. Pyramidal decomposition is applied to this image data to produce a plurality of decomposition levels with each level having a set of image pairs derived from the image data. The image resolution at each subsequent decomposition level is lower than the image resolution at the preceding decomposition level. A stereo disparity algorithm is executed at each decomposition level with the algorithm assigning a predetermined disparity shift value to each level. The stereo disparity algorithm applies interpolation onto the image data instead of onto derived disparity data. A disparity map is determined at each decomposition level with each disparity map being subsequently combined into an initial X/Y disparity map of the scene. The disparity shift values compensate for resolution differences that exist between the decomposition levels. Finally the pitch, roll, and yaw errors associated with angular misalignment of the first and second sensors are dynamically estimated on the captured image data and these error estimates are used to determine a final X/Y disparity map. 
     Further areas of applicability will become apparent from the description provided herein. It should be understood that the description and specific examples are intended for purposes of illustration only and are not intended to limit the scope of the present disclosure. 
    
    
     
       BRIEF DESCRIPTION OF THE DRAWINGS 
       The drawings described herein are for illustration purposes only and are not intended to limit the scope of the present disclosure in any way. 
         FIG. 1  is schematic representation of a stereovision system according to one embodiment of the present invention. 
         FIG. 2  is a schematic representation of a method for analyzing the stereovision image of a natural scene according to one embodiment of the present invention. 
         FIG. 3A  is a schematic representation of an image observed by the left or first camera in a stereovision system; 
         FIG. 3B  is a disparity map determined for the image of  FIG. 3A  according to the teachings of the present invention; 
         FIG. 3C  is a conventional disparity map determined for the image of  FIG. 3A  using conventional methodology; 
         FIG. 4  is a disparity map of a surface from the ground plane plotted as a function of the Y direction; 
         FIG. 5  is a plot of pitch, roll, and yaw errors determined during dynamic calibration of a stereovision system according to the teachings of the present invention. 
     
    
    
     DETAILED DESCRIPTION 
     The following description is merely exemplary in nature and is in no way intended to limit the present disclosure or its application or uses. It should be understood that throughout the description and drawings, corresponding reference numerals indicate like or corresponding parts and features. 
     The present invention generally provides a method of performing dynamic calibration of a stereo vision system using a specific stereo disparity algorithm adapted to provide for the determination of disparity in two dimensions, X and Y. In one embodiment of the present invention, an X/Y disparity map may be calculated using this algorithm without having to perform pre-warping or first finding the epipolar directions. Thus information related to camera misalignment and/or distortion can be preserved in the resulting X/Y disparity map and later extracted. 
     Referring to  FIG. 1 , a stereovision system  10  is shown that comprises a left camera (i.e., first sensor)  15  and a right camera (i.e., second sensor)  20  adapted to observe a scene  5  in real time. Both cameras  15 ,  20  generate a digital image  25 L,  25 R of the observed scene. The system further comprises a processing unit  30  that includes a calibration module  35  capable of dynamic calibration with respect to pitch, range, and yaw error. The processing unit is capable of applying predetermined algorithms and transforms to the digital image and calibration data to calculate an X/Y disparity map. 
     Referring now to  FIG. 2 , a method  100  for analyzing a stereovision image of a natural scene in order to produce an X/Y disparity map of the scene for use in controlling an adaptive or active system may comprise the steps of capturing  110  first image data of the scene with a first sensor and capturing  120  second image data of the scene with a second sensor. The method then applies  130  a technique known as pyramidal decomposition to the first image and second image data to produce a plurality of decomposition levels. Each of these decomposition levels has a set of image pairs derived from the first image data and second image data. A stereo disparity algorithm is then executed  140  at each decomposition level with a predetermined disparity shift value being assigned to each of these decomposition levels. This method includes determining  150  a disparity map at each of the decomposition levels and combining  160  these disparity maps to form an initial X/Y disparity map of the scene. The disparity shift values compensate for resolution differences that exist between the different decomposition levels. Finally, estimating  170  the pitch, roll, and yaw errors associated with angular misalignment of the first and second sensors on the captured image data allows for correcting  180  the image data for the errors and for determining  190  a final X/Y disparity map. One advantage of this method is that the stereo disparity algorithm applies interpolation onto the image data instead of onto derived disparity data. 
     According to one aspect of the present invention, a stereo disparity algorithm is independently applied  130  to each level in a series of pyramidal decomposition levels. Pyramidal decomposition levels mean that for each given image pair, multiple sets of image pairs are produced; with each set being at a different image resolution. The image resolution at each subsequent decomposition level is on the order of one-half the value determined for the previous or preceding level. Pyramidal decomposition may be performed according to any process known to one skilled-in-the-art, including but not limited to pixel resize, pixel average, Gaussian, or any wavelet decomposition process. 
     Upon application or execution  140  of the stereo disparity algorithm to each decomposition level, a predetermined disparity shift value is assigned to each decomposition level with the largest shift value being assigned to the lowest decomposition level, i.e., the level with the highest image resolution. The disparity maps determined  150  at each decomposition level are ultimately combined  160  together with the disparity shift values compensating for the pixel resolution differences that exist between the various decomposition levels. 
     The processing unit interprets each image as a continuous function (Ψ) equal to the summation over the entire level of two continuous gradients, represented by G(x,y) for the left image and F(x,y) for the right image, as shown by Equation 2. In Equation 2, the factor (x, y) represents the pixel location, (s x , s y ) is the predetermined disparity shift factor, (d x , d y ) is the remaining X/Y disparity, and b is the bias intensity at each decomposition level. The solution to this disparity calculation arises when d x , d y , and b are minimized.
 
Ψ=[ F ( x+s   x   +d   x   ,y+s   y   +d   y )− G ( x,y )+ b]   2   (Eq. 2)
 
     When d x  and d y  are small, a Taylor expansion may be used to approximate the continuous function as shown in Equation 3 where (F x , F y ) represents the gradient of F(x, y). 
     
       
         
           
             
               
                 
                   Ψ 
                   ≈ 
                   
                     ∑ 
                     
                       
                         [ 
                         
                           
                             
                               
                                 
                                   F 
                                   ⁡ 
                                   
                                     ( 
                                     
                                       
                                         x 
                                         + 
                                         
                                           s 
                                           x 
                                         
                                       
                                       , 
                                       
                                         y 
                                         + 
                                         
                                           s 
                                           y 
                                         
                                       
                                     
                                     ) 
                                   
                                 
                                 + 
                                 
                                   
                                     
                                       F 
                                       x 
                                     
                                     ⁡ 
                                     
                                       ( 
                                       
                                         
                                           x 
                                           + 
                                           
                                             s 
                                             x 
                                           
                                         
                                         , 
                                         
                                           y 
                                           + 
                                           
                                             s 
                                             y 
                                           
                                         
                                       
                                       ) 
                                     
                                   
                                   · 
                                   
                                     d 
                                     x 
                                   
                                 
                                 + 
                               
                             
                           
                           
                             
                               
                                 
                                   
                                     
                                       F 
                                       y 
                                     
                                     ⁡ 
                                     
                                       ( 
                                       
                                         
                                           x 
                                           + 
                                           
                                             s 
                                             x 
                                           
                                         
                                         , 
                                         
                                           y 
                                           + 
                                           
                                             s 
                                             y 
                                           
                                         
                                       
                                       ) 
                                     
                                   
                                   · 
                                   
                                     d 
                                     y 
                                   
                                 
                                 - 
                                 
                                   G 
                                   ⁡ 
                                   
                                     ( 
                                     
                                       x 
                                       , 
                                       y 
                                     
                                     ) 
                                   
                                 
                                 + 
                                 b 
                               
                             
                           
                         
                         ] 
                       
                       2 
                     
                   
                 
               
               
                 
                   ( 
                   
                     Eq 
                     . 
                     
                         
                     
                     ⁢ 
                     3 
                   
                   ) 
                 
               
             
           
         
       
     
     By letting, ∂Ψ/∂d x =∂Ψ/∂d y =∂Ψ/∂d b =0, then d x , d y  and b can be calculated using a simple linear function as described by Equation 4. The solution for Equation 4 becomes less reliable when the determinant of the 3×3 matrix becomes too small. Therefore when this determinant falls below a fixed threshold, the image pair at that decomposition level is automatically disqualified. For cases where Y disparity is always zero, the second row in Equation 4 and the second column in the 3×3 matrix can be eliminated. 
     
       
         
           
             
               
                 
                   
                     
                       ( 
                       
                         
                           
                             
                               
                                 F 
                                 x 
                                 2 
                               
                               _ 
                             
                           
                           
                             
                               
                                 
                                   F 
                                   x 
                                 
                                 ⁢ 
                                 
                                   F 
                                   y 
                                 
                               
                               _ 
                             
                           
                           
                             
                               
                                 F 
                                 x 
                               
                               _ 
                             
                           
                         
                         
                           
                             
                               
                                 
                                   F 
                                   x 
                                 
                                 ⁢ 
                                 
                                   F 
                                   y 
                                 
                               
                               _ 
                             
                           
                           
                             
                               
                                 F 
                                 y 
                                 2 
                               
                               _ 
                             
                           
                           
                             
                               
                                 F 
                                 y 
                               
                               _ 
                             
                           
                         
                         
                           
                             
                               
                                 F 
                                 x 
                               
                               _ 
                             
                           
                           
                             
                               
                                 F 
                                 y 
                               
                               _ 
                             
                           
                           
                             1 
                           
                         
                       
                       ) 
                     
                     · 
                     
                       ( 
                       
                         
                           
                             
                               d 
                               x 
                             
                           
                         
                         
                           
                             
                               d 
                               y 
                             
                           
                         
                         
                           
                             b 
                           
                         
                       
                       ) 
                     
                   
                   = 
                   
                     ( 
                     
                       
                         
                           
                             
                               
                                 F 
                                 x 
                               
                               · 
                               
                                 ( 
                                 
                                   G 
                                   - 
                                   F 
                                 
                                 ) 
                               
                             
                             _ 
                           
                         
                       
                       
                         
                           
                             
                               
                                 F 
                                 y 
                               
                               · 
                               
                                 ( 
                                 
                                   G 
                                   - 
                                   F 
                                 
                                 ) 
                               
                             
                             _ 
                           
                         
                       
                       
                         
                           
                             
                               ( 
                               
                                 G 
                                 - 
                                 F 
                               
                               ) 
                             
                             _ 
                           
                         
                       
                     
                     ) 
                   
                 
               
               
                 
                   ( 
                   
                     Eq 
                     . 
                     
                         
                     
                     ⁢ 
                     4 
                   
                   ) 
                 
               
             
           
         
       
     
     Since Equation 4 may be solved only when d x  and d y  are small, the solution of this equation is similar to, but not identical with the real solution that minimizes Equation 2. The solution of Equation 4 is then added into (s x , s y ), which becomes the disparity shift value for the next iteration. A final result or solution convergence will usually result prior to performing about 10 iterations and is highly dependent upon the value selected for the initial disparity shift value. 
     Preferably the calculation of the disparity starts with a zero initial disparity shift value assigned to the highest decomposition level where the disparity values are the smallest. The disparity map calculated at this higher decomposition level is then subsequently used as the initial disparity shift value to calculate the disparity map at the next lower decomposition level. The disparity maps determined  150  at different decomposition levels may be combined  160 . The disparity value from a lower decomposition level is selected unless the discrepancy between successive levels exceeds a fixed threshold value or the remaining error calculated by Equation 2 is determined to be lower. This latter method is preferable since it involves one less parameter, namely, the disparity discrepancy threshold. 
     The analysis of stereovision images using the teachings of the present invention provides several advantages over conventional analysis methodology. First, since the disparity shift values used for s x  and s y  in Equation 3 are floating-point numbers, the values for F, F x  and F y  in Equation 4 can be calculated via bilinear interpolation. Since this interpolation is applied onto the original data (i.e. the image) instead of the calculated data (i.e. the disparity), the resulting disparity map is more accurate. In addition, since the disparity map calculated at a higher decomposition level is used as the initial disparity shift for the next lower decomposition level increasing the number of the pyramid decomposition levels does not lead to any substantial increase in computation time or expenses. Finally, the fact that the local image intensity bias can be calculated automatically at the same time eliminates the need for special preprocessing such as Laplacian pyramid decomposition or feature (e.g. edge/corner) detection. It is no longer necessary to know the range of disparity beforehand as long as the largest disparity value remains small at the highest decomposition level. One skilled-in-the-art will understand that the stereo disparity algorithm can be directly applied to calculate X/Y disparities, and motion vector field (if the image pair consists of current and previous frames instead of left and right images). 
     Referring now to  FIGS. 3A-3C , results obtained in regards to creating a disparity map are depicted. In  FIG. 3A , the left image observed by the stereo vision camera system  10  is shown. In  FIG. 3B , an X/Y disparity map created according to the teachings of the present invention is shown. For comparison, an X/Y disparity map created utilizing conventional methodology is provided in  FIG. 3C . The X/Y disparity maps in both  FIGS. 3B and 3C  have been smoothed using the same median filter. In addition, since both of these disparity maps are color coded according to the same grey scale, the same grey shade represents the same disparity value in both maps. 
     The X/Y disparity map created according to the teachings of the present invention ( FIG. 3B ) provides several advantages over the X/Y disparity map created in a conventional manner ( FIG. 2C ) First, in the ground area, the disparity values are much smoother in the horizontal direction, which better reflects the flatness of the ground surface. Secondly, more details in the scene are revealed and fewer empty “holes” are found in the disparity map shown in  FIG. 3B . Finally, the edges of the various objects shown in the disparity map of  FIG. 3B  are observed with more detail or sharpness than the comparable object shown in the disparity map of  FIG. 3C . 
     Misalignments can occur between cameras or between the imaging sensor and the lens within a camera. These misalignments can be angular (i.e. relative yaw, pitch, and roll), or translational (i.e. baseline error, and sensor/lens shifting relative to the optical axis) in nature. It is known that the effect of the angular misalignments can be much larger than the effect of the translational misalignments. The angular misalignments inside a camera can be combined into the angular misalignments between the cameras if these misalignments are relatively small. The overall effect of yaw error is to cause the image to move horizontally, the pitch error to cause the image to move vertically, and the roll error to cause the image to rotate around the optical axis. The pitch, roll, and yaw errors can be estimated  170  and used to correct  180  the image data in order to determine  190  a final X/Y disparity map. 
     If the yaw, pitch, and roll angles (denoted as φ Y , φ P , and φ R ) of the right camera relative to the left camera are all very small, the error of disparity caused by this misalignment can be accurately estimated  170  according to Equation 5. In Equation 5, x and y represent the pixel location, f x , and f y  represent the lens focal lengths in pixels in the X and Y directions, and d x , and d y  represent the disparity errors in the X and Y directions. The coefficients, (f x /f y ) and (f y /f x ), for φ R  are a result of the fact that the image pixel is not always square. 
     
       
         
           
             
               
                 
                   { 
                   
                     
                       
                         
                           
                             d 
                             x 
                           
                           = 
                           
                             
                               
                                 + 
                                 
                                   
                                     
                                       f 
                                       x 
                                       2 
                                     
                                     + 
                                     
                                       x 
                                       2 
                                     
                                   
                                   
                                     f 
                                     x 
                                   
                                 
                               
                               · 
                               
                                 ϕ 
                                 Y 
                               
                             
                             - 
                             
                               
                                 
                                   
                                     f 
                                     x 
                                   
                                   · 
                                   y 
                                 
                                 
                                   f 
                                   y 
                                 
                               
                               · 
                               
                                 ϕ 
                                 R 
                               
                             
                           
                         
                       
                     
                     
                       
                         
                           
                             d 
                             y 
                           
                           = 
                           
                             
                               
                                 - 
                                 
                                   
                                     
                                       f 
                                       y 
                                       2 
                                     
                                     + 
                                     
                                       y 
                                       2 
                                     
                                   
                                   
                                     f 
                                     y 
                                   
                                 
                               
                               · 
                               
                                 ϕ 
                                 P 
                               
                             
                             - 
                             
                               
                                 
                                   
                                     f 
                                     y 
                                   
                                   · 
                                   x 
                                 
                                 
                                   f 
                                   x 
                                 
                               
                               · 
                               
                                 ϕ 
                                 R 
                               
                             
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   
                     Eq 
                     . 
                     
                         
                     
                     ⁢ 
                     5 
                   
                   ) 
                 
               
             
           
         
       
     
     Misalignment can be a serious issue if not controlled. For example, if a stereovision system has 720×240 image resolution and a 15° field-of-view. Then f x  and f y  can be computed to be 2733 and 1250, respectively. For such a system, at the center of the field of view, 0.1° of yaw error can result in 4.8 pixels of disparity error or 49% of range error at the range of 50 meters; and 0.1° of pitch error can result in 2.2 pixels of row misalignment during disparity calculation. 
     After creating the initial X/Y disparity map according to the teachings of the present disclosure, it is another objective of the present invention to estimate  170  and correct  180  for pitch, roll, and yaw errors. The pitch error and roll error may be estimated  170  based on the simple fact that the disparity value in the Y direction should be otherwise all zeros. Thus the pitch and roll error can be estimated  170  by finding φ P  and φ R  that minimize the solution for Equation 6 where (x, y) represents the pixel location, and d y  represents the calculated Y disparity. 
     
       
         
           
             
               
                 
                   
                     Ψ 
                     = 
                     
                       ∑ 
                       
                         
                           
                             [ 
                             
                               
                                 
                                   d 
                                   y 
                                 
                                 ⁡ 
                                 
                                   ( 
                                   
                                     x 
                                     , 
                                     y 
                                   
                                   ) 
                                 
                               
                               + 
                               
                                 
                                   
                                     C 
                                     P 
                                   
                                   ⁡ 
                                   
                                     ( 
                                     y 
                                     ) 
                                   
                                 
                                 · 
                                 
                                   ϕ 
                                   P 
                                 
                               
                               + 
                               
                                 
                                   
                                     C 
                                     R 
                                   
                                   ⁡ 
                                   
                                     ( 
                                     x 
                                     ) 
                                   
                                 
                                 · 
                                 
                                   ϕ 
                                   R 
                                 
                               
                             
                             ] 
                           
                           2 
                         
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         where 
                       
                     
                   
                   ⁢ 
                   
                     
 
                   
                   ⁢ 
                   
                     
                       
                         C 
                         P 
                       
                       ⁡ 
                       
                         ( 
                         y 
                         ) 
                       
                     
                     = 
                     
                       
                         
                           f 
                           y 
                           2 
                         
                         + 
                         
                           y 
                           2 
                         
                       
                       
                         f 
                         y 
                       
                     
                   
                 
               
               
                 
                   ( 
                   
                     Eq 
                     . 
                     
                         
                     
                     ⁢ 
                     6 
                   
                   ) 
                 
               
             
           
         
       
     
     By letting ∂Ψ/∂Ø p =∂Ψ/∂Ø R =0, then φ P  and φ R  can be calculated using linear Equation 7. 
     
       
         
           
             
               
                 
                   
                     
                       ( 
                       
                         
                           
                             
                               
                                 C 
                                 P 
                                 2 
                               
                               _ 
                             
                           
                           
                             
                               
                                 
                                   C 
                                   P 
                                 
                                 · 
                                 
                                   C 
                                   R 
                                 
                               
                               _ 
                             
                           
                         
                         
                           
                             
                               
                                 
                                   C 
                                   P 
                                 
                                 · 
                                 
                                   C 
                                   R 
                                 
                               
                               _ 
                             
                           
                           
                             
                               
                                 C 
                                 R 
                                 2 
                               
                               _ 
                             
                           
                         
                       
                       ) 
                     
                     · 
                     
                       ( 
                       
                         
                           
                             
                               ϕ 
                               P 
                             
                           
                         
                         
                           
                             
                               ϕ 
                               R 
                             
                           
                         
                       
                       ) 
                     
                   
                   = 
                   
                     - 
                     
                       ( 
                       
                         
                           
                             
                               
                                 
                                   C 
                                   P 
                                 
                                 · 
                                 
                                   d 
                                   y 
                                 
                               
                               _ 
                             
                           
                         
                         
                           
                             
                               
                                 
                                   C 
                                   R 
                                 
                                 · 
                                 
                                   d 
                                   y 
                                 
                               
                               _ 
                             
                           
                         
                       
                       ) 
                     
                   
                 
               
               
                 
                   ( 
                   
                     Eq 
                     . 
                     
                         
                     
                     ⁢ 
                     7 
                   
                   ) 
                 
               
             
           
         
       
     
     Compared to estimation of pitch error and roll error, estimation of yaw error is more complex because a reference disparity map is not available unless it is a controlled scene, such as an image template at a known distance. However, road scenes that are frequently encountered include ground area that is relatively flat or smooth and occupy a substantial portion of the field of view. The range, R, as a function of vertical pixel location, y, in the image can be determined from Equation 8, where f y  is lens focal length in pixels along Y direction, H is the height of the cameras above the ground, θ is the angle of the cameras tilting toward the ground, and α is the current vehicle pitch relative to the ground plane. 
     
       
         
           
             
               
                 
                   R 
                   = 
                   
                     
                       H 
                       · 
                       
                         cos 
                         ⁡ 
                         
                           ( 
                           α 
                           ) 
                         
                       
                     
                     
                       
                         sin 
                         ⁡ 
                         
                           ( 
                           
                             θ 
                             - 
                             α 
                           
                           ) 
                         
                       
                       - 
                       
                         
                           y 
                           
                             f 
                             y 
                           
                         
                         · 
                         
                           cos 
                           ⁡ 
                           
                             ( 
                             
                               θ 
                               - 
                               α 
                             
                             ) 
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   
                     Eq 
                     . 
                     
                         
                     
                     ⁢ 
                     8 
                   
                   ) 
                 
               
             
           
         
       
     
     After the range R is converted into X disparity, Equation 8 is transformed into Equation 9 where f x  represents the lens focal length in pixels along X direction, B represents the baseline between the two cameras, and d x  represents the horizontal disparity. 
     
       
         
           
             
               
                 
                   
                     d 
                     x 
                   
                   = 
                   
                     
                       
                         
                           f 
                           x 
                         
                         · 
                         B 
                       
                       
                         H 
                         · 
                         
                           cos 
                           ⁡ 
                           
                             ( 
                             α 
                             ) 
                           
                         
                       
                     
                     · 
                     
                       [ 
                       
                         
                           sin 
                           ⁡ 
                           
                             ( 
                             
                               θ 
                               - 
                               α 
                             
                             ) 
                           
                         
                         - 
                         
                           
                             y 
                             
                               f 
                               y 
                             
                           
                           · 
                           
                             cos 
                             ⁡ 
                             
                               ( 
                               
                                 θ 
                                 - 
                                 α 
                               
                               ) 
                             
                           
                         
                       
                       ] 
                     
                   
                 
               
               
                 
                   ( 
                   
                     Eq 
                     . 
                     
                         
                     
                     ⁢ 
                     9 
                   
                   ) 
                 
               
             
           
         
       
     
     By taking yaw error and roll error into consideration, Equations 10, 11a, and 11b can be obtained from a combination of Equations 5 and 9. 
     
       
         
           
             
               
                 
                   
                       
                   
                   ⁢ 
                   
                     
                       d 
                       x 
                     
                     = 
                     
                       
                         
                           d 
                           0 
                         
                         ⁡ 
                         
                           ( 
                           x 
                           ) 
                         
                       
                       + 
                       
                         K 
                         · 
                         y 
                       
                     
                   
                 
               
               
                 
                   ( 
                   
                     Eq 
                     . 
                     
                         
                     
                     ⁢ 
                     10 
                   
                   ) 
                 
               
             
             
               
                 
                   { 
                   
                     
                       
                         
                           
                             
                               d 
                               0 
                             
                             ⁡ 
                             
                               ( 
                               x 
                               ) 
                             
                           
                           = 
                           
                             
                               
                                 
                                   + 
                                   
                                     
                                       
                                         f 
                                         x 
                                         2 
                                       
                                       + 
                                       
                                         x 
                                         2 
                                       
                                     
                                     
                                       f 
                                       x 
                                     
                                   
                                 
                                 · 
                                 
                                   ϕ 
                                   Y 
                                 
                               
                               + 
                               
                                 
                                   
                                     f 
                                     x 
                                   
                                   · 
                                   B 
                                   · 
                                   
                                     sin 
                                     ⁡ 
                                     
                                       ( 
                                       
                                         θ 
                                         - 
                                         α 
                                       
                                       ) 
                                     
                                   
                                 
                                 
                                   H 
                                   · 
                                   
                                     cos 
                                     ⁡ 
                                     
                                       ( 
                                       α 
                                       ) 
                                     
                                   
                                 
                               
                             
                             = 
                             
                               
                                 
                                   + 
                                   
                                     
                                       
                                         f 
                                         x 
                                         2 
                                       
                                       + 
                                       
                                         x 
                                         2 
                                       
                                     
                                     
                                       f 
                                       x 
                                     
                                   
                                 
                                 · 
                                 
                                   ϕ 
                                   Y 
                                 
                               
                               + 
                               
                                 
                                   
                                     f 
                                     x 
                                   
                                   · 
                                   B 
                                   · 
                                   
                                     sin 
                                     ⁡ 
                                     
                                       ( 
                                       θ 
                                       ) 
                                     
                                   
                                 
                                 H 
                               
                               - 
                               
                                 Δ 
                                 ⁡ 
                                 
                                   ( 
                                   α 
                                   ) 
                                 
                               
                             
                           
                         
                       
                     
                     
                       
                         
                           K 
                           = 
                           
                             
                               
                                 
                                   - 
                                   
                                     
                                       f 
                                       x 
                                     
                                     
                                       f 
                                       y 
                                     
                                   
                                 
                                 · 
                                 
                                   ϕ 
                                   R 
                                 
                               
                               - 
                               
                                 
                                   
                                     f 
                                     x 
                                   
                                   · 
                                   B 
                                   · 
                                   
                                     cos 
                                     ⁡ 
                                     
                                       ( 
                                       
                                         θ 
                                         - 
                                         α 
                                       
                                       ) 
                                     
                                   
                                 
                                 
                                   
                                     f 
                                     y 
                                   
                                   · 
                                   H 
                                   · 
                                   
                                     cos 
                                     ⁡ 
                                     
                                       ( 
                                       α 
                                       ) 
                                     
                                   
                                 
                               
                             
                             = 
                             
                               
                                 
                                   - 
                                   
                                     
                                       f 
                                       x 
                                     
                                     
                                       f 
                                       y 
                                     
                                   
                                 
                                 · 
                                 
                                   ϕ 
                                   R 
                                 
                               
                               - 
                               
                                 
                                   
                                     f 
                                     x 
                                   
                                   · 
                                   B 
                                   · 
                                   
                                     cos 
                                     ⁡ 
                                     
                                       ( 
                                       θ 
                                       ) 
                                     
                                   
                                 
                                 
                                   
                                     f 
                                     y 
                                   
                                   · 
                                   H 
                                 
                               
                               - 
                               
                                 
                                   
                                     tan 
                                     ⁡ 
                                     
                                       ( 
                                       θ 
                                       ) 
                                     
                                   
                                   
                                     f 
                                     y 
                                   
                                 
                                 · 
                                 
                                   Δ 
                                   ⁡ 
                                   
                                     ( 
                                     α 
                                     ) 
                                   
                                 
                               
                             
                           
                         
                       
                     
                   
                   ⁢ 
                   
                       
                   
                 
               
               
                 
                   ( 
                   
                     
                       Eq 
                       . 
                       
                           
                       
                       ⁢ 
                       11 
                     
                     ⁢ 
                     a 
                   
                   ) 
                 
               
             
             
               
                 
                   
                       
                   
                   ⁢ 
                   
                     
                       where 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         Δ 
                         ⁡ 
                         
                           ( 
                           α 
                           ) 
                         
                       
                     
                     = 
                     
                       
                         
                           
                             f 
                             x 
                           
                           · 
                           B 
                           · 
                           
                             cos 
                             ⁡ 
                             
                               ( 
                               θ 
                               ) 
                             
                           
                         
                         H 
                       
                       · 
                       
                         tan 
                         ⁡ 
                         
                           ( 
                           α 
                           ) 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   
                     
                       Eq 
                       . 
                       
                           
                       
                       ⁢ 
                       11 
                     
                     ⁢ 
                     b 
                   
                   ) 
                 
               
             
           
         
       
     
     Since both θ and α are usually small, the vehicle pitch α has little effect on K, but a large effect on d 0 (x). In addition, d 0 (x) is almost independent of x because usually f x   2 &gt;&gt;x 2 . According to Equations 10 and 11(a-b), the disparity map in the ground area is a surface that almost linearly depends solely on the vertical pixel location y. After calculating roll error φ R  according to Equations 6-7 and ignoring the vehicle&#39;s pitch, α, the slope K of this surface can be calculated. 
     Referring to  FIG. 4  an example of the disparity map representing the surface from the ground plane obtained according to the teachings of the present disclosure is shown. The disparity map in  FIG. 4  has been turned 90° sideway, and therefore it is shown as a plot of disparity vs. y. Because this surface is almost a linear plane with a fixed known slope, the disparities on this surface (i.e. the ground area) can be isolated using conventional algorithms, such as a Hugh transform. 
     The unknown vehicle pitch α can be estimated using simple lane detection techniques that detect straight lines using conventional algorithms. Once a straight section of a lane is detected in the image, the vanishing point can be easily located using the intersection of the lane edges. By letting R=∞ in Equation 8 or d x =0 in Equation 9, the vertical pixel location of the vanishing point can be written as provided in Equation 12. Therefore the vehicle pitch α can be computed if the vertical pixel location of the vanishing point is known.
 
 y=f   y ·tan(θ−α)  (Eq. 12)
 
     The yaw error can be estimated  170  by the minimization of Equation 13 provided the ground area has been isolated and the vehicle pitch α is known. In Equation 13, (x, y) represents the pixel location, d x  is the calculated X (or horizontal) disparity, and Σ is summation over the ground area.
 
Ψ=Σ[ d   x ( x,y )− d   0 ( x )− K·y]   2   (Eq. 13)
 
     By letting ∂Ψ/∂Ø y =0, φ Y  can easily be calculated using Equations 14a and 14b. 
     
       
         
           
             
               
                 
                   
                     ϕ 
                     Y 
                   
                   = 
                   
                     
                       
                         
                           
                             C 
                             Y 
                           
                           · 
                           
                             d 
                             x 
                           
                         
                         _ 
                       
                       - 
                       
                         K 
                         · 
                         
                           
                             
                               C 
                               Y 
                             
                             · 
                             y 
                           
                           _ 
                         
                       
                       - 
                       
                         
                           
                             C 
                             Y 
                           
                           _ 
                         
                         · 
                         
                           D 
                           0 
                         
                       
                     
                     
                       
                         C 
                         Y 
                         2 
                       
                       _ 
                     
                   
                 
               
               
                 
                   ( 
                   
                     
                       Eq 
                       . 
                       
                           
                       
                       ⁢ 
                       14 
                     
                     ⁢ 
                     a 
                   
                   ) 
                 
               
             
             
               
                 
                   
                     
                       where 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         C 
                         Y 
                       
                     
                     = 
                     
                       
                         
                           f 
                           x 
                           2 
                         
                         + 
                         
                           x 
                           2 
                         
                       
                       
                         f 
                         x 
                       
                     
                   
                   , 
                   
                     
 
                   
                   ⁢ 
                   
                     
                       and 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         D 
                         0 
                       
                     
                     = 
                     
                       
                         
                           
                             f 
                             x 
                           
                           · 
                           B 
                           · 
                           
                             sin 
                             ⁡ 
                             
                               ( 
                               θ 
                               ) 
                             
                           
                         
                         H 
                       
                       - 
                       
                         Δ 
                         ⁡ 
                         
                           ( 
                           α 
                           ) 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   
                     
                       Eq 
                       . 
                       
                           
                       
                       ⁢ 
                       14 
                     
                     ⁢ 
                     b 
                   
                   ) 
                 
               
             
           
         
       
     
     Referring now to  FIG. 5 , an example dynamic calibration resulting from an image sequence of 132 frames is shown. In this sequence, a vehicle was moving straight forward with only one left turn near the end of the sequence (i.e. from frame # 80  to frame # 110 ). The angular misalignments were estimated  170  for every frame in the entire sequence. The yaw error is shown as triangles, the pitch error is shown as squares, and the roll error is shown as diamonds. The frame-by-frame misalignment estimation is shown as scattered data points, the average of the estimation is shown as solid lines, and the misalignment information from offline calibration is shown as dotted lines. If the vehicle pitch is assumed to be zero there will be a small difference between the actual yaw error and the frame-by-frame estimated yaw error as shown in  FIG. 4 . Based on Equation 14, this small difference can be determined from Equation 15 where f x  is the lens focal length in pixels along the X direction, B is the baseline between the two cameras, H is the height of the cameras above the ground, θ is the angle of the cameras tilting towards the ground, and a is the current vehicle pitch relative to the ground plane. In Equation 15, it is assumed that f x   2 &gt;&gt;x 2  and cos(θ)·tan(α)≈α, since these conditions are usually accurate. 
     
       
         
           
               
             
               
                 
                   
                     
                       
                         
                           
                             δ 
                             
                               ϕ 
                               ⁢ 
                               
                                   
                               
                               ⁢ 
                               Y 
                             
                           
                           = 
                             
                           ⁢ 
                           
                             
                               
                                 
                                   C 
                                   Y 
                                 
                                 _ 
                               
                               
                                 
                                   C 
                                   Y 
                                   2 
                                 
                                 _ 
                               
                             
                             · 
                             Δα 
                           
                         
                       
                     
                     
                       
                         
                           ≈ 
                             
                           ⁢ 
                           
                             
                               1 
                               
                                 f 
                                 x 
                               
                             
                             · 
                             
                               
                                 
                                   f 
                                   x 
                                 
                                 · 
                                 B 
                                 · 
                                 
                                   cos 
                                   ⁡ 
                                   
                                     ( 
                                     θ 
                                     ) 
                                   
                                 
                               
                               H 
                             
                             · 
                             
                               tan 
                               ⁡ 
                               
                                 ( 
                                 α 
                                 ) 
                               
                             
                           
                         
                       
                     
                     
                       
                         
                           ≈ 
                             
                           ⁢ 
                           
                             
                               B 
                               H 
                             
                             · 
                             α 
                           
                         
                       
                     
                   
                 
                 
                   
                     ( 
                     
                       Eq 
                       . 
                       
                           
                       
                       ⁢ 
                       15 
                     
                     ) 
                   
                 
               
             
           
         
       
     
       FIG. 5  demonstrates that the estimation of the pitch error is very stable, almost independent of the scene change. This is an indication of high accuracy even though its average is about 0.028° higher than the result from off-line calibration. The estimation of the roll error is slightly less stable because, according to Equation 5, only the disparities near left and right image boundaries have significant contribution in the calculation. But the average estimated roll error is very close to the result from off-line calibration being only about 0.0037° lower. The estimation of the yaw error has the largest fluctuation due to disparities in the ground area making a significant contribution in the calculation. 
     The estimated  170  pitch, roll, and yaw errors can be used to correct  180  the image data displayed in the initial X/Y disparity map data in order to determine  190  a final X/Y disparity map that can be used for the effective control of an adaptive or active system. Thus after dynamic calibration, the initial X/Y disparity map may be converted into an X disparity map, which in turn can be converted into a range map using Equation 1 directly. Compared to existing algorithms, the new stereo dispersity algorithm is more accurate because it applies interpolation onto the original data (i.e. the image) instead of the calculated data (i.e. the disparity). This algorithm also replaces sequential search with directional search based on local image gradient, and therefore fewer steps are needed and the search can be conducted in two dimensions directly. With this algorithm, the X/Y disparity map can be calculated without pre-warping, and therefore the information of camera misalignment (both before and after offline calibration) can be preserved in the final X/Y disparity map and later extracted. The estimation of the pitch and roll errors is more accurate than the estimation of the yaw error. As for yaw error, the long-term average of the estimation is still fairly accurate even when the vehicle pitch is not taken into consideration. If desired to enhance stability of performance the results from dynamic calibration may be averaged or filtered over a long period of time before they are applied to the system. 
     The foregoing description of various embodiments of the invention has been presented for purposes of illustration and description. It is not intended to be exhaustive or to limit the invention to the precise embodiments disclosed. Numerous modifications or variations are possible in light of the above teachings. The embodiments discussed were chosen and described to provide the best illustration of the principles of the invention and its practical application to thereby enable one of ordinary skill in the art to utilize the invention in various embodiments and with various modifications as are suited to the particular use contemplated. All such modifications and variations are within the scope of the invention as determined by the appended claims when interpreted in accordance with the breadth to which they are fairly, legally, and equitably entitled.