Abstract:
A moving sensor suite for imaging a scene has three Doppler radars, two moving and one fixed, a fixed video camera and a fixed GPS receiver. The Doppler radars measure the relative velocities between the radars and the scene, as well as the scene&#39;s electromagnetic reflectivity, while the video camera records the motion of the camera and the optical property of the scene. The correct registration of the Doppler radars and the camera is established by finding the intersections of the moving Doppler radar motion vectors with the image plane of the video camera. The scene features in the first frame are determined by Doppler circle intersections. The correspondences of the features in the next two frames are established by a feature matching operation.

Description:
TECHNICAL FIELD 
       [0001]    The invention relates to a 3D video-Doppler-radar (VIDAR) imaging system. 
       BACKGROUND OF THE INVENTION 
       [0002]    Traditionally, a Doppler radar only measures a relative velocity at line-of-sight (LOS). In  FIG. 3 , the relative velocity of a stationary scene point is calculated in terms of Doppler frequency by 
         [0000]        f   D   =Kν   r  cos(θ r ),   (1) 
         [0000]    the relative velocity of an approaching moving object is calculated as ( FIG. 4 ) 
         [0000]        f   D   =K[ν   r  cos(θ r )+ν t  cos(φ t )]  (2) 
         [0000]    and the relative velocity of a leaving moving object is ( FIG. 5 ) 
         [0000]        f   D   =K[ν   r  cos(θ r )−ν t  cos(θ t )]  (3) 
         [0000]    where K is a Doppler frequency conversion constant. No range and other 3D structure information is available from single Doppler radar. 
         [0003]    There are a number of methods of deriving 3D structure information from either a single image or a sequence (or sequences) of images, collectively called the “structure from X methods”. These methods include: structure from texture, structure from shadow, structure from motion, structure from stereo, structure from focusing/defocusing, etc. However, none of these methods can reliably provide 3D information without strict conditions. 
         [0004]    Since a Doppler radar and a camera measure different properties of a 3D scene, fusion of radar and camera signals is desired to obtain reliable information about the scene. Correct registration of a Doppler radar with a camera is crucial for information fusion. So far, there is no reliable way of registering a Doppler radar and a camera, such that the measurements from the Doppler radar and camera all refer to the same physical point in the scene. 
         [0005]      FIG. 4  illustrates the relative velocity measurement of an approaching moving object. 
         [0006]      FIG. 5  illustrates the relative velocity measurement of a leaving moving object. 
       SUMMARY  
       [0007]    An imaging system to generate an image of a scene may include a first movable radar to generate a first radar motion ray, a second movable radar to generate a second radar motion ray, a third fixed radar to generate a third radar direction ray, and a video camera to receive data from the intersection of the first radar motion ray and the second radar motion ray. 
         [0008]    The imaging system may generate a first Doppler difference based upon the difference between the first radar signal and the third radar signal, and the imaging system may generate a second Doppler difference based upon the difference between the second radar signal and the third radar signal. 
         [0009]    The imaging system may generate a first cone angle and a first circle based upon the first Doppler difference, and the imaging system may generate a second cone angle and a second circle based upon the second Doppler difference. 
         [0010]    The imaging system may generate a Doppler ratio based upon the first Doppler difference and the second Doppler difference. 
         [0011]    The imaging system may generate a feature point based upon the first and second cone angles and the first and second circles, and the first radar may be a first Doppler radar. 
         [0012]    The second radar may be a second Doppler radar, and the third fixed radar may be a third Doppler radar. 
         [0013]    The sensor apparatus may include a global positioning system receiver. 
         [0014]    An imaging system adapted for registration may include a first movable radar to generate a first radar motion ray, a second movable radar to generate a second radar motion ray, a third fixed radar to generate a third radar direction ray and a video camera to define a virtual image plane. The first radar motion ray and the second radar motion ray may be registered with the virtual image plane. 
     
    
     
       BRIEF DESCRIPTION OF THE DRAWINGS 
         [0015]    The invention may be understood by reference to the following description taken in conjunction with the accompanying drawings, in which, like reference numerals identify like elements, and in which: 
           [0016]      FIG. 1  illustrates a sensor system for 3D video Doppler radar (vidar) imaging; 
           [0017]      FIG. 2  illustrates the layout of the sensor system; 
           [0018]      FIG. 3  illustrates the relative velocity of a stationary scene point; 
           [0019]      FIG. 4  illustrates the relative velocity measurement of an approaching moving object; 
           [0020]      FIG. 5  illustrates the relative velocity measurement of a leaving moving object; 
           [0021]      FIG. 6  illustrates the functional flow chart of the system; and 
           [0022]      FIG. 7  illustrates the registration of the first and second moving Doppler radars with the video camera. 
       
    
    
     DETAILED DESCRIPTION 
       [0023]    While the term “video-Doppler-radar” is used herein, it also may refer to other combinations, such as “Doppler-radar-video”, “camera-Doppler-radar”, Doppler-radar-camera“, etc. The term “video” may refer to “any image sequences” which may be generated by electro-optical or thermal or hyper-spectral devices. The invention discussed here can be applied to the case of multiple video cameras. 
         [0024]    A sensor system for 3D vidar imaging is shown in  FIG. 1  where  1 —the sensor system which may include a sensor suite or apparatus,  2 —a first moving Doppler radar motion ray,  3 —a second moving Doppler radar motion ray,  4 —a radar direction ray connecting the sensor apparatus  1  to a scene point  9 ,  5 —the sensor suite motion vector,  6 —the camera virtual image plane of the video camera  14 ,  7 —the intersection of the first Doppler radar motion ray  2  with the virtual image plane  6 ,  8 —the intersection of the second Doppler radar motion ray  3  with the virtual image plane  6 , and  9 —a 3D scene point, and  10 —the intersection of a direction ray connecting the sensor apparatus  1  and the 3D scene point  9  with the virtual image plane  6 . 
         [0025]      FIG. 2  shows the layout of the sensor apparatus  1  where  11 —a first moving Doppler radar,  12 —a second moving Doppler radar,  13 —a fixed or stationary Doppler radar,  14 —a fixed or stationary video camera,  15 —a GPS receiver and  100 —information computing device such as a computer, laptop, personal computer, PDA or other such device. 
         [0026]    The functional flow chart of the system is shown in  FIG. 6 . In the following, we will describe the functional blocks. 
         [0027]    1. Register Doppler Radars and Video Camera 
         [0028]    The first and second Doppler radars  11 , 12  in the sensor apparatus  1  may be extended or retracted or moved side to side as illustrated in steps  601 ,  605  by a motor (not shown) which may be a DC or stepper motor or other movement device and may be moved on sliding tracks (not shown). An optical encoder (not shown) may be mounted on the shaft of the motor, so the sliding speeds of the Doppler radars (ν r     1    and ν r     2    in  FIG. 2 ) may be determined. The sliding track orientation angles (θ r     1    and θ r     2    in  FIG. 2 ) are predetermined. Using a calibration method, the intersections (C 1  and C 2  in  FIG. 1 ) of the first and second motion rays  2 ,  3  with the virtual image planes  6  are determined. Note, this registration method can be applied to a plurality of Doppler radars and cameras. 
         [0029]    It can be seen in  FIG. 7 , showing the registration of the first and second moving Doppler radars  11 , 12  with the video camera  14 , with the determination of C 1  and C 2  that the first and second moving Doppler radars  11 ,  12  may be substantially precisely registered with the video camera  14 . The locations of substantially equal-Doppler cones of each of the radars  11 ,  12  may be determined on the camera&#39;s virtual image plane  6 , so that the physical information from the scene  9  may be calculated from both Doppler and video signals from the first moving radar  11 , the second moving radar  12 , the stationary Doppler radar  13  and the video camera  14 . The computing device  100  may accept inputs from the above described elements and may perform the following calculations. 
         [0030]    2. Calculate Doppler Differences for k th Frame 
         [0031]    Assume the current time is the time of the k th video image frame, i.e., t=k in step  607 ,  609 . The Doppler frequencies of scene point  p   i  induced by both moving Doppler radars may be given by 
         [0000]        f   D     k     1,i   =K   1 [ν p     k    cos(α k   i )+ν r     k    cos(θ r     1k     i )],  i= 1, . . . ,  N    (4) 
         [0000]      and 
         [0000]        f   D     k     2,i   =K   2 [ν p     k    cos(α k   i )+ν r     k    cos(θ r     2k     i )],  i= 1, . . . ,  N    (5) 
         [0000]    where N may be the number of scene points  9 , K 1  and K 2  may be Doppler conversion constants for the first and second moving Doppler radar ( 11  and  12  in  FIG. 2 ), and θ r     1     k   i , θ r     2     k   i , and α k   i  are depicted in  FIG. 2  with an additional time index k and a scene point index i. A fixed Doppler radar  13  may be used to sense the platform motion 
         [0000]        f   D     k     3,i   =K   3 ν p     k    cos(α k   i ),  i= 1, . . . ,  N    (6) 
         [0000]    where K 3  is the Doppler conversion constant for the fixed Doppler radar ( 13  in  FIG. 2 ). Since all three radars  11 , 12 , 13  may be located together and assuming that the distance from the sensor suite to point  p   i  may be much larger than the distance between radars  11 , 12 , 13 , the following Doppler differences may be 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         Δ 
                          
                         
                             
                         
                          
                         
                           f 
                           
                             D 
                             k 
                           
                           
                             1 
                             , 
                             i 
                           
                         
                       
                       = 
                       
                         
                           
                             
                               f 
                               
                                 D 
                                 k 
                               
                               
                                 1 
                                 , 
                                 i 
                               
                             
                             
                               K 
                               1 
                             
                           
                           - 
                           
                             
                               f 
                               
                                 D 
                                 k 
                               
                               
                                 3 
                                 , 
                                 i 
                               
                             
                             
                               K 
                               3 
                             
                           
                         
                         = 
                         
                           
                             v 
                             
                               r 
                               
                                 1 
                                  
                                 
                                     
                                 
                                  
                                 k 
                               
                             
                           
                            
                           
                             cos 
                              
                             
                               ( 
                               
                                 θ 
                                 
                                   r 
                                   
                                     1 
                                      
                                     
                                         
                                     
                                      
                                     k 
                                   
                                 
                                 i 
                               
                               ) 
                             
                           
                         
                       
                     
                     , 
                     
                       i 
                       = 
                       1 
                     
                     , 
                     … 
                      
                     
                         
                     
                     , 
                     N 
                   
                    
                   
                     
 
                   
                    
                   and 
                 
               
               
                 
                   ( 
                   7 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       Δ 
                        
                       
                           
                       
                        
                       
                         f 
                         
                           D 
                           k 
                         
                         
                           2 
                           , 
                           i 
                         
                       
                     
                     = 
                     
                       
                         
                           
                             f 
                             
                               D 
                               k 
                             
                             
                               2 
                               , 
                               i 
                             
                           
                           
                             K 
                             2 
                           
                         
                         - 
                         
                           
                             f 
                             
                               D 
                               k 
                             
                             
                               3 
                               , 
                               i 
                             
                           
                           
                             K 
                             3 
                           
                         
                       
                       = 
                       
                         
                           v 
                           
                             r 
                             
                               2 
                                
                               
                                   
                               
                                
                               k 
                             
                           
                         
                          
                         
                           cos 
                            
                           
                             ( 
                             
                               θ 
                               
                                 r 
                                 
                                   2 
                                    
                                   
                                       
                                   
                                    
                                   k 
                                 
                               
                               i 
                             
                             ) 
                           
                         
                       
                     
                   
                   , 
                   
                     i 
                     = 
                     1 
                   
                   , 
                   … 
                    
                   
                       
                   
                   , 
                   N 
                 
               
               
                 
                   ( 
                   8 
                   ) 
                 
               
             
           
         
       
     
         [0000]    where the impact of the platform motion may have been removed. Eqs. (7) and (8) may actually recover the substantially independent motion Doppler signals of the first and second moving Doppler radars  11 ,  12 , except for the conversion constants. The Doppler differences in Eqs. (7) and (8) are the ones for the first frame. 
         [0032]    3. Calculate Doppler Cone Angles and Doppler Circles 
         [0033]    From Eqs. (7) and (8), Doppler cone angles at t=k may be calculated as illustrated in steps  611 ,  613  as 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       θ 
                       
                         r 
                         
                           1 
                            
                           
                               
                           
                            
                           k 
                         
                       
                       i 
                     
                     = 
                     
                       
                         cos 
                         
                           - 
                           1 
                         
                       
                        
                       
                         ( 
                         
                           
                             Δ 
                              
                             
                                 
                             
                              
                             
                               f 
                               
                                 D 
                                 k 
                               
                               
                                 1 
                                 , 
                                 i 
                               
                             
                           
                           
                             v 
                             
                               r 
                               
                                 1 
                                  
                                 
                                     
                                 
                                  
                                 k 
                               
                             
                           
                         
                         ) 
                       
                     
                   
                    
                   
                     
 
                   
                    
                   and 
                 
               
               
                 
                   ( 
                   9 
                   ) 
                 
               
             
             
               
                 
                   
                     θ 
                     
                       r 
                       
                         2 
                          
                         
                             
                         
                          
                         k 
                       
                     
                     i 
                   
                   = 
                   
                     
                       
                         cos 
                         
                           - 
                           1 
                         
                       
                        
                       
                         ( 
                         
                           
                             Δ 
                              
                             
                                 
                             
                              
                             
                               f 
                               
                                 D 
                                 k 
                               
                               
                                 2 
                                 , 
                                 i 
                               
                             
                           
                           
                             v 
                             
                               r 
                               
                                 2 
                                  
                                 
                                     
                                 
                                  
                                 k 
                               
                             
                           
                         
                         ) 
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   10 
                   ) 
                 
               
             
           
         
       
     
         [0034]    Using Doppler cone angles in Eqs. (9) and (10), Doppler circles 1  may be constructed on the virtual image plane  6 , as shown in  FIG. 7 ). The intersection of the Doppler circles specified by θ r     1k     i  and θ r     2k     i  may be a feature point, q i , as indicated in  FIG. 7 ).  1 Precisely speaking, these may be ellipses. Due to a small angle between radar motion vectors, the ellipses may be well approximated as circles. 
         [0035]      4 . Calculate Doppler Ratio for the k th Frame 
         [0036]    In step numerals  615 ,  617 , let the moving Doppler radar motion vectors be 
         [0000]        ν   r     1k     =[n   x1     k     , n   y1     k     , n   z1     k   ],  ν   r     2k     =[n   x2     k     , n   y2     k     , n   z2     k   ]  (11) 
         [0000]    which are known from the calibration. Assume the locations of the sensor apparatus  1  may be known from the on-board GPS  15 :  S   k =[x s , y s , z s ] k . The vector from the location of sensor apparatus  1   S   k  to point p i =[x i , y i , z i ] may be 
         [0000]          V     Sp     k     i   =[x   i   −x   s     k     , y   i   −y   s     k     , z   i   −z   s     k   ]  (12) 
         [0000]    and the angles, θ r     1k     i  and θ r     2k     i  and may be defined by 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       θ 
                       
                         r 
                         
                           1 
                            
                           
                               
                           
                            
                           k 
                         
                       
                       i 
                     
                     = 
                     
                       
                         cos 
                         
                           - 
                           1 
                         
                       
                        
                       
                         
                           
                             
                               V 
                               _ 
                             
                             
                               Sp 
                               k 
                             
                             i 
                           
                           · 
                           
                             
                               v 
                               _ 
                             
                             
                               r 
                               
                                 1 
                                  
                                 
                                     
                                 
                                  
                                 k 
                               
                             
                           
                         
                         
                           
                              
                             
                               
                                 V 
                                 _ 
                               
                               
                                 Sp 
                                 k 
                               
                               i 
                             
                              
                           
                            
                           
                              
                             
                               
                                 v 
                                 _ 
                               
                               
                                 r 
                                 
                                   1 
                                    
                                   
                                       
                                   
                                    
                                   k 
                                 
                               
                             
                              
                           
                         
                       
                     
                   
                    
                   
                     
 
                   
                    
                   and 
                 
               
               
                 
                   ( 
                   13 
                   ) 
                 
               
             
             
               
                 
                   
                     θ 
                     
                       r 
                       - 
                       
                         2 
                          
                         
                             
                         
                          
                         k 
                       
                     
                     i 
                   
                   = 
                   
                     
                       cos 
                       
                         - 
                         1 
                       
                     
                      
                     
                       
                         
                           
                             
                               V 
                               _ 
                             
                             
                               Sp 
                               k 
                             
                             i 
                           
                           · 
                           
                             
                               v 
                               _ 
                             
                             
                               r 
                               
                                 2 
                                  
                                 
                                     
                                 
                                  
                                 k 
                               
                             
                           
                         
                         
                           
                              
                             
                               
                                 V 
                                 _ 
                               
                               
                                 Sp 
                                 k 
                               
                               i 
                             
                              
                           
                            
                           
                              
                             
                               
                                 v 
                                 _ 
                               
                               
                                 r 
                                 
                                   2 
                                    
                                   
                                       
                                   
                                    
                                   k 
                                 
                               
                             
                              
                           
                         
                       
                       . 
                     
                   
                 
               
               
                 
                   ( 
                   14 
                   ) 
                 
               
             
           
         
       
     
         [0037]    Substituting Eqs. (13) and (14) into Eqs. (7) and (8) gives 
         [0000]    
       
         
           
             
               
                 
                   
                     Δ 
                      
                     
                         
                     
                      
                     
                       f 
                       
                         D 
                         k 
                       
                       
                         1 
                         , 
                         i 
                       
                     
                   
                   = 
                   
                     
                       
                         
                           
                             V 
                             _ 
                           
                           
                             Sp 
                             k 
                           
                           i 
                         
                         · 
                         
                           
                             v 
                             _ 
                           
                           
                             r 
                             
                               1 
                                
                               
                                   
                               
                                
                               k 
                             
                           
                         
                       
                       
                          
                         
                           
                             V 
                             _ 
                           
                           
                             Sp 
                             k 
                           
                           i 
                         
                          
                       
                     
                     = 
                   
                 
               
               
                 
                   ( 
                   15 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       
                         
                           n 
                           
                             x 
                              
                             
                                 
                             
                              
                             
                               1 
                               k 
                             
                           
                         
                          
                         
                           ( 
                           
                             
                               x 
                               i 
                             
                             - 
                             
                               x 
                               
                                 s 
                                 k 
                               
                             
                           
                           ) 
                         
                       
                       + 
                       
                         
                           n 
                           
                             y 
                              
                             
                                 
                             
                              
                             
                               1 
                               k 
                             
                           
                         
                          
                         
                           ( 
                           
                             
                               y 
                               i 
                             
                             - 
                             
                               y 
                               
                                 s 
                                 k 
                               
                             
                           
                           ) 
                         
                       
                       + 
                       
                         
                           n 
                           
                             z 
                              
                             
                                 
                             
                              
                             
                               1 
                               k 
                             
                           
                         
                          
                         
                           ( 
                           
                             
                               z 
                               i 
                             
                             - 
                             
                               z 
                               
                                 s 
                                 k 
                               
                             
                           
                           ) 
                         
                       
                     
                     
                       
                         
                           
                             ( 
                             
                               
                                 x 
                                 i 
                               
                               - 
                               
                                 x 
                                 
                                   s 
                                   k 
                                 
                               
                             
                             ) 
                           
                           2 
                         
                         + 
                         
                           
                             ( 
                             
                               
                                 y 
                                 i 
                               
                               - 
                               
                                 y 
                                 
                                   s 
                                   k 
                                 
                               
                             
                             ) 
                           
                           2 
                         
                         + 
                         
                           
                             ( 
                             
                               
                                 z 
                                 i 
                               
                               - 
                               
                                 z 
                                 
                                   s 
                                   k 
                                 
                               
                             
                             ) 
                           
                           2 
                         
                       
                     
                   
                    
                   
                     
 
                   
                    
                   and 
                 
               
               
                 
                   ( 
                   16 
                   ) 
                 
               
             
             
               
                 
                   
                     Δ 
                      
                     
                         
                     
                      
                     
                       f 
                       
                         D 
                         k 
                       
                       
                         2 
                         , 
                         i 
                       
                     
                   
                   = 
                   
                     
                       
                         
                           
                             V 
                             _ 
                           
                           
                             Sp 
                             k 
                           
                           i 
                         
                         · 
                         
                           
                             v 
                             _ 
                           
                           
                             r 
                             
                               2 
                                
                               
                                   
                               
                                
                               k 
                             
                           
                         
                       
                       
                          
                         
                           
                             V 
                             _ 
                           
                           
                             Sp 
                             k 
                           
                           i 
                         
                          
                       
                     
                     = 
                   
                 
               
               
                 
                   ( 
                   17 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       
                         
                           n 
                           
                             x 
                              
                             
                                 
                             
                              
                             
                               2 
                               k 
                             
                           
                         
                          
                         
                           ( 
                           
                             
                               x 
                               i 
                             
                             - 
                             
                               x 
                               
                                 s 
                                 k 
                               
                             
                           
                           ) 
                         
                       
                       + 
                       
                         
                           n 
                           
                             y 
                              
                             
                                 
                             
                              
                             
                               2 
                               k 
                             
                           
                         
                          
                         
                           ( 
                           
                             
                               y 
                               i 
                             
                             - 
                             
                               y 
                               
                                 s 
                                 k 
                               
                             
                           
                           ) 
                         
                       
                       + 
                       
                         
                           n 
                           
                             z 
                              
                             
                                 
                             
                              
                             
                               2 
                               k 
                             
                           
                         
                          
                         
                           ( 
                           
                             
                               z 
                               i 
                             
                             - 
                             
                               z 
                               
                                 s 
                                 k 
                               
                             
                           
                           ) 
                         
                       
                     
                     
                       
                         
                           
                             ( 
                             
                               
                                 x 
                                 i 
                               
                               - 
                               
                                 x 
                                 
                                   s 
                                   k 
                                 
                               
                             
                             ) 
                           
                           2 
                         
                         + 
                         
                           
                             ( 
                             
                               
                                 y 
                                 i 
                               
                               - 
                               
                                 y 
                                 
                                   s 
                                   k 
                                 
                               
                             
                             ) 
                           
                           2 
                         
                         + 
                         
                           
                             ( 
                             
                               
                                 z 
                                 i 
                               
                               - 
                               
                                 z 
                                 
                                   s 
                                   k 
                                 
                               
                             
                             ) 
                           
                           2 
                         
                       
                     
                   
                   . 
                 
               
               
                 
                   ( 
                   18 
                   ) 
                 
               
             
           
         
       
     
         [0038]    A Doppler difference ratio at t=k may be derived as 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       Δ 
                        
                       
                           
                       
                        
                       
                         f 
                         
                           D 
                           k 
                         
                         
                           1 
                           , 
                           i 
                         
                       
                     
                     
                       Δ 
                        
                       
                           
                       
                        
                       
                         f 
                         
                           D 
                           k 
                         
                         
                           2 
                           , 
                           i 
                         
                       
                     
                   
                   = 
                   
                     
                       
                         
                           n 
                           
                             x 
                              
                             
                                 
                             
                              
                             
                               1 
                               k 
                             
                           
                         
                          
                         
                           ( 
                           
                             
                               x 
                               i 
                             
                             - 
                             
                               x 
                               
                                 s 
                                 k 
                               
                             
                           
                           ) 
                         
                       
                       + 
                       
                         
                           n 
                           
                             y 
                              
                             
                                 
                             
                              
                             
                               1 
                               k 
                             
                           
                         
                          
                         
                           ( 
                           
                             
                               y 
                               i 
                             
                             - 
                             
                               y 
                               
                                 s 
                                 k 
                               
                             
                           
                           ) 
                         
                       
                       + 
                       
                         
                           n 
                           
                             z 
                              
                             
                                 
                             
                              
                             
                               1 
                               k 
                             
                           
                         
                          
                         
                           ( 
                           
                             
                               z 
                               i 
                             
                             - 
                             
                               z 
                               
                                 s 
                                 k 
                               
                             
                           
                           ) 
                         
                       
                     
                     
                       
                         
                           n 
                           
                             x 
                              
                             
                                 
                             
                              
                             
                               2 
                               k 
                             
                           
                         
                          
                         
                           ( 
                           
                             
                               x 
                               i 
                             
                             - 
                             
                               x 
                               
                                 s 
                                 k 
                               
                             
                           
                           ) 
                         
                       
                       + 
                       
                         
                           n 
                           
                             y 
                              
                             
                                 
                             
                              
                             
                               2 
                               k 
                             
                           
                         
                          
                         
                           ( 
                           
                             
                               y 
                               i 
                             
                             - 
                             
                               y 
                               
                                 s 
                                 k 
                               
                             
                           
                           ) 
                         
                       
                       + 
                       
                         
                           n 
                           
                             z 
                              
                             
                                 
                             
                              
                             
                               2 
                               k 
                             
                           
                         
                          
                         
                           ( 
                           
                             
                               z 
                               i 
                             
                             - 
                             
                               z 
                               
                                 s 
                                 k 
                               
                             
                           
                           ) 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   19 
                   ) 
                 
               
             
           
         
       
     
         [0000]    which may be a 3D line passing through scene point p i . Three 3D lines intersecting at a same point are needed to uniquely determine a 3D point. 
         [0039]    5. Calculate Doppler Difference and Ratio for (k+1)th Frame 
         [0040]    Now in steps  619 ,  621 ,  623  the sensor apparatus  1  moves to a new position  S   k+1  and takes an image at t=k+1. Point  q   k   i , as determined in step  614 , becomes  q   k+1   i  and the correspondence is established by a feature matching operator in step  618 . Let  q   k+1   i =[u k−1   i , ν k+1   i ]. The Doppler angles passing  q   k+1   i  are determined as 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         
                           
                             θ 
                             
                               k 
                               + 
                               1 
                             
                             
                               1 
                               , 
                               i 
                             
                           
                           = 
                             
                            
                           
                             
                               cos 
                               
                                 - 
                                 1 
                               
                             
                              
                             
                               
                                 
                                   
                                     V 
                                     _ 
                                   
                                   
                                     q 
                                     
                                       k 
                                       + 
                                       1 
                                     
                                   
                                   i 
                                 
                                 · 
                                 
                                   
                                     v 
                                     _ 
                                   
                                   
                                     r 
                                     
                                       
                                         1 
                                          
                                         
                                             
                                         
                                          
                                         k 
                                       
                                       + 
                                       1 
                                     
                                   
                                 
                               
                               
                                 
                                    
                                   
                                     
                                       V 
                                       _ 
                                     
                                     
                                       q 
                                       
                                         k 
                                         + 
                                         1 
                                       
                                     
                                     i 
                                   
                                    
                                 
                                  
                                 
                                    
                                   
                                     
                                       v 
                                       _ 
                                     
                                     
                                       r 
                                       
                                         
                                           1 
                                            
                                           
                                               
                                           
                                            
                                           k 
                                         
                                         + 
                                         1 
                                       
                                     
                                   
                                    
                                 
                               
                             
                           
                         
                       
                     
                     
                       
                         
                           = 
                             
                            
                           
                             
                               cos 
                               
                                 - 
                                 1 
                               
                             
                              
                             
                               
                                 
                                   
                                     n 
                                     
                                       x 
                                        
                                       
                                           
                                       
                                        
                                       
                                         1 
                                         
                                           k 
                                           + 
                                           1 
                                         
                                       
                                     
                                   
                                    
                                   
                                     u 
                                     
                                       k 
                                       + 
                                       1 
                                     
                                     i 
                                   
                                 
                                 + 
                                 
                                   
                                     n 
                                     
                                       y 
                                        
                                       
                                           
                                       
                                        
                                       
                                         1 
                                         
                                           k 
                                           + 
                                           1 
                                         
                                       
                                     
                                   
                                    
                                   
                                     v 
                                     
                                       k 
                                       + 
                                       1 
                                     
                                     i 
                                   
                                 
                                 + 
                                 
                                   
                                     n 
                                     
                                       z 
                                        
                                       
                                           
                                       
                                        
                                       
                                         1 
                                         
                                           k 
                                           + 
                                           1 
                                         
                                       
                                     
                                   
                                    
                                   f 
                                 
                               
                               
                                 
                                   
                                     
                                       ( 
                                       
                                         u 
                                         
                                           k 
                                           + 
                                           1 
                                         
                                         i 
                                       
                                       ) 
                                     
                                     2 
                                   
                                   + 
                                   
                                     
                                       ( 
                                       
                                         v 
                                         
                                           k 
                                           + 
                                           1 
                                         
                                         i 
                                       
                                       ) 
                                     
                                     2 
                                   
                                   + 
                                   
                                     f 
                                     2 
                                   
                                 
                               
                             
                           
                         
                       
                     
                   
                    
                   
                     
 
                   
                    
                   and 
                 
               
               
                 
                   ( 
                   20 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       
                         
                           θ 
                           
                             k 
                             + 
                             1 
                           
                           
                             2 
                             , 
                             i 
                           
                         
                         = 
                           
                          
                         
                           
                             cos 
                             
                               - 
                               1 
                             
                           
                            
                           
                             
                               
                                 
                                   V 
                                   _ 
                                 
                                 
                                   q 
                                   
                                     k 
                                     + 
                                     1 
                                   
                                 
                                 i 
                               
                               · 
                               
                                 
                                   v 
                                   _ 
                                 
                                 
                                   r 
                                   
                                     
                                       2 
                                        
                                       
                                           
                                       
                                        
                                       k 
                                     
                                     + 
                                     1 
                                   
                                 
                               
                             
                             
                               
                                  
                                 
                                   
                                     V 
                                     _ 
                                   
                                   
                                     q 
                                     
                                       k 
                                       + 
                                       1 
                                     
                                   
                                   i 
                                 
                                  
                               
                                
                               
                                  
                                 
                                   
                                     v 
                                     _ 
                                   
                                   
                                     r 
                                     
                                       
                                         2 
                                          
                                         
                                             
                                         
                                          
                                         k 
                                       
                                       + 
                                       1 
                                     
                                   
                                 
                                  
                               
                             
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                           
                          
                         
                           
                             cos 
                             
                               - 
                               1 
                             
                           
                            
                           
                             
                               
                                 
                                   n 
                                   
                                     x 
                                      
                                     
                                         
                                     
                                      
                                     
                                       2 
                                       
                                         k 
                                         + 
                                         1 
                                       
                                     
                                   
                                 
                                  
                                 
                                   u 
                                   
                                     k 
                                     + 
                                     1 
                                   
                                   i 
                                 
                               
                               + 
                               
                                 
                                   n 
                                   
                                     y 
                                      
                                     
                                         
                                     
                                      
                                     
                                       2 
                                       
                                         k 
                                         + 
                                         1 
                                       
                                     
                                   
                                 
                                  
                                 
                                   v 
                                   
                                     k 
                                     + 
                                     1 
                                   
                                   i 
                                 
                               
                               + 
                               
                                 
                                   n 
                                   
                                     z 
                                      
                                     
                                         
                                     
                                      
                                     
                                       2 
                                       
                                         k 
                                         + 
                                         1 
                                       
                                     
                                   
                                 
                                  
                                 f 
                               
                             
                             
                               
                                 
                                   
                                     ( 
                                     
                                       u 
                                       
                                         k 
                                         + 
                                         1 
                                       
                                       i 
                                     
                                     ) 
                                   
                                   2 
                                 
                                 + 
                                 
                                   
                                     ( 
                                     
                                       v 
                                       
                                         k 
                                         + 
                                         1 
                                       
                                       i 
                                     
                                     ) 
                                   
                                   2 
                                 
                                 + 
                                 
                                   f 
                                   2 
                                 
                               
                             
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   21 
                   ) 
                 
               
             
           
         
       
     
         [0000]    where f is the camera focus length and  V   q     k+1     i =[u k+1   i , ν k+1   i , f] T  is the vector pointing from  S   k+1  (the origin of camera coordinate) through  q   k+1   i  and to  p   i . Then, we have 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         
                           
                             Δ 
                              
                             
                                 
                             
                              
                             
                               f 
                               
                                 D 
                                 
                                   k 
                                   + 
                                   1 
                                 
                               
                               
                                 1 
                                 , 
                                 i 
                               
                             
                           
                           = 
                             
                            
                           
                             
                               v 
                               
                                 r 
                                 
                                   k 
                                   + 
                                   1 
                                 
                               
                             
                              
                             
                               cos 
                                
                               
                                 ( 
                                 
                                   θ 
                                   
                                     k 
                                     + 
                                     1 
                                   
                                   
                                     
                                       
                                         K 
                                         a 
                                       
                                        
                                       1 
                                     
                                     , 
                                     i 
                                   
                                 
                                 ) 
                               
                             
                           
                         
                       
                     
                     
                       
                         
                           = 
                             
                            
                           
                             
                               
                                 v 
                                 
                                   r 
                                   
                                     k 
                                     + 
                                     1 
                                   
                                 
                               
                                
                               
                                 [ 
                                 
                                   
                                     
                                       
                                         
                                           
                                             n 
                                             
                                               x 
                                                
                                               
                                                   
                                               
                                                
                                               
                                                 1 
                                                 
                                                   k 
                                                   + 
                                                   1 
                                                 
                                               
                                             
                                           
                                            
                                           
                                             ( 
                                             
                                               
                                                 x 
                                                 i 
                                               
                                               - 
                                               
                                                 x 
                                                 
                                                   s 
                                                   
                                                     k 
                                                     + 
                                                     1 
                                                   
                                                 
                                               
                                             
                                             ) 
                                           
                                         
                                         + 
                                         
                                           
                                             n 
                                             
                                               y 
                                                
                                               
                                                   
                                               
                                                
                                               
                                                 1 
                                                 
                                                   k 
                                                   + 
                                                   1 
                                                 
                                               
                                             
                                           
                                            
                                           
                                             ( 
                                             
                                               
                                                 y 
                                                 i 
                                               
                                               - 
                                               
                                                 y 
                                                 
                                                   s 
                                                   
                                                     k 
                                                     + 
                                                     1 
                                                   
                                                 
                                               
                                             
                                             ) 
                                           
                                         
                                         + 
                                       
                                     
                                   
                                   
                                     
                                       
                                         
                                           n 
                                           
                                             z 
                                              
                                             
                                                 
                                             
                                              
                                             
                                               1 
                                               
                                                 k 
                                                 + 
                                                 1 
                                               
                                             
                                           
                                         
                                          
                                         
                                           ( 
                                           
                                             
                                               z 
                                               i 
                                             
                                             - 
                                             
                                               z 
                                               
                                                 s 
                                                 
                                                   k 
                                                   + 
                                                   1 
                                                 
                                               
                                             
                                           
                                           ) 
                                         
                                       
                                     
                                   
                                 
                                 ] 
                               
                             
                             
                               
                                 
                                   
                                     ( 
                                     
                                       
                                         x 
                                         i 
                                       
                                       - 
                                       
                                         x 
                                         
                                           s 
                                           
                                             k 
                                             + 
                                             1 
                                           
                                         
                                       
                                     
                                     ) 
                                   
                                   2 
                                 
                                 + 
                                 
                                   
                                     ( 
                                     
                                       
                                         y 
                                         i 
                                       
                                       - 
                                       
                                         y 
                                         
                                           s 
                                           
                                             k 
                                             + 
                                             1 
                                           
                                         
                                       
                                     
                                     ) 
                                   
                                   2 
                                 
                                 + 
                                 
                                   
                                     ( 
                                     
                                       
                                         z 
                                         i 
                                       
                                       - 
                                       
                                         z 
                                         
                                           s 
                                           
                                             k 
                                             + 
                                             1 
                                           
                                         
                                       
                                     
                                     ) 
                                   
                                   2 
                                 
                               
                             
                           
                         
                       
                     
                   
                    
                   
                     
 
                   
                    
                   and 
                 
               
               
                 
                   ( 
                   22 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       
                         
                           Δ 
                            
                           
                               
                           
                            
                           
                             f 
                             
                               D 
                               
                                 k 
                                 + 
                                 1 
                               
                             
                             
                               2 
                               , 
                               i 
                             
                           
                         
                         = 
                           
                          
                         
                           
                             v 
                             
                               r 
                               
                                 k 
                                 + 
                                 1 
                               
                             
                           
                            
                           
                             cos 
                              
                             
                               ( 
                               
                                 θ 
                                 
                                   k 
                                   + 
                                   1 
                                 
                                 
                                   
                                     
                                       K 
                                       a 
                                     
                                      
                                     2 
                                   
                                   , 
                                   i 
                                 
                               
                               ) 
                             
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                           
                          
                         
                           
                             
                               
                                 v 
                                 
                                   r 
                                   
                                     k 
                                     + 
                                     1 
                                   
                                 
                               
                                
                               
                                 [ 
                                 
                                   
                                     
                                       
                                         
                                           
                                             n 
                                             
                                               x 
                                                
                                               
                                                   
                                               
                                                
                                               
                                                 2 
                                                 
                                                   k 
                                                   + 
                                                   1 
                                                 
                                               
                                             
                                           
                                            
                                           
                                             ( 
                                             
                                               
                                                 x 
                                                 i 
                                               
                                               - 
                                               
                                                 x 
                                                 
                                                   s 
                                                   
                                                     k 
                                                     + 
                                                     1 
                                                   
                                                 
                                               
                                             
                                             ) 
                                           
                                         
                                         + 
                                         
                                           
                                             n 
                                             
                                               y 
                                                
                                               
                                                   
                                               
                                                
                                               
                                                 2 
                                                 
                                                   k 
                                                   + 
                                                   1 
                                                 
                                               
                                             
                                           
                                            
                                           
                                             ( 
                                             
                                               
                                                 y 
                                                 i 
                                               
                                               - 
                                               
                                                 y 
                                                 
                                                   s 
                                                   
                                                     k 
                                                     + 
                                                     1 
                                                   
                                                 
                                               
                                             
                                             ) 
                                           
                                         
                                         + 
                                       
                                     
                                   
                                   
                                     
                                       
                                         
                                           n 
                                           
                                             z 
                                              
                                             
                                                 
                                             
                                              
                                             
                                               2 
                                               
                                                 k 
                                                 + 
                                                 1 
                                               
                                             
                                           
                                         
                                          
                                         
                                           ( 
                                           
                                             
                                               z 
                                               i 
                                             
                                             - 
                                             
                                               z 
                                               
                                                 s 
                                                 
                                                   k 
                                                   + 
                                                   1 
                                                 
                                               
                                             
                                           
                                           ) 
                                         
                                       
                                     
                                   
                                 
                                 ] 
                               
                             
                             
                               
                                 
                                   
                                     ( 
                                     
                                       
                                         x 
                                         i 
                                       
                                       - 
                                       
                                         x 
                                         
                                           s 
                                           
                                             k 
                                             + 
                                             1 
                                           
                                         
                                       
                                     
                                     ) 
                                   
                                   2 
                                 
                                 + 
                                 
                                   
                                     ( 
                                     
                                       
                                         y 
                                         i 
                                       
                                       - 
                                       
                                         y 
                                         
                                           s 
                                           
                                             k 
                                             + 
                                             1 
                                           
                                         
                                       
                                     
                                     ) 
                                   
                                   2 
                                 
                                 + 
                                 
                                   
                                     ( 
                                     
                                       
                                         z 
                                         i 
                                       
                                       - 
                                       
                                         z 
                                         
                                           s 
                                           
                                             k 
                                             + 
                                             1 
                                           
                                         
                                       
                                     
                                     ) 
                                   
                                   2 
                                 
                               
                             
                           
                           . 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   23 
                   ) 
                 
               
             
           
         
       
     
         [0041]    The second 3D line equation may also be derived as 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       Δ 
                        
                       
                           
                       
                        
                       
                         f 
                         
                           D 
                           
                             k 
                             + 
                             1 
                           
                         
                         
                           1 
                           , 
                           i 
                         
                       
                     
                     
                       Δ 
                        
                       
                           
                       
                        
                       
                         f 
                         
                           D 
                           
                             k 
                             + 
                             1 
                           
                         
                         
                           2 
                           , 
                           i 
                         
                       
                     
                   
                   = 
                   
                     
                       
                         
                           
                             
                               
                                 
                                   n 
                                   
                                     x 
                                      
                                     
                                         
                                     
                                      
                                     
                                       1 
                                       
                                         k 
                                         + 
                                         1 
                                       
                                     
                                   
                                 
                                  
                                 
                                   ( 
                                   
                                     
                                       x 
                                       i 
                                     
                                     - 
                                     
                                       x 
                                       
                                         s 
                                         
                                           k 
                                           + 
                                           1 
                                         
                                       
                                     
                                   
                                   ) 
                                 
                               
                               + 
                               
                                 
                                   n 
                                   
                                     y 
                                      
                                     
                                         
                                     
                                      
                                     
                                       1 
                                       k 
                                     
                                   
                                 
                                  
                                 
                                   ( 
                                   
                                     
                                       y 
                                       i 
                                     
                                     - 
                                     
                                       y 
                                       
                                         s 
                                         
                                           k 
                                           + 
                                           1 
                                         
                                       
                                     
                                   
                                   ) 
                                 
                               
                               + 
                             
                           
                         
                         
                           
                             
                               
                                 n 
                                 
                                   z 
                                    
                                   
                                       
                                   
                                    
                                   
                                     1 
                                     
                                       k 
                                       + 
                                       1 
                                     
                                   
                                 
                               
                                
                               
                                 ( 
                                 
                                   
                                     z 
                                     i 
                                   
                                   - 
                                   
                                     z 
                                     
                                       s 
                                       
                                         k 
                                         + 
                                         1 
                                       
                                     
                                   
                                 
                                 ) 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 
                                   n 
                                   
                                     x 
                                      
                                     
                                         
                                     
                                      
                                     
                                       2 
                                       
                                         k 
                                         + 
                                         1 
                                       
                                     
                                   
                                 
                                  
                                 
                                   ( 
                                   
                                     
                                       x 
                                       i 
                                     
                                     - 
                                     
                                       x 
                                       
                                         s 
                                         
                                           k 
                                           + 
                                           1 
                                         
                                       
                                     
                                   
                                   ) 
                                 
                               
                               + 
                               
                                 
                                   n 
                                   
                                     y 
                                      
                                     
                                         
                                     
                                      
                                     
                                       2 
                                       
                                         k 
                                         + 
                                         1 
                                       
                                     
                                   
                                 
                                  
                                 
                                   ( 
                                   
                                     
                                       y 
                                       i 
                                     
                                     - 
                                     
                                       y 
                                       
                                         s 
                                         
                                           k 
                                           + 
                                           1 
                                         
                                       
                                     
                                   
                                   ) 
                                 
                               
                               + 
                             
                           
                         
                         
                           
                             
                               
                                 n 
                                 
                                   z 
                                    
                                   
                                       
                                   
                                    
                                   
                                     2 
                                     
                                       k 
                                       + 
                                       1 
                                     
                                   
                                 
                               
                                
                               
                                 ( 
                                 
                                   
                                     z 
                                     i 
                                   
                                   - 
                                   
                                     z 
                                     
                                       s 
                                       
                                         k 
                                         + 
                                         1 
                                       
                                     
                                   
                                 
                                 ) 
                               
                             
                           
                         
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   24 
                   ) 
                 
               
             
           
         
       
     
         [0042]    6. Calculate Doppler Difference and Ratio for (k+2)th Frame 
         [0043]    Repeating procedure in steps  622 ,  625 , 627 , 629  of section 5 at t=k+2, we will obtain the third 3D line equation as 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       Δ 
                        
                       
                           
                       
                        
                       
                         f 
                         
                           D 
                           
                             k 
                             + 
                             2 
                           
                         
                         
                           1 
                           , 
                           i 
                         
                       
                     
                     
                       Δ 
                        
                       
                           
                       
                        
                       
                         f 
                         
                           D 
                           
                             k 
                             + 
                             2 
                           
                         
                         
                           2 
                           , 
                           i 
                         
                       
                     
                   
                   = 
                   
                     
                       
                         
                           
                             
                               
                                 
                                   n 
                                   
                                     x 
                                      
                                     
                                         
                                     
                                      
                                     
                                       1 
                                       
                                         k 
                                         + 
                                         2 
                                       
                                     
                                   
                                 
                                  
                                 
                                   ( 
                                   
                                     
                                       x 
                                       i 
                                     
                                     - 
                                     
                                       x 
                                       
                                         s 
                                         
                                           k 
                                           + 
                                           2 
                                         
                                       
                                     
                                   
                                   ) 
                                 
                               
                               + 
                               
                                 
                                   n 
                                   
                                     y 
                                      
                                     
                                         
                                     
                                      
                                     
                                       1 
                                       k 
                                     
                                   
                                 
                                  
                                 
                                   ( 
                                   
                                     
                                       y 
                                       i 
                                     
                                     - 
                                     
                                       y 
                                       
                                         s 
                                         
                                           k 
                                           + 
                                           2 
                                         
                                       
                                     
                                   
                                   ) 
                                 
                               
                               + 
                             
                           
                         
                         
                           
                             
                               
                                 n 
                                 
                                   z 
                                    
                                   
                                       
                                   
                                    
                                   
                                     1 
                                     
                                       k 
                                       + 
                                       2 
                                     
                                   
                                 
                               
                                
                               
                                 ( 
                                 
                                   
                                     z 
                                     i 
                                   
                                   - 
                                   
                                     z 
                                     
                                       s 
                                       
                                         k 
                                         + 
                                         2 
                                       
                                     
                                   
                                 
                                 ) 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 
                                   n 
                                   
                                     x 
                                      
                                     
                                         
                                     
                                      
                                     
                                       2 
                                       
                                         k 
                                         + 
                                         2 
                                       
                                     
                                   
                                 
                                  
                                 
                                   ( 
                                   
                                     
                                       x 
                                       i 
                                     
                                     - 
                                     
                                       x 
                                       
                                         s 
                                         
                                           k 
                                           + 
                                           2 
                                         
                                       
                                     
                                   
                                   ) 
                                 
                               
                               + 
                               
                                 
                                   n 
                                   
                                     y 
                                      
                                     
                                         
                                     
                                      
                                     
                                       2 
                                       
                                         k 
                                         + 
                                         2 
                                       
                                     
                                   
                                 
                                  
                                 
                                   ( 
                                   
                                     
                                       y 
                                       i 
                                     
                                     - 
                                     
                                       y 
                                       
                                         s 
                                         
                                           k 
                                           + 
                                           2 
                                         
                                       
                                     
                                   
                                   ) 
                                 
                               
                               + 
                             
                           
                         
                         
                           
                             
                               
                                 n 
                                 
                                   z 
                                    
                                   
                                       
                                   
                                    
                                   
                                     2 
                                     
                                       k 
                                       + 
                                       2 
                                     
                                   
                                 
                               
                                
                               
                                 ( 
                                 
                                   
                                     z 
                                     i 
                                   
                                   - 
                                   
                                     z 
                                     
                                       s 
                                       
                                         k 
                                         + 
                                         2 
                                       
                                     
                                   
                                 
                                 ) 
                               
                             
                           
                         
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   25 
                   ) 
                 
               
             
           
         
       
     
         [0044]    7. 3D Scene Calculation 
         [0045]    Solving Eqs. (19), (24) and (25) gives us 3D information of scene point p i  in step  631 . A collection of 3D scene point  9  may comprise the 3D scene. The above process may be repeated any number of times. 
         [0046]    While the invention is susceptible to various modifications and alternative forms, specific embodiments thereof have been shown by way of example in the drawings and are herein described in detail. It should be understood, however, that the description herein of specific embodiments is not intended to limit the invention to the particular forms disclosed.