Abstract:
A system and method is provided for solving the AX=XB calibration problem. A calibration method is presented to determine the unknown X in the AX=XB calibration problem. Sensor data is filtered, such that data without the desired screw theory invariants are discarded. The correspondence between A and B is then computed, either through a probabilistic Batch method that treats the data streams as probability density functions, or by formulating the data streams as a time-evolving differential equation which allows for online calibration of the device. Also, a calibration phantom and software is also provided. The phantom is an extension of known Z-fiducial phantoms, in which the Z-fiducials are oriented based on consideration of imaging physics. An additional phantom is designed that does not utilize rods within the phantom to perform the calibration.

Description:
CROSS-REFERENCE TO RELATED APPLICATIONS 
       [0001]    This application is based on, claims priority to, and incorporates herein by reference U.S. Provisional Application Ser. No. 61/870,385, filed Aug. 27, 2013, and entitled “Apparatus and Method for the AX=XB Calibration Problem.” 
     
    
     BACKGROUND 
       [0002]    The present disclosure relates to medical imaging and, more particularly, to calibration of an ultrasound system or other imaging modality by solving for the calibration parameter, X, between A and B in the AX=XB calibration problem for noisy and temporally un-synced data. 
         [0003]    Image-guided surgery systems are often used to provide surgeons with informational support. Due to several unique advantages, such as the absence of ionizing radiation, ease of use, and real-time imaging, ultrasound (US) is a common medical imaging modality used in image-guided surgery systems. To perform advanced forms of guidance with ultrasound, such as virtual image overlays or automated robotic actuation, an ultrasound calibration process must be performed. This process recovers the rigid body transformation between a tracked marker attached to the ultrasound transducer and the ultrasound image. A phantom or model with known geometry is also required. 
         [0004]    There have been many different categories of phantoms or models used for ultrasound calibration including wall, cross-wire, Z-fiducial, and AX=XB phantoms. Z-fiducial phantoms are comprised of three wires, or rods, which are oriented in a plane to form a Z or an N shape. Given a plane intersecting all three wires of multiple Z-fiducials, the unique positioning of the plane can be determined. AX=XB phantoms are those that allow the relative rigid body transformation between two images to be recovered based on each image&#39;s content. 
         [0005]    When utilizing AX=XB phantoms, a sensor calibration problem known as the AX=XB problem arises. In this problem, A, X, and B are homogeneous transformations, with A and B given from sensor measurements, and X is unknown. To solve for X, two compatible instances of independent exact measurements, (A 1 , B 1 ) and (A 2 , B 2 ) can be used. However, sensor data often features asynchronous timing, differing sampling rates, dropped sensor readings, and/or noise that cause the relationship between A&#39;s and B&#39;s to be damaged. 
         [0006]    It would therefore be desirable to have a sensor data collection apparatus (phantom) specifically designed to address constraints in ultrasound physics and image processing as well as a method to solve the AX=XB problem when the correspondence (temporal matching) between A&#39;s and B&#39;s is unknown and the data is noisy. 
       SUMMARY OF THE INVENTION 
       [0007]    The present disclosure overcomes the aforementioned drawbacks by providing both a method and an apparatus for solving the AX=XB calibration problem. A calibration method is presented to determine the unknown X in the AX=XB calibration problem. Sensor data is filtered, such that data without the desired screw theory invariants are discarded. The correspondence between A and B is then corrected, either through a probabilistic Batch method that treats the data streams as probability density functions (and therefore does not require correspondence), or by using the information given by the invariants to recreate the corresponded. The calibration parameter, X, can be found by solving the Batch equations or by formulating the data streams as a time-evolving differential equation which allows for online calibration of the device. Also, a calibration phantom and software is provided. The phantom is an extension of known Z-fiducial phantoms, in which the fiducials are oriented based on consideration of imaging physics. A further evolved extension of this phantom is also presented that does not utilize rod fiducials within the phantom to perform the calibration. 
         [0008]    In one aspect of the disclosure, a system for calibrating an imaging system is described. The system includes an input to receive data acquired by at least one sensor of an imaging system, an input configured to receive data acquired by at least one sensor of a tracking system, and a processor to receive the data from the inputs. The processor uses the data to populate a calibration model having a form of AX=BX, wherein A, X, and B are homogeneous transformations with A and B determined from the data and X being an unknown calibration parameter, filters the data using screw theory invariants, computes the calibration parameter, X, between A and B, uses solutions from Batch methods which do not require correspondence of the input data, and finds correspondence using the invariants and finds the solution from a time-evolving method which allows real time computation and update. 
         [0009]    Another aspect of the disclosure provides a phantom for calibration of an imaging system that includes a frame. The frame includes first and second opposing walls and a base with a first edge and a second edge. The first edge is joined to an edge of the first opposing wall and the second edge is joined to an edge of the second opposing wall. The phantom also includes a plurality of rods spaced between the first and second opposing walls. The rods lie in non-parallel planes. 
         [0010]    In accordance with another aspect of the disclosure, a method is provided for calibration of an imaging system. The method includes providing a phantom and placing an imaging system in a first position relative to the phantom. The method also includes acquiring a first ultrasound image of the phantom, determining a spatial relationship between the phantom and the first image, and repositioning the imaging system in a second position relative to the phantom. The method further includes acquiring a second ultrasound image of the phantom, determining a second spatial relationship between the phantom and the second image, and relaying the first and second spatial relationships to a processor. The method also includes processing the spatial relationship data by using the data to populate a calibration model having a form of AX=BX, wherein A, X, and B are homogeneous transformations with A and B determined from the data and X being an unknown. Processing also includes filtering the data using a screw theory, disregarding portions of the data without desired screw theory invariants, and computing an unknown transformation between the imaging system and the phantom. 
         [0011]    In accordance with yet another aspect of the disclosure, a phantom is provided for calibration of an imaging system. The phantom includes first and second opposing walls and a base with a first edge and a second edge. The first edge is joined to an edge of the first opposing wall and the second edge is joined to an edge of the second opposing wall. The phantom also includes a cover portion opposing the base, the cover portion having a first edge joined to an edge of the first opposing wall and a second edge joined to an edge of the second opposing wall and a plurality of apertures through the cover portion sized to receive a plurality of sensors. 
         [0012]    The foregoing, and other aspects and advantages of the invention, will appear from the following description. In the description, reference is made to the accompanying drawings that form a part hereof, and in which there is shown by way of illustration a preferred embodiment of the invention. Such embodiment does not necessarily represent the full scope of the invention, however, and reference is made therefore to the claims and herein for interpreting the scope of the invention. 
     
    
     
       BRIEF DESCRIPTION OF THE DRAWINGS 
         [0013]      FIG. 1  is a graphical representation of two rigid body motions modeled in accordance with the present disclosure. 
           [0014]      FIG. 2  is a pair of graphs of shifted data streams for theta invariants and their correlation prepared in accordance with the present disclosure. 
           [0015]      FIG. 3  is an illustrative representation of the calibration system in accordance with the present disclosure. 
           [0016]      FIG. 4A  is a representation of a previous calibration phantom model. 
           [0017]      FIG. 4B  is a representation of an updated calibration phantom. 
           [0018]      FIG. 5  is a schematic illustration of a system and method of using the system in accordance with the present disclosure. 
           [0019]      FIG. 6  is graphic illustration showing known points in a reference image and an affine transformation of these points, determined with image registration and/or tracking methods, to determine corresponding points in a new image. 
       
    
    
     DETAILED DESCRIPTION OF THE INVENTION 
       [0020]    The present disclosure provides a system and method for calibrating a medical imaging device. With the method of the present disclosure, calibration can be performed without knowing the correspondence between pairs of measurements that come from cameras, ultrasound probes, optical or magnetic pose tracking systems, and the like. Regardless of the specific device or constraints of a given modality, each imaging modality includes a “sensor” (be it, optical, magnetic, ultrasound, and the like) and, thus, the term “sensor” will be used herein to refer to a system or sub-system of a system that detects or collects data, such as cameras, ultrasound probes, optical or magnetic pose tracking systems, and the like. 
         [0021]    The sensor calibration problem, AX=XB, appears often in the fields of robotics and computer vision. The variables A, X and B are rigid-body motions, with each pair of measurements (A, B) coming from sensors, and X is the unknown rigid-body motion that is found when solving the problem. 
         [0022]    Any motion in Euclidean space can be described as a homogeneous transformation matrix of the form 
         [0000]    
       
         
           
             
               
                 
                   
                     H 
                      
                     
                       ( 
                       
                         R 
                         , 
                         t 
                       
                       ) 
                     
                   
                   = 
                   
                     ( 
                     
                       
                         
                           R 
                         
                         
                           t 
                         
                       
                       
                         
                           
                             0 
                             T 
                           
                         
                         
                           1 
                         
                       
                     
                     ) 
                   
                 
               
               
                 
                   ( 
                   1 
                   ) 
                 
               
             
           
         
       
     
         [0000]    where t is a translation vector and R is a rotation matrix. In the three-dimensional case t∈           3  and R is 3×3 and is an element of the SO(3) group of orthogonal matrices. Given multiple equations of the form 
         [0000]        A   i   X=XB   i             A   i   =XB   i   X   −1   (2)
 
         [0000]    a solution X∈SE(3) is sought in a wide variety of application. 
         [0023]    A method is needed to solve for X wherein there does not need to be a priori knowledge of the correspondence between A i  and B i . By viewing the two sets of reference frames A i  and B i  as probability densities ƒ A H and ƒ B H where H∈SE(3), and X as a probability density on SE(3) we can solve for X as the solution to a minimization problem where the cost is 
         [0000]        C ( X )= D   KL ƒ A ∥δ X *ƒ B *δ X-1   (3)
 
         [0000]    Where * denotes convolution on SE(3) and D KL •∥• is the Kullback-Leibler divergence for SE(3). 
         [0024]    The convolution of two probability density functions on SE(3) is defined as 
         [0000]      ƒ 1 *ƒ 2   H′=∫   SE(3) ƒ 1   Hƒ   2   H   −1   H′dH   (4)
 
         [0000]    where H, H′∈SE(3) with H serving as a dummy variable of integration for each value of H′ and dH is the natural (bi-variant) Riemannian volume element for SE(3). 
         [0025]    A Dirac delta function can be defined for SE(3) to have properties 
         [0000]      ∫ SE(3)   δHdH= 1 and ƒ*δ H=ƒI   4  
 
         [0000]    where            4 =H(           3 ,0) is the 4×4 identity matrix which is the identity element of SE(3). Intuitively a Dirac delta can be though of as a function that has a spike with infinite height at the identity and vanishes everywhere else. A shifted Dirac delta function can be defined as 
         [0000]      δ X   H=δX   −1   H  
 
         [0000]    which places the spike at X∈SE(3) and allows for equation (2) to be written as 
         [0000]      δ A1   H=δ   X *δ B1 *δ X-1   H   (5)
 
         [0000]    Because convolution is a linear operation on functions, we can write all n instances of 5) into a single equation of the form 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         f 
                         A 
                       
                        
                       H 
                     
                     = 
                     
                       
                         δ 
                         X 
                       
                       * 
                       
                         f 
                         B 
                       
                       * 
                       
                         δ 
                         
                           X 
                           
                             - 
                             1 
                           
                         
                       
                        
                       H 
                     
                   
                    
                   
                     
 
                   
                    
                   where 
                    
                   
                     
 
                   
                    
                   
                     
                       
                         f 
                         A 
                       
                        
                       H 
                     
                     = 
                     
                       
                         
                           1 
                           n 
                         
                          
                         
                           
                             ∑ 
                             
                               i 
                               = 
                               1 
                             
                             n 
                           
                            
                           
                               
                           
                            
                           
                             δ 
                              
                             
                                 
                             
                              
                             
                               A 
                               i 
                               
                                 - 
                                 1 
                               
                             
                              
                             H 
                              
                             
                                 
                             
                              
                             and 
                              
                             
                                 
                             
                              
                             
                               f 
                               B 
                             
                              
                             H 
                           
                         
                       
                       = 
                       
                         
                           1 
                           n 
                         
                          
                         
                           
                             ∑ 
                             
                               i 
                               = 
                               1 
                             
                             n 
                           
                            
                           
                               
                           
                            
                           
                             δ 
                              
                             
                                 
                             
                              
                             
                               B 
                               i 
                               
                                 - 
                                 1 
                               
                             
                              
                             
                               H 
                               . 
                             
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   6 
                   ) 
                 
               
             
           
         
       
     
         [0000]    When written in this form, it does not matter if the correspondence between A i  and B i  is known, as the above functions are normalized to be probability densities. 
         [0026]    Functions ƒ A (H) and ƒ B (H) are not in L 2 (SE(3)), but this can be solved if each delta function can be replaced with a Gaussian distribution or the two whole set of delta functions, which result from samples obtained at discrete times, are each replaced with Gaussians that have the same mean and covariance at teach of the sets. 
         [0027]    It is convenient to define the subset se &lt; (3)⊂se(3) which is a depleted version of SE(3) in which all screw rotations having an angle of π have been removed. The mean and covariance of a probability density ƒ(H) can be defined by the conditions 
         [0000]      ∫ SE(3)  log  M   −1   HƒHdh=               
 
         [0000]      and 
         [0000]      Σ∫ SE(3)  log               M   −1   H [ log           M   −1   H]   T   ƒHdh.   (7)
 
         [0028]    Explicit formulas defining the matrix exponential exp:se(3)→SE(3) and logarithm log:SE &lt; (3)→se(3) and log         :SE &lt; (3)∈           6  are reviewed below. The transpose on the second log log          H is defined to ensure that Σ is a 6×6 symmetric matrix. The operation log(H) takes any element in H∈SE &lt; (3) into the corresponding unique element in the Lie algebra SE(3) such that exp(log(H))=H. That is, the logarithm map is not surjective, unless we consider the target space to be se &lt; (3)⊂se(3), which can be though of as the Cartesian product of the open solid ball of radius π with            3 . Since SO(3) can be viewed as a solid closed three-dimensional ball of radius π with antipodal points identified, the exclusion of the bounding sphere of radius π in SE(3) defines a 5D set of measure zero that has no effect on the computation of Σ in the above equation. 
         [0029]    We can therefore write 
         [0000]    
       
         
           
             
               log 
                
               
                   
               
                
               H 
             
             = 
             
               ( 
               
                 
                   
                     Ω 
                   
                   
                     v 
                   
                 
                 
                   
                     
                       0 
                       T 
                     
                   
                   
                     0 
                   
                 
               
               ) 
             
           
         
       
     
         [0000]    where Ω=−Ω T ∈so(3). The map V: se 3→           6  is then composed with the log to give z=log          H=[ωT,νT]∈           6  where ω∈           3  is the vector corresponding to Ω such that Ωx=ω×x for any x∈           3 , where x is the vector cross product. 
         [0030]    If ƒ(H) is a sum of Dirac deltas like ƒ A (H) above, then 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         ∑ 
                         
                           i 
                           = 
                           1 
                         
                         n 
                       
                        
                       
                           
                       
                        
                       
                         log 
                          
                         
                             
                         
                          
                         
                           M 
                           A 
                           
                             - 
                             1 
                           
                         
                          
                         
                           A 
                           i 
                         
                       
                     
                     = 
                      
                   
                    
                   
                     
 
                   
                    
                   and 
                    
                   
                     
 
                   
                    
                   
                     
                       ∑ 
                       A 
                     
                      
                     
                       = 
                       
                         
                           1 
                           n 
                         
                          
                         
                           
                             ∑ 
                             
                               i 
                               = 
                               1 
                             
                             n 
                           
                            
                           
                               
                           
                            
                           
                             
                               log 
                                
                             
                              
                             
                                 
                             
                              
                             
                               M 
                               A 
                               
                                 - 
                                 1 
                               
                             
                              
                             
                               
                                 
                                   A 
                                   i 
                                 
                                  
                                 
                                   [ 
                                   
                                     
                                       log 
                                        
                                     
                                      
                                     
                                         
                                     
                                      
                                     
                                       M 
                                       A 
                                       
                                         - 
                                         1 
                                       
                                     
                                      
                                     
                                       A 
                                       i 
                                     
                                   
                                   ] 
                                 
                               
                               T 
                             
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   8 
                   ) 
                 
               
             
           
         
       
     
         [0031]    The adjoin matrix 
         [0000]    
       
         
           
             
               Ad 
                
               
                   
               
                
               H 
             
             = 
             
               ( 
               
                 
                   
                     R 
                   
                   
                      
                   
                 
                 
                   
                     tR 
                   
                   
                     R 
                   
                 
               
               ) 
             
           
         
       
     
         [0000]    is used heavily in computations, as it has the property that 
         [0000]      log           H   1   H   2   H   1   −1   =AdH   1  log           H   2 . 
         [0000]    Here for any a∈           3 , â is the skew-symmetric matrix such that ab=a×b.           is used as the reverse map which gives â         =a. 
         [0032]    Evaluating the mean and covariance defined in (7) with the functions in (6) using the bi-invariance of the integration measure gives 
         [0000]        M   A   =XM   B   X   −1   (9)
 
         [0000]      and 
         [0000]      Σ A   =ADXΣ   B   Ad   T   X   (10)
 
         [0000]    Taking the trace of both sides of (9) gives that the angle of rotation around the screw axes of M A  and M B  must be the same, θ A =θ B . This is one of the two invariants for SE(3). 
         [0033]    To begin to solve for X, (9) can be rewritten as 
         [0000]      log            M   A   =ADX  log            M   B   (11)
 
         [0000]    in which the solution space of all possible X&#39;s that satisfy this equation is known to be two dimensional when the rotation angle is in the range (0, π). This can be seen by defining 
         [0000]    
       
         
           
             
               
                 log 
                  
               
                
               
                   
               
                
               
                 M 
                 A 
               
             
             = 
             
               ( 
               
                 
                   
                     
                       
                         θ 
                         A 
                       
                        
                       
                         n 
                         A 
                       
                     
                   
                 
                 
                   
                     
                       v 
                       A 
                     
                   
                 
               
               ) 
             
           
         
       
     
         [0000]    and breaking (11) up into rotational and translational parts as 
         [0000]        n   A   =R   X   n   B   (12)
 
         [0000]      and 
         [0000]        V   A =θ B   t   X   R   X   n   B   +R   X   v   B   (13)
 
         [0000]    Equation (12) has a one-dimensional solution space of the form R X =Rn A ,n B Rn B , φ where φ∈0,2π is free and Rn A ,n B  is any rotation matrix that rotates the vector n B  into n A . In particular, we choose 
         [0000]    
       
         
           
             
               
                 
                   
                     R 
                      
                     
                         
                     
                      
                     
                       n 
                       A 
                     
                   
                   , 
                   
                     
                       n 
                       B 
                     
                     = 
                     
                        
                       + 
                       
                         
                           n 
                           B 
                         
                         × 
                         
                           n 
                           A 
                         
                       
                       + 
                       
                         
                           
                             1 
                             - 
                             
                               
                                 n 
                                 B 
                               
                               · 
                               
                                 n 
                                 A 
                               
                             
                           
                           
                             
                                
                               
                                 
                                   n 
                                   B 
                                 
                                 × 
                                 
                                   n 
                                   A 
                                 
                               
                                
                             
                             2 
                           
                         
                          
                         
                           n 
                           B 
                         
                         × 
                         
                           
                             n 
                             A 
                           
                           2 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   14 
                   ) 
                 
               
             
           
         
       
     
         [0000]    The rotation Rn B ,φ is given by Euler&#39;s formula 
         [0000]        Rn   B ,φ=         +sin φ n   B +1−cos φ n   B   2 .
 
         [0034]    Substituting R x =Rn A ,n B Rn B ,φ into (13) gives 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         
                           R 
                            
                           
                               
                           
                            
                           
                             n 
                             A 
                           
                         
                         , 
                         
                           
                             n 
                             B 
                           
                            
                           
                               
                           
                            
                           R 
                            
                           
                               
                           
                            
                           
                             n 
                             B 
                           
                         
                         , 
                         
                           
                             φ 
                              
                             
                                 
                             
                              
                             
                               v 
                               B 
                             
                           
                           - 
                           
                             v 
                             A 
                           
                         
                       
                        
                       
                           
                       
                     
                     
                       θ 
                       B 
                     
                   
                   = 
                   
                     
                       n 
                       A 
                     
                      
                     
                       t 
                       X 
                     
                   
                 
               
               
                 
                   ( 
                   15 
                   ) 
                 
               
             
           
         
       
     
         [0000]    where the skew-symmetric matrix n A  has rank 2, and a free translation degree of freedom exists in t x  along the n A  direction. This means that t x  can be defined as 
         [0000]        t   X   =t ( s )= sn   A   +am   A   +bm   A   ×n   A   (16)
 
         [0000]    where s∈          is a second free parameter, m A  and m A ×n A  are defined to be orthogonal to n A  by construction. If n A =[n 1 ,n 2 ,n 3 ] T  and n 1 ,n 2  are not simultaneously zero, then we define 
         [0000]    
       
         
           
             
               m 
               A 
             
             ≐ 
             
               
                 1 
                 
                   
                     
                       n 
                       1 
                       2 
                     
                     + 
                     
                       n 
                       2 
                       2 
                     
                   
                 
               
                
               
                 
                   ( 
                   
                     
                       
                         
                           - 
                           
                             n 
                             2 
                           
                         
                       
                     
                     
                       
                         
                           n 
                           1 
                         
                       
                     
                     
                       
                         0 
                       
                     
                   
                   ) 
                 
                 . 
               
             
           
         
       
     
         [0000]    The coefficients a and b are then computed by substituting (16) into (15) and using the fact that n A ,m A ,n A ×m A  is an orthonormal basis for            3 . Explicitly, 
         [0000]    
       
         
           
             a 
             = 
             
               
                 ( 
                 
                   
                     
                       
                         R 
                          
                         
                             
                         
                          
                         
                           n 
                           A 
                         
                       
                       , 
                       
                         
                           n 
                           B 
                         
                          
                         
                             
                         
                          
                         R 
                          
                         
                             
                         
                          
                         
                           n 
                           B 
                         
                       
                       , 
                       
                         
                           φ 
                            
                           
                               
                           
                            
                           
                             v 
                             B 
                           
                         
                         - 
                         
                           v 
                           A 
                         
                       
                     
                      
                     
                         
                     
                   
                   
                     θ 
                     B 
                   
                 
                 ) 
               
               · 
               
                 m 
                 A 
               
             
           
         
       
       
         
           and 
         
       
       
         
           
             b 
             = 
             
               
                 
                   ( 
                   
                     
                       
                         
                           R 
                            
                           
                               
                           
                            
                           
                             n 
                             A 
                           
                         
                         , 
                         
                           
                             n 
                             B 
                           
                            
                           
                               
                           
                            
                           R 
                            
                           
                               
                           
                            
                           
                             n 
                             B 
                           
                         
                         , 
                         
                           
                             φ 
                              
                             
                                 
                             
                              
                             
                               v 
                               B 
                             
                           
                           - 
                           
                             v 
                             A 
                           
                         
                       
                        
                       
                           
                       
                     
                     
                       θ 
                       B 
                     
                   
                   ) 
                 
                 · 
                 
                   m 
                   A 
                 
               
               × 
               
                 
                   n 
                   A 
                 
                 . 
               
             
           
         
       
     
         [0000]    The feasible solutions can therefore be completely parameterized as 
         [0000]        Xφ,s=HRn   A   ,n   B   Rn   B   ,φ,t ( s )  (17)
 
         [0000]    where φ,s∈0,2π×          and H(R,t) is the same as in (1). 
         [0035]    Given that (9) constrains the possible solutions, X(Ø, s), to a two-dimensional ‘cylinder’ defined by (17), solving for X reduces to that of solving (10) on this cylinder by calculating the values (Ø, s). 
         [0036]    In one approach, we backsubstitute X=X(Ø, s) into (10) and minimize the cost function 
         [0000]        C   1   φ,s=∥Ad ([ Xφ,s]   −1 )Σ A −Σ B   Ad   T   Xφ,s∥   2   (18)
 
         [0000]    The parameter s then appears linearly inside the norm, and C 1 φ,s is quadratic in s and can be written as 
         [0000]        C   1   φ,s=C   10   +C   11 (φ) s+ ½ C   12 (φ) s   2 .
 
         [0000]    The minimization over s can then be solved in closed form as 
         [0000]    
       
         
           
             s 
             = 
             
               - 
               
                 
                   
                     
                       C 
                       11 
                     
                      
                     
                       ( 
                       φ 
                       ) 
                     
                   
                   
                     
                       C 
                       12 
                     
                      
                     
                       ( 
                       φ 
                       ) 
                     
                   
                 
                 . 
               
             
           
         
       
     
         [0037]    Back-substituting this value into C 1 φ,s allows for an efficient 1D search over φ∈0,2π to be performed. 
         [0038]    In another approach, Gaussian statistics for two populations of measured {A i } and {B j } are produced. {A i } and {(B j } naturally behave as Gaussian distributions as the amount of data becomes large. If, however, the population is not large enough, it is possible to ‘corral’ the data so that it has Gaussian statistics, using the following approach. 
         [0039]    A Gaussian on SE(3) can be defined when the norm ∥Σ∥ is small as 
         [0000]    
       
         
           
             
               
                 ρ 
                  
                 
                     
                 
                  
                 H 
               
               ; 
               M 
             
             , 
             
               ∑ 
               
                   
               
                
               
                 = 
                 
                   
                     1 
                     
                       2 
                        
                       
                         π 
                         3 
                       
                        
                       
                          
                         ∑ 
                          
                       
                        
                       
                         1 
                         2 
                       
                     
                   
                    
                   
                      
                     
                       
                         - 
                         
                           1 
                           2 
                         
                       
                        
                       F 
                        
                       
                           
                       
                        
                       
                         M 
                         
                           - 
                           1 
                         
                       
                        
                       H 
                     
                   
                 
               
             
           
         
       
     
         [0000]    where |Σ| denotes the determinant of Σ and 
         [0000]        FH =[ log           H]   T Σ −1 [ log           H].  
 
         [0000]    When H is parameterized with exponential coordinates, H=expZ, this means that F expZ=z T Σ −1 z where z=Z          and ρ expZ;            4 ,Σ becomes a zero-mean Gaussian distribution on the Lie algebra SE(3), with covariance Σ. 
         [0040]    There is no loss in generality in assuming that ∥Σ A ∥ and ∥Σ B ∥ are small because the constraint equation (10) is linear in ΣA and Σ B , and so if they are not small, they can be normalized resulting in Σ′ A =Σ A /10∥Σ A ∥ and likewise Σ′ B =Σ B /10∥Σ B ∥. 
         [0041]    Moreover, standard tests from multivariate statistical analysis such as q-q plots can be used to assess whether or not the data sets are Gaussian. If not, they can be made Gaussian without loss of information or by introducing changes to the original mean and covariance. Since A i =XB i X −1 , it follows that A i   p =XB i   p X −1  for any power p∈         . Practically speaking, we can sample p at fractional powers in the range p∈−1,1 and introduce multiple instances of samples with a Gaussian weighting that depends on p, causing the resulting augmented data set to behave as a Gaussian. 
         [0042]    Solving for X can therefore be reduced to finding the global minimum of the cost function 
         [0000]        C   2   X=D   KL ƒ A ∥δ X *ƒ B *δ X-1   (19)
 
         [0000]    where ƒ A H=ρH; M A ,ΣA and 
         [0000]      δ X *ƒ B *δ X-1   H=ρH;XM   B   X   −1   ,AdXΣ   B   Ad   T   X.  
 
         [0000]    In general, the integral in this cost function cannot be solved in closed form because the log function is nonlinear, and in terms of exponential coordinates dH=|J(z)|dz where |J0|=1, but this Jacobian is a nonlinear function of z. 
         [0043]    However, if a priori we limit the search for X to the cylinder defined in (17), then XM B X −1 =M A . We can then define a variable K=M A   −1 H, and using the property of invariance of integration under shifts, 
         [0000]        C   2   Xφ,s=D   KL ƒ′ A ∥ƒ′ B  
 
         [0000]      where 
         [0000]      ƒ′ A ( K )=ρ K;             4 ,Σ A  
 
         [0000]      and 
         [0000]      ƒ′ B ( K )=ρ K;             4   ,AdXφ,sΣ   B   Ad   T   Xφ,s.  
 
         [0000]    When restricting to the cylinder, logarithms and exponentials cancel. Moreover, scaling covariances so that they are small ensures that the integral over SE(3) reduces to a Gaussian integral over se(3)≅           6  since |J0|=1. The KL divergence of two distributions on            n  with means m i  and covariances Σ i  is 
         [0000]    
       
         
           
             
               
                 D 
                 KL 
               
                
               
                   
               
                
               
                 f 
                 1 
               
                
               
                  
                  
               
                
               
                 f 
                 2 
               
             
             = 
             
               
                 
                   1 
                   2 
                 
                  
                 
                   [ 
                   
                     
                       tr 
                        
                       
                           
                       
                        
                       
                         
                           ∑ 
                           2 
                           
                             - 
                             1 
                           
                         
                          
                         
                           
                             ∑ 
                             1 
                           
                            
                           
                             + 
                             
                               m 
                               2 
                             
                           
                         
                       
                     
                     - 
                     
                       
                         
                           m 
                           1 
                         
                         T 
                       
                        
                       
                         
                           ∑ 
                           2 
                           
                             - 
                             1 
                           
                         
                          
                         
                           m 
                           2 
                         
                       
                     
                     - 
                     
                       m 
                       1 
                     
                     - 
                     n 
                     - 
                     
                       ln 
                        
                       
                         ( 
                         
                           
                              
                             
                               ∑ 
                               1 
                             
                              
                           
                           
                              
                             
                               ∑ 
                               2 
                             
                              
                           
                         
                         ) 
                       
                     
                   
                   ] 
                 
               
               . 
             
           
         
       
     
         [0000]    In our problem, m 2 −m 1 =0 and since SE(3) is unimodular, |Ad X|=1 and so when evaluating Σ 1 =ΣA and Ad X φ,s Σ B Ad T  X φ,s, the final term in the above expression for D KL  ƒ 1 ∥ƒ 2  is independent of X. Moreover, for our purposes additive and positive multiplicative constants can be ignored and so we can simply consider the first term in the KL-divergence, scaled by a factor of two: 
         [0000]    
       
      
       C′ 
       2 
       Xφ,s=trΣ 
       A 
       −1 
       AdXφ,sΣ 
       B 
       Ad 
       T 
       Xφ,s,  
      
     
         [0000]    minimized over φ,s∈0,2π×         . Minimization over s can be done in closed form since          C′ 2  X φ,s is also quadratic in s, and the result is substituted back in for a one-dimensional search over Ø. 
         [0044]    The preceding methods assume that the data are free of measurement errors. It is possible, however, to modify these approaches to allow for the fact that noise may exist in the data. 
         [0045]    If X*=X φ*,s* is the optimal solution computed using any of the above methods, and this is treated as a good initial estimate rather than the final solution when the data have noise, then the optimal solution with noise can be written as X=X−X*Y where ∥log Y∥ is small. Then, without assuming that M A =XM B X −1 , but assuming that ∥Σ A ∥ and ∥log M A ∥ are small (and likewise for B), then 
         [0000]    
       
         
           
             
               
                 log 
                  
               
                
               
                   
               
                
               
                 M 
                 B 
                 
                   - 
                   1 
                 
               
                
               H 
             
             ≈ 
             
               
                 [ 
                 
                    
                   + 
                   
                     
                       1 
                       2 
                     
                      
                     Ad 
                      
                     
                         
                     
                      
                     
                       M 
                       B 
                     
                   
                 
                 ] 
               
                
               
                 log 
                  
               
                
               
                   
               
                
               H 
             
           
         
       
       
         
           and 
         
       
       
         
           
             
               Ad 
                
               
                   
               
                
               X 
               * 
               Y 
             
             = 
             
               
                 Ad 
                  
                 
                     
                 
                  
                 X 
                 * 
                 
                     
                 
                  
                 Ad 
                  
                 
                     
                 
                  
                 Y 
               
               = 
               
                 Ad 
                  
                 
                     
                 
                  
                 X 
                 * 
                 
                   [ 
                   
                     
                        
                       6 
                     
                     + 
                     
                       ad 
                        
                       
                           
                       
                        
                       log 
                        
                       
                           
                       
                        
                       Y 
                     
                   
                   ] 
                 
               
             
           
         
       
       
         
           where 
         
       
       
         
           
             
               ad 
                
               
                   
               
                
               log 
                
               
                   
               
                
               Y 
             
             = 
             
               
                 
                   [ 
                   
                     
                       
                         Ω 
                       
                       
                         v 
                       
                     
                     
                       
                         0 
                       
                       
                         0 
                       
                     
                   
                   ] 
                 
                  
                 
                     
                 
                  
                 so 
                  
                 
                     
                 
                  
                 
                   ad 
                    
                   
                     ( 
                     
                       log 
                        
                       
                           
                       
                        
                       Y 
                     
                     ) 
                   
                 
               
               = 
               
                 [ 
                 
                   
                     
                       Ω 
                     
                     
                        
                     
                   
                   
                     
                       V 
                     
                     
                       Ω 
                     
                   
                 
                 ] 
               
             
           
         
       
     
         [0000]    and has the property that exp ad log Y=Ad expY. 
         [0046]    An exact solution to the A i X=XB i  problem only exists when four independent invariant quantities exist between two pairs of A&#39;s and B&#39;s, which are defined from classical screw theory. From screw theory it is known that any homogenous transformation can be written as 
         [0000]    
       
         
           
             H 
             = 
             
               ( 
               
                 
                   
                     
                        
                       
                         θ 
                          
                         
                             
                         
                          
                         N 
                       
                     
                   
                   
                     
                       
                         
                           ( 
                           
                             
                                
                               3 
                             
                             - 
                             
                                
                               
                                 θ 
                                  
                                 
                                     
                                 
                                  
                                 N 
                               
                             
                           
                           ) 
                         
                          
                         p 
                       
                       + 
                       
                         d 
                          
                         
                             
                         
                          
                         n 
                       
                     
                   
                 
                 
                   
                     
                       0 
                       T 
                     
                   
                   
                     1 
                   
                 
               
               ) 
             
           
         
       
     
         [0000]    where e θN  denotes the matrix exponential,            n  is the n×n identity matrix, and θΣ0, π is the angle of rotation. 
         [0000]    
       
         
           
             N 
             = 
             
               ( 
               
                 
                   
                     0 
                   
                   
                     
                       - 
                       
                         n 
                         3 
                       
                     
                   
                   
                     
                       n 
                       2 
                     
                   
                 
                 
                   
                     
                       n 
                       3 
                     
                   
                   
                     0 
                   
                   
                     
                       - 
                       
                         n 
                         1 
                       
                     
                   
                 
                 
                   
                     
                       - 
                       
                         n 
                         2 
                       
                     
                   
                   
                     
                       n 
                       1 
                     
                   
                   
                     0 
                   
                 
               
               ) 
             
           
         
       
     
         [0000]    where n=[n 1 , n 2 , n 3 ] T ∈           3  is the unit vector describing the axis of rotation, which connects the origin and any point on the unit sphere, and p·n=0. Together, {θ, d, n, p} define the Plücker coordinates of the screw motion. 
         [0047]    If we write A i X=XB i  as 
         [0000]        A   i   =XB   i   X   −1  where  i∈{ 1,2},  (20)
 
         [0000]    then calculating and equating the matrix product gives two invariant relations, 
         [0000]      θ A     i   =θ B     i      d   A     i     =d   B     i   ,  (21)
 
         [0000]    where d A     i    and d B     i    are computed from A i  and B i . Additionally, let 
         [0000]      1 A     i   ,( t )= p   A     i     +tn   A     i    and 1 B     i   ,( t )= p   B     i     +tn   B     i      
         [0000]    be the directed screw axis lines of A i  and B i  in three-dimensional Euclidean space. If the lines are not parallel or anti-parallel, i.e. if n A     i   ≠±n B     i   , then the distance between the two lines is given by 
         [0000]    
       
         
           
             
               
                 
                   
                     Δ 
                      
                     
                       ( 
                       
                         
                           1 
                           
                             
                               A 
                               i 
                             
                              
                             1 
                           
                         
                         , 
                         
                           1 
                           
                             
                               A 
                               i 
                             
                              
                             2 
                           
                         
                       
                       ) 
                     
                   
                   = 
                   
                     
                        
                       
                         
                           n 
                           
                             
                               A 
                               i 
                             
                              
                             1 
                           
                         
                         , 
                         
                           n 
                           
                             
                               A 
                               i 
                             
                              
                             2 
                           
                         
                         , 
                         
                           
                             p 
                             
                               
                                 A 
                                 i 
                               
                                
                               2 
                             
                           
                           - 
                           
                             p 
                             
                               
                                 A 
                                 i 
                               
                                
                               1 
                             
                           
                         
                       
                        
                     
                     
                        
                       
                         
                           n 
                           
                             
                               A 
                               i 
                             
                              
                             1 
                           
                         
                         × 
                         
                           n 
                           
                             
                               A 
                               i 
                             
                              
                             2 
                           
                         
                       
                        
                     
                   
                 
               
               
                 
                   ( 
                   22 
                   ) 
                 
               
             
           
         
       
     
         [0000]    where for any a,b,c∈           3  the triple product is a,b,c≐a·b×c. 
         [0048]    If in addition, Δ(1 A     i     1 ,1 A     i     2 )≠0, i.e., if the lines are skew, then the angle φ(1 A     i     1 ,1 A     i     2 )∈0,2π is uniquely specified by 
         [0000]      cos φ(1 A     i     1 ,1 A     i     2 )= n   A     i     1   ·n   A     i     2  
 
         [0000]      sin φ(1 A     i     1 ,1 A     i     2 )=Δ(1 A     i     1 ,1 A     i     2 ) −1   [n   A     i     1   ,n   A     i     2   ,p   A     i     2   −p   A     i     1 ]  (23)
 
         [0049]    Therefore, if (θ A     i     1 ,θ A     i     2 )∈(0,π) and φ(1 A     i     1 ,1 A     i     2 )∉{0,π}, then a unique solution of A i X=XB i  exists if and only if the following four conditions hold: 
         [0000]      θ A     i     1 ,θ B     i     1  and θ A     i     2 ,θ B     i     2 ;  1)
 
         [0000]        d   A     i     1   ,d   B     i     1  and  d   A     i     2   ,d   B     i     2 ;  2)
 
         [0000]      φ(1 A     i     1 ,1 B     i     2 )=φ(1 B     i     1 ,1 B     i     2 )  3)
 
         [0000]      Δ(1 A     i     1 ,1 B     i     2 )=Δ(1 B     i     1 ,1 B     i     2 )  4)
 
         [0000]      FIG. 1  illustrates the Plücker coordinates, and the parameters of the above four conditions for two arbitrary rigid-body motions. 
         [0050]    One method of solving for X uses the first two invariants, θ and d, to compute a correlation by re-shifting temporally misaligned data. Given n, (A i , B i ) pairs drawn from sensor data, the set of A&#39;s is shifted by a set amount to mimic the effects of the asynchronous data transfer. The SE(3) invariants are then extracted from each of the A i &#39;s and B i &#39;s in the new temporally shifted set. We can then perform a correlation of the A invariants (θ A , d A ) with the B invariants (θ B , d B ) using the Discrete Fourier Transform. 
         [0051]    Given a sequence of N complex numbers, the DFT is defined as 
         [0000]    
       
         
           
             
               F 
               n 
             
             = 
             
               
                 ℱ 
                  
                 
                   ⌊ 
                   
                     
                       { 
                       
                         f 
                         k 
                       
                       } 
                     
                     
                       k 
                       = 
                       0 
                     
                     
                       N 
                       - 
                       1 
                     
                   
                   ⌋ 
                 
                  
                 
                   ( 
                   n 
                   ) 
                 
                  
                 
                     
                 
                  
                 where 
                  
                 
                     
                 
                  
                 
                   F 
                   n 
                 
               
               ≐ 
               
                 
                   ∑ 
                   
                     k 
                     = 
                     0 
                   
                   
                     N 
                     - 
                     1 
                   
                 
                  
                 
                     
                 
                  
                 
                   
                     f 
                     k 
                   
                    
                   
                      
                     
                       
                         - 
                         2 
                       
                        
                       
                         π 
                          
                         in 
                       
                        
                       
                         k 
                         N 
                       
                     
                   
                 
               
             
           
         
       
     
         [0000]    and the inverse transform is given as 
         [0000]    
       
         
           
             
               f 
               k 
             
             ≐ 
             
               
                 1 
                 N 
               
                
               
                 
                   ∑ 
                   
                     k 
                     = 
                     0 
                   
                   
                     N 
                     - 
                     1 
                   
                 
                  
                 
                     
                 
                  
                 
                   
                     F 
                     n 
                   
                    
                   
                     
                        
                       
                         2 
                          
                         
                           π 
                            
                           ik 
                         
                          
                         
                           n 
                           N 
                         
                       
                     
                     . 
                   
                 
               
             
           
         
       
     
         [0000]    The convolution theorem for the Discrete Fourier Transform indicates that a correlation, Corr ƒ,g, of two sequences of finite length can be obtained as the inverse transform of the product of one individual transform with the complex conjugate (*) of the other transform: 
         [0000]      Corr ƒ, g=             −1   F·G*.  
 
         [0000]    The location of largest correlation corresponds with the amount of shift in the A&#39;s.  FIG. 2  shows an example case where the data streams are shifted by −13 units. The shifted theta streams can be seen in the top graph and the correlated plot, where the maximum value is at the predicted location (−13), is shown in the bottom graph. This method accurately recovers the shift in the data streams and then corrects the shift to calculate for X. 
         [0052]    This approach is accurate with uniform shifts, or if the data has large gaps that allow for substreams to be created between gaps. However, if there are largely varying, non-uniform shifts, or a large number of small groups in the data, the previous algorithm begins to break down. As an alternative option, an algorithm using all four invariants is presented. 
         [0053]    Consider the case that data streams of sensor measurements |         |={A i } and |         B|={B i } are presented with significant unknown temporal shifts between the two sets, and gaps within each one. The number of points in these sets are |         |=m and |         |=n. 
         [0054]    Here we present an approach to recovering X and establishing a correspondence between the subsets          ′ ⊂          and          ′⊂          that do correspond where |         ′|=|         ′|=p≦min(m,n). For such data, we find the correspondence, which is a permutation on p letters, π∈Π p , such that A i X=XB π(i)  for i=1, . . . , p where A i ∈         ′ and B π(i) ∈         ′. 
         [0055]    This is accomplished using the invariants of the Special Euclidean group, SE(3), under conjugation. The procedure is as follows. Compute (θ A     I   ,d A     i   ) for each A i ∈          and (θ B     j   ,θ B     j   ) for each B j ∈         . Next, form a 2D grid on the θ-d plane that ranges from min i,j (θ A     i   ,θ B     j   ) to max i,j (θ A     i   ,θ B     j   ) and min i,j (d A     i   ,d B     j   ) to max i,j (d A     i   ,d B     j   ). This grid will give r rectangles, e.g., if it is a 10×10 grid, then r=100. Assuming that no data falls exactly on a grid line, this will partition A and B into r disjoint subsets:            1 ,            2 , . . . ,            r  and            1 ,            2 , . . . ,            r  where 
         [0000]    
       
         
           
             
               
                 
                   A 
                   
                     i 
                     1 
                   
                 
                 ⋂ 
                 
                   A 
                   
                     i 
                     2 
                   
                 
               
               = 
               
                 
                   
                     0 
                     / 
                     and 
                   
                    
                   
                       
                   
                    
                   
                     ⋃ 
                     
                       i 
                       = 
                       1 
                     
                   
                    
                   
                     A 
                     i 
                   
                 
                 = 
                 A 
               
             
             , 
           
         
       
     
         [0000]    and similarly for B. 
         [0056]    This allows for all potentially matching A&#39;s and B&#39;s to be in corresponding partitions A i  and B i , since having the same value of θ and d is a necessary condition for a solution to AX=XB to exist. Constructing the grid with finite resolution allows for the possibility of some measurement errors in A&#39;s and B&#39;s. 
         [0057]    Let |           i |=m i  and |           j |=n j , then 
         [0000]    
       
         
           
             
               
                 ∑ 
                 
                   i 
                   = 
                   1 
                 
                 r 
               
                
               
                   
               
                
               
                 m 
                 i 
               
             
             = 
             
               
                 m 
                  
                 
                     
                 
                  
                 and 
                  
                 
                     
                 
                  
                 
                   
                     ∑ 
                     
                       i 
                       = 
                       1 
                     
                     r 
                   
                    
                   
                       
                   
                    
                   
                     n 
                     j 
                   
                 
               
               = 
               
                 n 
                 . 
               
             
           
         
       
     
         [0000]    Pick two bins for which all of the numbers in the pairs (m i     1   ,n j     1   ) and (m i     2   ,n j     2   ) are small, but greater than 2, to allow for the fact that measurement error may result in incorrect binning, and also that the angle φ(1 A     i     1 ,1 A     i     2 ) might not always be in the range (0,π). We interrogate all m i     1   ×n j     1   ×m i     2   ×n j     2    possibilities as candidates. The further necessary conditions for the existence of a solution are φ(1 A     i     1 ,1 A     i     2 )=φ(1 B     j     1 ,1 B     j     2 ) and Δ(1 A     i     1 ,1 A     i     2 )=Δ(1 B     j     1 ,1 B     j     2 ). From among all pairs that satisfy these conditions, we can use existing AX=XB solvers to determine X. 
         [0058]    To solve the, now registered, AX=XB, a common method that finds a least-squares solution is used by using the Kronecker product. If C is a matrix, vec(C) is the long vector produced by stacking the columns of C. This is a linear operation in the sense that vec α 1 C 1 +α 2 C 2 =α 1 vec C 1 +α 2 vec C 2 . Moreover, if           denotes the Kronecker product, and C, D, E are matrices with dimensions compatible for multiplication, then vec CDE=E T           C vec D. If D is already a column vector (n×1 matrix), then it is unaltered by the vec(•), and if D is a row vector (1×n matrix), then vec(•) transposes it. If C is a matrix and α is a scalar, then α         C=C         α=αC, the scalar multiple of C by α. 
         [0059]    Therefore, we can write the AX=XB equation as 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       J 
                       i 
                     
                      
                     x 
                   
                   = 
                   
                     b 
                     i 
                   
                 
               
               
                 
                     
                 
               
             
             
               
                 where 
               
               
                 
                     
                 
               
             
             
               
                 
                   
                     
                       J 
                       i 
                     
                     = 
                     
                       [ 
                       
                         
                           
                             
                               
                                  
                                 9 
                               
                               - 
                               
                                 
                                   R 
                                   
                                     B 
                                     i 
                                   
                                 
                                 ⊗ 
                                 
                                   R 
                                   
                                     A 
                                     i 
                                   
                                 
                               
                             
                           
                           
                             
                                
                               
                                 9 
                                 × 
                                 3 
                               
                             
                           
                         
                         
                           
                             
                               
                                 t 
                                 
                                   B 
                                   i 
                                 
                                 T 
                               
                               ⊗ 
                               
                                  
                                 3 
                               
                             
                           
                           
                             
                               
                                  
                                 3 
                               
                               - 
                               
                                 R 
                                 
                                   A 
                                   i 
                                 
                               
                             
                           
                         
                       
                       ] 
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   24 
                   ) 
                 
               
             
             
               
                 
                   
                     x 
                     = 
                     
                       
                         
                           [ 
                           
                             
                               
                                 
                                   vec 
                                    
                                   
                                       
                                   
                                    
                                   
                                     K 
                                     
                                       R 
                                       X 
                                     
                                   
                                 
                               
                             
                             
                               
                                 
                                   K 
                                   
                                     t 
                                     X 
                                   
                                 
                               
                             
                           
                           ] 
                         
                          
                         
                             
                         
                          
                         and 
                          
                         
                             
                         
                          
                         
                           b 
                           i 
                         
                       
                       = 
                       
                         [ 
                         
                           
                             
                               
                                 0 
                                 9 
                               
                             
                           
                           
                             
                               
                                 t 
                                 
                                   A 
                                   i 
                                 
                               
                             
                           
                         
                         ] 
                       
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   25 
                   ) 
                 
               
             
           
         
       
     
         [0000]                m  is the m×m identity,            m×n  is the m×n zero matrix, and 0 n  is the n-dimensional zero vector. 
         [0060]    J i  is a 12×6 matrix and b i  is six-dimensional. By stacking multiple such equations for different pairs, (A i , B i ), we obtain Jx=b where J is 12n×6n and b is 6n-dimensional. The least-squares solution for x can be then found using SVD methods or using a pseudo-inverse. 
         [0061]    For example, the least-squares solution to ∥Jx−b∥ M  where M=M T ∈           6n×6n  is 
         [0000]        x=J   T   MJ   −1   J   T   Mb   (26)
 
         [0000]    This is the over-constrained pseudo-inverse, as opposed to the under-constrained pseudo-inverse typically used in redundancy resolution. 
         [0062]    For some applications of the AX=XB problem, the sensor data is only rotational. The (A i , B i ) pairs are now drawn form the group of rigid-body rotations, SO(3), a subgroup of SE(3). In this case there is no d or Δ. The presented algorithms are successful for data of this type, despite the absence of translation information. For the algorithm using correlations, θ is used to match the data streams. The second algorithm, which uses the binning procedure, is successful using only the θ and Ø invariants. 
         [0063]    It is also possible to solve for X in the AX=XB problem using a Batch Method, the algorithm as follows. SE(3) is a Lie group, and hence concepts of integration and convolution exist. If X∈SE(3) is a generic 4×4 homogeneous transformation of the form H R, 
         [0000]    
       
         
           
             t 
             = 
             
               ( 
               
                 
                   
                     R 
                   
                   
                     t 
                   
                 
                 
                   
                     
                       0 
                       T 
                     
                   
                   
                     1 
                   
                 
               
               ) 
             
           
         
       
     
         [0000]    where the rotation is parameterized in terms of Euler angles as R=R 3  α R 1  β R 3  γ (where R i (θ) is a counterclockwise rotation by θ around coordinate axis i) and the translation is t=[t x , t y ,t z ] T , then the natural integral of any rapidly decaying function is computed as 
         [0000]      ∫ SE(3)   ƒHdH=             3 ∫ SO(3)   ƒHR,tdRdt  
 
         [0000]    where dR=sin βdαdβdγ and dt=dt x dt y dt z , with −∞&lt;t x ,t y ,t z &lt;∞ and |α,β,γ|∈0,2π×0, π×0,2π. This integral is natural in the sense that it is unique such that 
         [0000]      ∫ SE(3)   ƒHdH=∫   SE(3)   ƒH   −1   dH=∫   SE(3)   ƒHH   0   dH=∫   SE(3)   ƒH   0   HdH    (27)
 
         [0000]    for any fixed H 0 ∈SE(3). This choice of integral, being invariant under shifts on the left and on the right in the above equation, is called the bi-invariant measure. This instantiation of the bi-invariant integral for SE(3) is not unique, as any parameterization of SE(3) can be used. 
         [0064]    If ∫ SE(3) |ƒH| p dH&lt;∞ then we say ƒ∈L p (SE(3)). Most of the following algorithm will be limited to functions ƒ∈L 1 (SE(3))∩L 2 (SE(3)), together with the special case of a Dirac delta function. 
         [0065]    In this light, we can think of A i X=XB i  as the equation 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       ( 
                       
                         
                           δ 
                           
                             A 
                             i 
                           
                         
                         * 
                         
                           δ 
                           x 
                         
                       
                       ) 
                     
                      
                     
                         
                     
                      
                     H 
                   
                   = 
                   
                     
                       ( 
                       
                         
                           δ 
                           x 
                         
                         * 
                         
                           δ 
                           
                             B 
                             i 
                           
                         
                       
                       ) 
                     
                      
                     
                         
                     
                      
                     H 
                   
                 
               
               
                 
                   ( 
                   28 
                   ) 
                 
               
             
           
         
       
     
         [0000]    This provides freedom to execute mathematical operations that could not be previously be performed. The addition of homogeneous transformation matrices is nonsensical, but can be performed with real-valued functions. 
         [0066]    In this form, the correspondence between A i  and B j  does not need to be known, as the above functions are normalized to be probability densities: 
         [0000]      ∫ SE(3) ƒ A   HdH=∫   SE(3) ƒ B   HdH= 1.
 
         [0067]    Assume that the set of A i &#39;s and B i &#39;s are clumped closely together. In other words, given a measure of distance between reference frames, d:SE(3)×SE(3)→           ≦0 , we have that d A i ,A j , d B i ,B j &lt;∈&lt;&lt;1. This assumption can be made true, for example, if we are using small relative motions between consecutive reference frames, regardless of the length of the whole trajectory. The mean and covariance of a probability density ƒ(H) can be defined by the conditions 
         [0000]      ∫ SE(3)  log  M   −1   HƒHdh=               
 
         [0000]      and 
         [0000]      Σ∫ SE(3)  log            M   −1   H [ log            M   −1   H]   T   ƒHdh   (29)
 
         [0068]    The definition of mean used above differs from that most often used in literature when taking a Riemannian-geometric (rather than Lie-group) approach which is of the form M′=argmin M ∫ SE(3) [d M,H] 2  ƒ H dH where d(M,H) is A Riemannian distance function (d M,H=∥log M −1 ,H∥ W   2 , for example) and W is a weighting matrix related to the Riemannian metric tensor that is chosen. There are two reasons for our definition. First, in our definition there is no need to introduce a weighting matrix, and therefore we avoid coloring the result by arbitrary choice. Second, in the context of robotics problems in which reference frames are attached to rigid links it is more natural in the following sense. 
         [0069]    If a single rigid link has a world frame attached to its base, and a reference frame attached to its distal end, and the distal reference frame is recorded at two different times as a joint at the base rotates, then the translation part of the average of these two reference frames should lie on the arc that joins the two. M will have this property, but M′ will not. If we consider data on SO(3) rather than SE(3), and if W=          were chosen, the two definitions would become the same thing since for SO(3) the distance function d R 1 ,R 2 ≐∥log R 1   −1 R is bi-invariant and Ad(R)=R. But for SE(3) neither of these statements are true: Ad H≠H and there does not exist a bi-invariant metric. 
         [0070]    If ƒ(H) is of the form ƒ A (H) then 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       ∑ 
                       
                         i 
                         = 
                         1 
                       
                       n 
                     
                      
                     
                         
                     
                      
                     
                       log 
                        
                       
                           
                       
                        
                       
                         M 
                         A 
                         
                           - 
                           1 
                         
                       
                        
                       
                         A 
                         i 
                       
                     
                   
                   = 
                   
                     
                        
                        
                       
                           
                       
                        
                       and 
                        
                       
                           
                       
                        
                       
                         ∑ 
                         A 
                       
                     
                     = 
                     
                       
                         1 
                         n 
                       
                        
                       
                         
                           ∑ 
                           
                             i 
                             = 
                             1 
                           
                           n 
                         
                          
                         
                             
                         
                          
                         
                           
                             log 
                              
                           
                            
                           
                               
                           
                            
                           
                             M 
                             A 
                             
                               - 
                               1 
                             
                           
                            
                           
                             
                               
                                 A 
                                 i 
                               
                                
                               
                                 [ 
                                 
                                   
                                     log 
                                      
                                   
                                    
                                   
                                       
                                   
                                    
                                   
                                     M 
                                     A 
                                     
                                       - 
                                       1 
                                     
                                   
                                    
                                   
                                     A 
                                     i 
                                   
                                 
                                 ] 
                               
                             
                             T 
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   30 
                   ) 
                 
               
             
           
         
       
     
         [0000]    An iterative process for computing M A  is performed in which an initial estimate of the form 
         [0000]    
       
         
           
             
               M 
               A 
               0 
             
             = 
             
               exp 
               ( 
               
                 
                   1 
                   n 
                 
                  
                 
                   
                     ∑ 
                     
                       i 
                       = 
                       1 
                     
                     n 
                   
                    
                   
                       
                   
                    
                   
                     log 
                      
                     
                         
                     
                      
                     
                       A 
                       i 
                     
                   
                 
               
               ) 
             
           
         
       
     
         [0000]    is chosen, and then a gradient descent procedure is used to update so as to minimize the cost C(M)=∥Σ i=1   n  log M −1 A i ∥ 2 , and the minimum defines M A . 
         [0071]    It can be shown that if these quantities are computed for two highly focused functions, ƒ 1  and ƒ 2 , that the same quantities for the convolution of these functions can be computed as 
         [0000]    
       
         
           
             
               
                 
                   
                     M 
                     
                       
                         1 
                         * 
                       
                        
                       2 
                     
                   
                   = 
                   
                     
                       
                         M 
                         1 
                       
                        
                       
                         M 
                         2 
                       
                        
                       
                           
                       
                        
                       and 
                        
                       
                           
                       
                        
                       
                         ∑ 
                         
                           
                             1 
                             * 
                           
                            
                           2 
                         
                       
                     
                     = 
                     
                       
                         
                           Ad 
                            
                           
                               
                           
                            
                           
                             M 
                             2 
                             
                               - 
                               1 
                             
                           
                            
                           
                               
                           
                            
                           
                             
                               ∑ 
                               1 
                             
                              
                             
                               
                                 Ad 
                                 T 
                               
                                
                               
                                   
                               
                                
                               
                                 M 
                                 2 
                                 
                                   - 
                                   1 
                                 
                               
                             
                           
                         
                         + 
                         
                           
                             ∑ 
                             2 
                           
                            
                           
                             
 
                           
                            
                           
                             where 
                              
                             
                                 
                             
                              
                             Ad 
                              
                             
                                 
                             
                              
                             H 
                           
                         
                       
                       = 
                       
                         
                           ( 
                           
                             
                               
                                 R 
                               
                               
                                  
                               
                             
                             
                               
                                 tR 
                               
                               
                                 R 
                               
                             
                           
                           ) 
                         
                         . 
                       
                     
                   
                 
               
               
                 
                   ( 
                   31 
                   ) 
                 
               
             
           
         
       
     
         [0072]    Here for any a∈           3 , â is the skew-symmetric matrix such that âb=a×b.           is used as the reverse map which gives â         =a. The mean of δ X H is M X =X, and its covariance is the zero matrix. Therefore 
         [0000]      ( a )  M   A   X=XM   B  and ( b ) AdX   −1 Σ A   Ad   T   X   −1 =Σ B   (32)
 
         [0000]    These two equations can be solved in a similar way to how the equations A 1 X=XB 1  and A 2 X=XB 2  are solved. 
         [0073]    First, we seek the rotational component, R X  of X. From (33a) we have that, 
         [0000]        n   M     A     =R   X   n   M     A     (33)
 
         [0000]    where n H  is the direction of the screw axis of the homogeneous transfer H. 
         [0074]    If we decompose Σ M     A    and Σ M     B    into blocks as 
         [0000]    
       
         
           
             
               ∑ 
               i 
             
              
             
               = 
               
                 ( 
                 
                   
                     
                       
                         ∑ 
                         i 
                         1 
                       
                     
                     
                       
                         ∑ 
                         i 
                         2 
                       
                     
                   
                   
                     
                       
                         ∑ 
                         i 
                         3 
                       
                     
                     
                       
                         ∑ 
                         i 
                         4 
                       
                     
                   
                 
                 ) 
               
             
           
         
       
     
         [0000]    where Σ i   3 =Σ i   2     T   , then we can take the first two blocks of (33b) and write 
         [0000]      Σ M     B     1   =R   X   T Σ M     A     1   R   X  and Σ M     B     1   =R   X   T Σ M     A     1   R   X   R   X     t     x   T   +R   X   T Σ M     A     2   R   X   (34)
 
         [0075]    We can then find the eigenvalue decomposition, Σ i =Q i ΛQ i   T , where Q i  is the square matrix whose ith column is the eigenvector of Σ i  and A is the diagonal matrix with corresponding eigenvalues as diagonal entries and write the first block equation of (35) as, 
         [0000]        A=Q   M     B     T   R   X   T   Q   M     A     ΛQ   M     A     T   R   X   Q   M     B   =         Λ           T   (35)
 
         [0000]    The set of Qs that satisfy this equation is given as, 
         [0000]    
       
         
           
             Q 
             = 
             
               { 
               
                 
                   ( 
                   
                     
                       
                         1 
                       
                       
                         0 
                       
                       
                         0 
                       
                     
                     
                       
                         0 
                       
                       
                         1 
                       
                       
                         0 
                       
                     
                     
                       
                         0 
                       
                       
                         0 
                       
                       
                         1 
                       
                     
                   
                   ) 
                 
                 , 
                 
                   ( 
                   
                     
                       
                         
                           - 
                           1 
                         
                       
                       
                         0 
                       
                       
                         0 
                       
                     
                     
                       
                         0 
                       
                       
                         
                           - 
                           1 
                         
                       
                       
                         0 
                       
                     
                     
                       
                         0 
                       
                       
                         0 
                       
                       
                         1 
                       
                     
                   
                   ) 
                 
                 , 
                 
                   ( 
                   
                     
                       
                         
                           - 
                           1 
                         
                       
                       
                         0 
                       
                       
                         0 
                       
                     
                     
                       
                         0 
                       
                       
                         1 
                       
                       
                         0 
                       
                     
                     
                       
                         0 
                       
                       
                         0 
                       
                       
                         
                           - 
                           1 
                         
                       
                     
                   
                   ) 
                 
                 , 
                 
                   ( 
                   
                     
                       
                         1 
                       
                       
                         0 
                       
                       
                         0 
                       
                     
                     
                       
                         0 
                       
                       
                         
                           - 
                           1 
                         
                       
                       
                         0 
                       
                     
                     
                       
                         0 
                       
                       
                         0 
                       
                       
                         
                           - 
                           1 
                         
                       
                     
                   
                   ) 
                 
               
               } 
             
           
         
       
     
         [0000]    with the simple condition that Q i  is constrained to be a rotation matrix. This means that the rotation component of X is given by, 
         [0000]        R   x   =Q   M     A               Q   M     B     T   (36)
 
         [0076]    The correct solution, form the set of 4 possibilities of R x  can be found by applying (34) and choosing the one that minimizes ∥n M     A   −R X n M     A   ∥. Once R x  is found in this way, t x  can be found from blocks 2 and 4 of (11b). Through numerical experimentation, it can be seen that, unlike traditional least-squares solution methods using the Kronecker product which degenerate with slight permutations of As and Bs, the Batch Solver finds the correct solution with any amount of permutation. 
         [0077]    The aforementioned algorithms can be used in sensor calibration for cameras, ultrasound probes, optical or magnetic pose tracking systems, and the like. The calibration of an ultrasound probe is discussed as a non-limiting example. This process recovers the rigid body transformation between a tracked marker attached to the ultrasound transducer and the ultrasound image. In order to perform this calibration, a phantom or model with known geometry is used. 
         [0078]    Generally, the present invention approaches the design of this phantom with consideration of ultrasound physics, and ensuring that the calibration process is easy for the user. The AX=XB is also referred to as the hand-eye calibration problem. As seen in  FIG. 3 , A i  and B i  are relative motions connected by the rigid body transformation X. A tracker provides the homogeneous transformation B i . The present invention, which can be seen in  FIG. 4 , provides an AX=XB phantom through which the transformations, A i , relating each image to the phantom&#39;s coordinate system, can be computed. The AX=XB framework is advantageous, as it does not require the transducer to be fixed at a specific location, or for the calibration phantom to be tracked by the external tracker. In addition, the phantom can be created without any need for additional assembly, adjustment, or treatment. In contrast to designs where only the frame is manufactured from a three-dimensional printer, the present invention is printed in its entirety including the wires (also referred to as “rods”). The rods shown in  FIG. 4  are 3-Dimensional rods with a circular cross section, however the rods can also have a cross section that is triangular, rectangular, hexagonal, and the like. This allows for the phantom to be “plug and play”—the user can download an appropriate computer-aided design (CAD) file, print it from a three-dimensional printer, and have the ability to perform ultrasound calibration. 
         [0079]    The length and the angle of the Z-fiducials allow for submillimeter translation. Ultrasound physics define an axial dimension that has higher resolution than the lateral dimension. With varying orientations, a proportion of the Z-fiducial change is reflected in the axial dimension. However, rods that are not perpendicular to the imaging plane have a non-optimal acoustic response. Therefore, the geometric configuration of the present invention is designed to have rods in non-parallel planes such that when one Z-fiducial becomes difficult to visualize, another Z-fiducial will have a rod that becomes increasingly perpendicular to the image plane. In one non-limiting example, the planes may diverge from parallel in a range from −30 to 30 degrees. Furthermore, the phantom has redundant rods, such that the subset of rods with the highest acoustic response, given the imaging orientation, can be chosen. 
         [0080]    Additionally, the phantom is configured to avoid the situation where many of the rods are shadowed. The Z-fiducials are oriented in the shape of a triangle such that when one face of the triangle is experiencing severe shadowing effects, the other faces will be unaffected. Furthermore, the phantom is designed such that probes of multiple lateral lengths can be used within the same phantom. Finally, the phantom allows for large range of motion of at least 3 cm for each translational degree of freedom and 45 degrees for each rotational degree of freedom. 
         [0081]    The first step of the segmentation algorithm applies an intensity threshold to the image. A connected regions algorithm is the used to cluster signal pixels together. A filter is the applied where only regions containing a certain range of pixels are retained. These steps allow for removal of noise and extraction of the rods from the ultrasound image. Then, a region closest to the transducer face is selected, which corresponds to the top rod in the phantom. The remaining regions in the ultrasound image will exhibit a triangular shape. Thus, the standard Hough transform can be applied to find the edges of the triangular pattern. An understanding of the location of the top rod and the edges of the triangular pattern allows for the establishment of the correspondences between the triangular pattern and the model. For each region, points are selected which lie closest to the transducer face from the centroid, as the top of the rods are most accurately represented in ultrasound imaging, resulting with a localization that has better lateral and axial resolution. 
         [0082]    In another embodiment of the phantom, a double triangle pyramid phantom as shown in  FIG. 5  is used. This phantom eliminates the rods previously described, and relies on points on the outer edge of a pyramid to perform calibration of the device. In this embodiment, a pose from a reference imaging system is used to create a reference image, and an additional pose from the imaging system to be calibrated is used to create a new image. The reference image and the new image are both composed of a left triangle and a right triangle, each triangle composed of three 3-dimensional points. As shown in  FIG. 6 , the known points in the reference image and the affine transformation of these points, determined with image registration and/or tracking methods, are used to determine corresponding points in the new image. 
         [0083]    In a further application of this phantom, the reference pose is not used. When using this application during the calibration process, there are no longer six known points which correspond to the new image. With the increase in unknowns, more images are necessary to recover all of the poses. 
         [0084]    The present invention has been described in terms of one or more preferred embodiments, and it should be appreciated that many equivalents, alternatives, variations, and modifications, aside from those expressly stated, are possible and within the scope of the invention.