Patent Publication Number: US-8538138-B2

Title: Global registration of multiple 3D point sets via optimization on a manifold

Description:
This application is a continuation of prior application Ser. No. 11/480,044 filed Jun. 30, 2006, now U.S. Pat. No. 7,831,090 B1 which is hereby incorporated by reference. 
    
    
     BACKGROUND OF THE INVENTION 
     The present invention is generally directed to registration of multiple 3D point sets to construct a 3D computer model. More specifically, the present invention is directed to registration of multiple 3D point sets via optimization on a manifold. 
     Constructing a three dimensional (3D) computer model of a real object from 3D surface measurement data has various applications in computer graphics, virtual reality, computer vision, and reverse engineering. To construct such a model, a 3D scanner can be used to gather surface measurement data of an object. However, a single view of an object is often insufficient due to self occlusion, the presence of shadows, and limitations of the field of view of the 3D scanner. Therefore, multiple partial views of the object are needed to provide surface measurement data for the entire object. 
     Typically, multiple overlapping views are obtained from multiple 3D scanners or from a single 3D scanner positioned at different positions and orientations with respect to an object. Multiple overlapping views can also be obtained from a 3D scanner in a fixed position taking time sampled images of an object on a moving turntable. Each view or image taken of the object consists of a 3D point set representing features of the surface of the object on a reference frame or coordinate system with an origin at a location of the 3D scanner used to obtain the 3D point set. However, the relative position and orientation between the views are known imprecisely or not at all. Accordingly, in order to generate a computer 3D model, the overlapping views must be combined, or “registered”, within a common reference frame or coordinate system by determining the relative position and orientation between the views. 
     Two-view registration is a well-studied problem in the art. As known in the art, a closed-form solution can be used to obtain the relative position and orientation of two 3D point sets, such that the least squared error between overlapping points of the two 3D point sets is minimized. 
     Multiview registration, however, is a more difficult problem. There are two strategies that can be used in order to register three or more views, local (sequential) registration and global (simultaneous) registration. Local registration involves sequentially aligning two overlapping views at a time followed by an integration step to ensure that all views are combined. However, local registration does not give an optimal solution because errors can accumulate and propagate when registering each sequential pair of views. 
     Global registration refers to registering all of the multiple views simultaneously by distributing the registration error evenly over all of the views. In this case an error function can be expressed in terms of translation vectors, which correspond to a relative position of an origin of a coordinate system for each view, and rotational matrices, which correspond to a relative rotational orientation of the coordinate system of each view. However, this error function is a non-convex function which is difficult to minimize globally. Conventional optimization approaches for minimizing the error function do not consistently converge at an optimum solution for the relative positions and orientations of the coordinate systems of the view. Accordingly, both conventional local and global registration techniques do not minimize the registration error, which is desirable when registering 3D point sets. 
     BRIEF SUMMARY OF THE INVENTION 
     The present invention is directed to a global method of registering multiple 3D point sets by determining relative positions and orientations of reference frames of the 3D point sets. This method converges quickly and accurately to optimal values for rotation matrices corresponding to the relative orientations of the 3D point sets by optimizing a registration error cost function on a product manifold of all of the rotation matrices. The product manifold can be many copies (i.e., a product) of a three-dimensional special orthogonal group (SO 3 ), which is the manifold of each rotation matrix. 
     In an embodiment of the present invention, initial values of rotational matrices corresponding to relative orientations of reference frames of multiple 3D point sets can be determined based on correspondence values between the 3D point sets. These initial values are determined using a closed form solution which gives optimal values of the rotation matrices in a noise free case. A registration error cost function is optimized on a product manifold of all of the rotation matrices starting with the determined initial values to determine optimal values for the rotation matrices. The optimization can be an iterative process to minimize the cost function while remaining on the product manifold of all of the rotational matrices. Optimal values of translation vectors corresponding to the relative positions of the reference frames of the 3D point sets are calculated based on the determined optimal values of the rotation matrices. The optimal translation vectors and rotational matrices are used to register the 3D point sets on a common reference frame. 
     In the registration method according to an embodiment of the present invention, the cost function is reduced with each iteration of the optimization process. This guarantees that the optimization process will converge to an optimal (i.e., minimum) value for the cost function. Since the optimization is being performed on the product manifold of all of the rotational matrices, the point on the product manifold which minimizes the cost function is equivalent to the optimal values for the rotational matrices corresponding to the 3D point sets. Furthermore, the optimization process of the present invention converges to an optimal value quadratically. 
     These and other advantages of the invention will be apparent to those of ordinary skill in the art by reference to the following detailed description and the accompanying drawings. 
    
    
     
       BRIEF DESCRIPTION OF THE DRAWINGS 
         FIG. 1  illustrates a method of globally registering multiple 3D point sets according to an embodiment of the present invention; 
         FIG. 2  illustrated an iterative process of optimizing a cost function to determine optimal values of rotation matrices according to an embodiment of the present invention; 
         FIG. 3  illustrates a method of calculating a descent direction according to an embodiment of the present invention; and 
         FIG. 4  illustrates a computer system configured to perform an embodiment of the present invention. 
     
    
    
     DETAILED DESCRIPTION 
     In order to construct a computer model, a laser scanner can be used to obtain multiple overlapping 3D point sets (“views”) corresponding to the surface of an object. Each overlapping 3D point set has a corresponding coordinate frame. According to an embodiment of the present invention, a registration method determines rigid body transformations between coordinate frames of the 3D point sets in order to align the 3D point sets into a common reference frame. The rigid body transformations can be expressed in terms of translation vectors T (relative position of the coordinate frames) and rotation matrices R (relative orientation of the coordinate frames). 
     A 3D object can be considered a set of 3D points W:={w k εR 3 |k=1, 2, . . . , n} in a ‘world’ reference frame. When multiple views are obtained for a 3D object, each view is obtained from a different vantage point (position) and viewing direction (orientation) and contains a point set having up to n 3D points. For each view, a translation vector t and a rotation matrix R respectively indicate the relative position and orientation of a coordinate frame of that view with respect to a reference coordinate frame. For N views, the relative rotations and translations can be denoted as (R 1 , t 1 ), . . . , (R N , t N ), that is, relative to the ‘world’reference frame, where R i  is a 3×3 rotation matrix, satisfying R i   T R i =I 3 , det (R i )=+1, and t i εR 3  is a translation vector. Each rotation matrix R is a point on a manifold of a three-dimensional special orthogonal group (SO 3 ). A product manifold of N rotation matrices is the product manifold SO 3   N . Thus the Cartesian product of N rotation matrices R is a point on the product manifold SO 3   N . 
     The i th  view consists of a point set having n i  points W i ={w i   k εR 3 |k=1, 2, . . . , n i }⊂W and is denoted V i ={v i   k εR 3 |k=1, 2, . . . , n i } and consists of images of the n i  points in W i  with the relative rotation matrices and translation vectors given by (R i , t i ). Thus in a noise free case,
 
 W   i   k   =R   i   v   i   k   +t   i   , k= 1,2 , . . . , n   i .  (1)
 
     When overlapping views are obtained, the points of one view that correspond to points in an overlapping view are known. Theses corresponding points in different view are referred to herein and correspondence values. Accordingly, W ij =W i ∩W j  can be the set of n ij  points in W i  for which there are corresponding points in W j , for i, j=1, 2, . . . , N. That is, W ij =W ji  consists of n ij =n ji  points w ij   k =w ji   k εR 3 , k=1, . . . , n ij . In view V i , the set of images of these points is denoted V ij :={v ij   k εR 3 |k=1, 2, . . . , n ij }⊂V i . In a noise free case,
 
 w   ij   k   =R   i   v   ij   k   +t   i   =R   j   v   ji   k   +t   j   ∀i,j= 1,2 , . . . , N, k= 1,2 , . . . , n   ij.   (2)
 
     When there is measurement noise, cost function can be used to show error between corresponding points in overlapping views. Accordingly, the cost function can penalize the error (R i v ij   k +t i )−(R j v ji   k +t j ) for all i, j=1, 2, . . . , N and k=1, 2, . . . , n ij . In one embodiment of the present invention, the cost function can be given by a sum of the squared Euclidean distances between the corresponding points in overlapping views. In this case the cost function can be expressed, 
     
       
         
           
             
               
                 
                   
                     
                       
                         
                           
                             g 
                             ⁡ 
                             
                               ( 
                               
                                 R 
                                 , 
                                 T 
                               
                               ) 
                             
                           
                           = 
                             
                           ⁢ 
                           
                             
                               ∑ 
                               
                                 i 
                                 = 
                                 1 
                               
                               N 
                             
                             ⁢ 
                             
                               
                                 ∑ 
                                 
                                   j 
                                   = 
                                   
                                     i 
                                     + 
                                     1 
                                   
                                 
                                 N 
                               
                               ⁢ 
                               
                                 
                                   ∑ 
                                   
                                     k 
                                     = 
                                     1 
                                   
                                   nij 
                                 
                                 ⁢ 
                                 
                                   
                                      
                                     
                                       
                                         ( 
                                         
                                           
                                             
                                               R 
                                               i 
                                             
                                             ⁢ 
                                             
                                               v 
                                               ij 
                                               k 
                                             
                                           
                                           + 
                                           
                                             t 
                                             i 
                                           
                                         
                                         ) 
                                       
                                       - 
                                       
                                         ( 
                                         
                                           
                                             
                                               R 
                                               j 
                                             
                                             ⁢ 
                                             
                                               v 
                                               ji 
                                               k 
                                             
                                           
                                           + 
                                           
                                             t 
                                             j 
                                           
                                         
                                         ) 
                                       
                                     
                                      
                                   
                                   2 
                                 
                               
                             
                           
                         
                         , 
                       
                     
                   
                   
                     
                       
                         = 
                           
                         ⁢ 
                         
                           
                             ∑ 
                             
                               i 
                               = 
                               1 
                             
                             N 
                           
                           ⁢ 
                           
                             
                               ∑ 
                               
                                 j 
                                 = 
                                 
                                   i 
                                   + 
                                   1 
                                 
                               
                               N 
                             
                             ⁢ 
                             
                               
                                 ∑ 
                                 
                                   k 
                                   = 
                                   1 
                                 
                                 nij 
                               
                               ⁢ 
                               
                                 ( 
                                 
                                   
                                     
                                        
                                       
                                         
                                           
                                             R 
                                             i 
                                           
                                           ⁢ 
                                           
                                             v 
                                             ij 
                                             k 
                                           
                                         
                                         - 
                                         
                                           
                                             R 
                                             j 
                                           
                                           ⁢ 
                                           
                                             v 
                                             ji 
                                             k 
                                           
                                         
                                       
                                        
                                     
                                     2 
                                   
                                   + 
                                 
                               
                             
                           
                         
                       
                     
                   
                   
                     
                       
                         
                             
                           ⁢ 
                           
                             
                               2 
                               ⁢ 
                               
                                 
                                   ( 
                                   
                                     
                                       t 
                                       i 
                                     
                                     - 
                                     
                                       t 
                                       j 
                                     
                                   
                                   ) 
                                 
                                 T 
                               
                               ⁢ 
                               
                                 ( 
                                 
                                   
                                     
                                       R 
                                       i 
                                     
                                     ⁢ 
                                     
                                       v 
                                       ij 
                                       k 
                                     
                                   
                                   - 
                                   
                                     
                                       R 
                                       j 
                                     
                                     ⁢ 
                                     
                                       v 
                                       ji 
                                       k 
                                     
                                   
                                 
                                 ) 
                               
                             
                             + 
                             
                               
                                  
                                 
                                   
                                     t 
                                     i 
                                   
                                   - 
                                   
                                     t 
                                     j 
                                   
                                 
                                  
                               
                               2 
                             
                           
                           ) 
                         
                         . 
                       
                     
                   
                 
               
               
                 
                   ( 
                   3 
                   ) 
                 
               
             
           
         
       
     
     Using matrix operations, it is possible to reformulate the cost function expressed in Equation 3. R is the Cartesian product of all of the rotational matrices, and T is the set of all of the translation vectors, as expressed below is Equations 4 and 5, respectively,
 
 R:=[R   1   R   2    . . . R   N   ]εR   3×3N   (4)
 
 T:=[t   1   t   2    . . . t   N   ]εR   3×N.   (5)
 
     If e i  denotes the i th  column of an N×N identity matrix and e ij :=e i −e j , then:
 
 R   i   =R ( e   i   T     I   3 ), t   i   =Te   i   ,t   i   −t   j   =Te   ij.   (6)
 
     Using the correspondence values of overlapping views, a vector can be constructed such that a ij   k :=(e i   T   I 3 )v ij   k −(e j   T   I 3 )v ji   k . This vector can be combined with the value of R i  from Equation 6, giving R i v ij   k −R j v ji   k =Ra ij   k . Thus, ∥R i v ij   k −R j v ji   k ∥ 2 =Ra ij   k ·Ra ij   k . Similarly substituting the value of t i , the inner expression of Equation 3 can be rewritten as Ra ij   k ·Ra ij   k +2Te ij ·Ra ij   k +Te ij ·Te ij . 
     It is possible to construct a matrix such that: 
     
       
         
           
             
               
                 
                   
                     [ 
                     
                       
                         
                           A 
                         
                         
                           B 
                         
                       
                       
                         
                           
                             B 
                             T 
                           
                         
                         
                           C 
                         
                       
                     
                     ] 
                   
                   = 
                   
                     
                       
                         ∑ 
                         
                           i 
                           = 
                           1 
                         
                         N 
                       
                       ⁢ 
                       
                         
                           ∑ 
                           
                             j 
                             = 
                             
                               i 
                               + 
                               1 
                             
                           
                           N 
                         
                         ⁢ 
                         
                           
                             ∑ 
                             
                               k 
                               = 
                               1 
                             
                             nij 
                           
                           ⁢ 
                           
                             
                               [ 
                               
                                 
                                   
                                     
                                       a 
                                       ij 
                                       k 
                                     
                                   
                                 
                                 
                                   
                                     
                                       e 
                                       ij 
                                     
                                   
                                 
                               
                               ] 
                             
                             ⁢ 
                             
                               ⌊ 
                               
                                 
                                   
                                     
                                       a 
                                       ij 
                                       kT 
                                     
                                   
                                   
                                     
                                       e 
                                       ij 
                                       T 
                                     
                                   
                                 
                               
                               ⌋ 
                             
                           
                         
                       
                     
                     ≥ 
                     0. 
                   
                 
               
               
                 
                   ( 
                   7 
                   ) 
                 
               
             
           
         
       
     
     Equation 7 can be used with a trace operation (tr), to rewrite the cost function. Using the fact that for vectors u and v, u·v=tr(uv T ), the cost function as expressed in Equation 3, can be rewritten as: 
     
       
         
           
             
               
                 
                   
                     
                       
                         
                           g 
                           ⁡ 
                           
                             ( 
                             
                               R 
                               , 
                               T 
                             
                             ) 
                           
                         
                         = 
                         
                           tr 
                           ⁡ 
                           
                             ( 
                             
                               
                                 RAR 
                                 T 
                               
                               + 
                               
                                 2 
                                 ⁢ 
                                 
                                   RBT 
                                   T 
                                 
                               
                               + 
                               
                                 TCT 
                                 T 
                               
                             
                             ) 
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                         
                           
                             tr 
                             ⁡ 
                             
                               ( 
                               
                                 
                                   
                                     [ 
                                     
                                       
                                         
                                           R 
                                         
                                         
                                           T 
                                         
                                       
                                     
                                     ] 
                                   
                                   ⁡ 
                                   
                                     [ 
                                     
                                       
                                         
                                           A 
                                         
                                         
                                           B 
                                         
                                       
                                       
                                         
                                           
                                             B 
                                             T 
                                           
                                         
                                         
                                           C 
                                         
                                       
                                     
                                     ] 
                                   
                                 
                                 ⁡ 
                                 
                                   [ 
                                   
                                     
                                       
                                         
                                           R 
                                           T 
                                         
                                       
                                     
                                     
                                       
                                         
                                           T 
                                           T 
                                         
                                       
                                     
                                   
                                   ] 
                                 
                               
                               ) 
                             
                           
                           ≥ 
                           0. 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   8 
                   ) 
                 
               
             
           
         
       
     
     The vec operation is a well know matrix operation. If a matrix X is an n×k matrix, vec(X) is a nk×1 vector form by writing the columns of X one at a time. It is well know that for two matrices X and Y, tr(XY T )=vec T (X)vec(Y). Accordingly, the cost function as expressed in Equation 8 can be rewritten as:
 
 g ( R,T )= tr ( RAR   T )+2vec T ( T )vec( RB )+vec T ( T )( C     I   3 )vec( T ).  (9)
 
     The cost function as expressed in Equation 9 is a quadratic function of vec(T). This function is convex, and thus has a unique minimum, if C I 3  is positive definite. An element c ii  of C is equal to Σ k≠i n ik  and c ij =−n ij  for j≠i. This implies that C is singular, since C1 (where 1 is an all-ones vector) vanishes (C1=0, where 0 is a zero vector). This is a consequence of the fact that only relative transformations (i.e., orientation and position) of the coordinate frames of the views can be determined, and not absolute transformations. That is, it is only possible to determine the relative position and orientation of each coordinate frame with respect to the other coordinate frames. Accordingly, a coordinate frame of one of the views (i.e., a first coordinate frame corresponding to a first view) can be fixed as a reference frame such that (R i , t i )=(I 3 , 0), where 0 is a zero vector. The reference frame is used as a common coordinate frame for registering the 3d point sets of the multiple view. Since the coordinate frame of the first view can be fixed as the reference frame, the first row and the first column from all of the above described matrices can be removed. 
     Eliminating the first row and column from C leaves a matrix that is symmetric and strictly dominant, i.e., the absolute value of each diagonal element is strictly larger than the absolute values of off-diagonal entries in the same row. It is a basic property that such matrices are positive definite, which consequently implies that C I 3  is positive definite. Thus, the cost function g(R, T) has a unique minimum for a fixed R and varying T. Thus the value of T which minimizes the cost function can be expressed in terms of R as:
 
vec( T *( R ))=−( C   −1     I   3 )vec( RB )=−vec( RBC   −1 )
 
 T *( R )=− RBC   −1   (10)
 
     T*(R) as expressed in Equation 10 can be substituted into the cost function as expressed in Equation 8 in order to express the registration error cost function depending only on the rotation matrices R: 
     
       
         
           
             
               
                 
                   
                     
                       
                         
                           
                             f 
                             ⁢ 
                             
                               ( 
                               R 
                               ) 
                             
                           
                           := 
                             
                           ⁢ 
                           
                             g 
                             ⁡ 
                             
                               ( 
                               
                                 R 
                                 , 
                                 
                                   T 
                                   ⁡ 
                                   
                                     ( 
                                     R 
                                     ) 
                                   
                                 
                               
                               ) 
                             
                           
                         
                       
                     
                     
                       
                         
                           = 
                             
                           ⁢ 
                           
                             tr 
                             ⁡ 
                             
                               ( 
                               RMR 
                               ) 
                             
                           
                         
                       
                     
                     
                       
                         
                           
                             = 
                               
                             ⁢ 
                             
                               
                                 
                                   vec 
                                   T 
                                 
                                 ⁡ 
                                 
                                   ( 
                                   
                                     R 
                                     T 
                                   
                                   ) 
                                 
                               
                               ⁢ 
                               
                                 ( 
                                 
                                   
                                     I 
                                     3 
                                   
                                   ⊗ 
                                   M 
                                 
                                 ) 
                               
                               ⁢ 
                               
                                 vec 
                                 ⁡ 
                                 
                                   ( 
                                   
                                     R 
                                     T 
                                   
                                   ) 
                                 
                               
                             
                           
                           , 
                         
                       
                     
                   
                   ⁢ 
                   
                     
 
                   
                   ⁢ 
                   wherein 
                   ⁢ 
                   
                     
 
                   
                   ⁢ 
                   
                     M 
                     := 
                     
                       A 
                       - 
                       
                         
                           BC 
                           
                             - 
                             1 
                           
                         
                         ⁢ 
                         
                           
                             B 
                             T 
                           
                           . 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   11 
                   ) 
                 
               
             
           
         
       
     
     The cost function as expressed in Equation 11 can be used to determine optimal rotation matrices which minimize the cost function in order to register multiple 3D point sets (views) with a minimal amount of registration error.  FIG. 1  illustrates such a method of registering multiple 3D point sets according to an embodiment of the present invention. At step  110 , values of the rotational matrices corresponding to the 3D point sets are initialized. That is, initial values of the rotational matrices are determined. 
     In a noise free case, for RεSO 3   N , the optimal value of the cost function is zero, such that:
 
vec T ( R   T )vec( MR   T )=0 vec( MR   T )=0
 
 MR T =0.  (12)
 
     Since M is symmetric, a singular value decomposition of M gives an orthogonal matrix U as expressed below: 
     
       
         
           
             
               
                 
                   M 
                   = 
                   
                     
                       U 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       Σ 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         U 
                         T 
                       
                     
                     = 
                     
                       
                         
                           
                             
                               [ 
                               
                                 
                                   U 
                                   a 
                                 
                                 ⁢ 
                                 
                                     
                                 
                                 ⁢ 
                                 
                                   U 
                                   b 
                                 
                               
                               ] 
                             
                             ⁡ 
                             
                               [ 
                               
                                 
                                   
                                     
                                       Σ 
                                       a 
                                     
                                   
                                   
                                     0 
                                   
                                 
                                 
                                   
                                     0 
                                   
                                   
                                     0 
                                   
                                 
                               
                               ] 
                             
                           
                           ⁡ 
                           
                             [ 
                             
                               
                                 
                                   
                                     U 
                                     a 
                                     T 
                                   
                                 
                               
                               
                                 
                                   
                                     U 
                                     b 
                                     T 
                                   
                                 
                               
                             
                             ] 
                           
                         
                         ⁢ 
                         
                           
 
                         
                         ⇒ 
                         
                           MU 
                           b 
                         
                       
                       = 
                       0. 
                     
                   
                 
               
               
                 
                   ( 
                   13 
                   ) 
                 
               
             
           
         
       
     
     To obtain R such that R 1 =I 3 , it is possible to define an orthogonal matrix Û such that Û:=[I 3  0]U b . Thus, in a noise free case, a closed form solution for the values of the rotation matrices can be determined to minimize the cost function. In a noise free case, this closed form solution can be used to obtain optimal values of the rotation matrices without an iterative optimization process. The closed form solution for the values of the rotational matrices can be expressed as:
 
 R=Û   −T   U   b   T.   (14)
 
     However, in the presence of noise, the optimal cost function is no longer equal to zero. In this case, U b  is chosen to be the set of right singular vectors associated with three least singular values of M, which may not be zero. These singular vectors might not be on the product manifold SO 3   N . Thus, an additional projection step can be used to obtain initial values of the rotation matrices that are on the product manifold SO 3   N . Denoting G i :=Û −T U b   T (e i   I 3 ), the initial value for each rotation matrix can be expressed as: 
     
       
         
           
             
               
                 
                   
                     R 
                     i 
                     opt 
                   
                   = 
                   
                     
                       arg 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         
                           min 
                           
                             
                               R 
                               i 
                             
                             ∈ 
                             
                               SO 
                               3 
                             
                           
                         
                         ⁢ 
                         
                            
                           
                             
                               R 
                               i 
                             
                             - 
                             
                               G 
                               i 
                             
                           
                            
                         
                       
                     
                     = 
                     
                       arg 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         
                           max 
                           
                             
                               R 
                               i 
                             
                             ∈ 
                             
                               SO 
                               3 
                             
                           
                         
                         ⁢ 
                         
                           
                             tr 
                             ⁡ 
                             
                               ( 
                               
                                 
                                   R 
                                   i 
                                   T 
                                 
                                 ⁢ 
                                 
                                   G 
                                   i 
                                 
                               
                               ) 
                             
                           
                           . 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   15 
                   ) 
                 
               
             
           
         
       
     
     By applying a singular value decomposition on G i , it is possible to use Equation 15 to determine the initial value for each rotation matrix, as shown below: 
     
       
         
           
             
               
                 
                   
                     
                       G 
                       i 
                     
                     = 
                     
                       W 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       Λ 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         Z 
                         T 
                       
                     
                   
                   , 
                   
                     
                       R 
                       i 
                       opt 
                     
                     = 
                     
                       
                         W 
                         ⁡ 
                         
                           [ 
                           
                             
                               
                                 
                                   I 
                                   2 
                                 
                               
                               
                                 0 
                               
                             
                             
                               
                                 0 
                               
                               
                                 
                                   det 
                                   ⁡ 
                                   
                                     ( 
                                     
                                       WZ 
                                       T 
                                     
                                     ) 
                                   
                                 
                               
                             
                           
                           ] 
                         
                       
                       ⁢ 
                       
                         Z 
                         T 
                       
                     
                   
                   , 
                   
                     
 
                   
                   ⁢ 
                   wherein 
                   , 
                   
                     
                       det 
                       ⁡ 
                       
                         ( 
                         
                           R 
                           i 
                           opt 
                         
                         ) 
                       
                     
                     = 
                     
                       + 
                       1. 
                     
                   
                 
               
               
                 
                   ( 
                   16 
                   ) 
                 
               
             
           
         
       
     
     At step  120 , the cost function is optimized on the product manifold of all of the rotation matrices using the initial values of the rotation matrices to determine optimal values of the rotation matrices. As described above, the rotational matrix corresponding to each 3D point set is a point on a manifold of the special orthogonal (SO) group. In particular, each rotational matrix is a point on the manifold of SO 3 . SO 3  denotes the group of 3×3 matrices with determinant +1, and R i εSO 3  for i=1, . . . , N. Furthermore, R, which is the Cartesian product of all of the rotational matrices, is a point on a product manifold of SO 3   N . 
     As known in the art, a Lie group is defined as a differential manifold G equipped with a product ·: G×G→G that satisfies the group axioms of associativity, identity, and has an inverse p −1 . Further, the maps (p,r) p·r, p p −1 . A Lie algebra is defined herein as a linear space V equipped with a Lie bracket, which is a bilinear skew-symmetric mapping [·,·]: V×V→V that obeys the following properties:
 
Skew Symmetry: [ F,G]=−[G,F];  
 
Scalar Multipliers: [α F,G]=α[F,G]∀αεR;  
 
Bilinearity: [ F+G,H]=[F,H]+[G,H ]; and
 
Jacobi&#39;s Identity: [ F,[G,H]]+[G,[H,F]]+[H,[F,G]]= 0.
 
     A vector field on a manifold G is a function that maps a point p on G to an element of the tangent space T p (G). A left invariant vector field X is a vector field such that for all gεG, XL g =L g X, where X is viewed as a differential operator and L g [f](x)=f(g·x). The vector space of all left invariant vector fields over a Lie group G is a Lie algebra with the operator [X, Y]=XY−YX, and is considered the associated Lie algebra g. 
     SO 3  is a Lie group with the group operator being matrix multiplication. Its associated Lie algebra so 3  is the set of 3×3 skew symmetric matrices of the form: 
     
       
         
           
             
               
                 
                   Ω 
                   = 
                   
                     
                       [ 
                       
                         
                           
                             0 
                           
                           
                             
                               - 
                               
                                 ω 
                                 z 
                               
                             
                           
                           
                             
                               ω 
                               y 
                             
                           
                         
                         
                           
                             
                               ω 
                               z 
                             
                           
                           
                             0 
                           
                           
                             
                               - 
                               
                                 ω 
                                 x 
                               
                             
                           
                         
                         
                           
                             
                               - 
                               
                                 ω 
                                 y 
                               
                             
                           
                           
                             
                               ω 
                               x 
                             
                           
                           
                             0 
                           
                         
                       
                       ] 
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   17 
                   ) 
                 
               
             
           
         
       
     
     There is a well known isomorphism from the Lie algebra (R 3 , x) to the Lie algebra (so 3 , [·,·]), wherein x denotes the cross product and [·,·] denotes the matrix communicator. This allows so 3  to be identified with R 3  using the mapping expressed in Equation 17, which maps a vector ω=[ω x  ω y  ω z ]εR 3  to a matrix Ωεso 3 . This isomorphism is expressed in Equations 18 and 19 below: 
     
       
         
           
             
               
                 
                   
                     
                       
                         Q 
                         x 
                       
                       := 
                       
                         [ 
                         
                           
                             
                               0 
                             
                             
                               0 
                             
                             
                               0 
                             
                           
                           
                             
                               0 
                             
                             
                               0 
                             
                             
                               
                                 - 
                                 1 
                               
                             
                           
                           
                             
                               0 
                             
                             
                               1 
                             
                             
                               0 
                             
                           
                         
                         ] 
                       
                     
                     , 
                     
                       
 
                     
                     ⁢ 
                     
                       
                         Q 
                         y 
                       
                       := 
                       
                         [ 
                         
                           
                             
                               0 
                             
                             
                               0 
                             
                             
                               1 
                             
                           
                           
                             
                               0 
                             
                             
                               0 
                             
                             
                               0 
                             
                           
                           
                             
                               
                                 - 
                                 1 
                               
                             
                             
                               0 
                             
                             
                               0 
                             
                           
                         
                         ] 
                       
                     
                     , 
                     
                       
 
                     
                     ⁢ 
                     and 
                   
                   ⁢ 
                   
                     
 
                   
                   ⁢ 
                   
                     
                       
                         Q 
                         z 
                       
                       := 
                       
                         [ 
                         
                           
                             
                               0 
                             
                             
                               
                                 - 
                                 1 
                               
                             
                             
                               0 
                             
                           
                           
                             
                               1 
                             
                             
                               0 
                             
                             
                               0 
                             
                           
                           
                             
                               0 
                             
                             
                               0 
                             
                             
                               0 
                             
                           
                         
                         ] 
                       
                     
                     ; 
                   
                 
               
               
                 
                   ( 
                   18 
                   ) 
                 
               
             
             
               
                 
                   Ω 
                   = 
                   
                     
                       Ω 
                       ⁡ 
                       
                         ( 
                         ω 
                         ) 
                       
                     
                     = 
                     
                       
                         
                           Q 
                           x 
                         
                         ⁢ 
                         
                           ω 
                           x 
                         
                       
                       + 
                       
                         
                           Q 
                           y 
                         
                         ⁢ 
                         
                           ω 
                           y 
                         
                       
                       + 
                       
                         
                           Q 
                           z 
                         
                         ⁢ 
                         
                           
                             ω 
                             z 
                           
                           . 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   19 
                   ) 
                 
               
             
           
         
       
     
     An identity which is used in optimizing the cost function is vec(Ω T )=Qω. The optimization is performed on an N-fold product of SO 3  which is a smooth manifold of dimension 3N, given by: 
     
       
         
           
             
               
                 
                   
                     SO 
                     3 
                     N 
                   
                   = 
                   
                     
                       
                         
                           
                             SO 
                             3 
                           
                           ⁢ 
                           x 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           … 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             xSO 
                             3 
                           
                         
                         ︷ 
                       
                       
                         N 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         times 
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   20 
                   ) 
                 
               
             
           
         
       
     
     The tangent space of SO 3  at any point R i  (i.e., rotation matrix) on the SO 3  manifold is defined as T R     i   SO 3 ={R i Ω i |Ω i εso 3 }, and the affine tangent space is T R     i     aff SO 3 {R i +R i Ω i |Ω i εso 3 }. It is possible to define:
 
{tilde over (Ω)}:=Ω 1 ⊕Ω 2    . . .  Ω N ,Ω i εso 3 .  (21)
 
     Due to the isomorphism described above, the tangent space of the product manifold of SO 3   N  a point R=[R 1  R 2  . . . R n ] (i.e., the set of all rotation matrices) can be identified as T R SO 3   N =R{tilde over (Ω)}, and the affine tangent space is T R   aff SO 3   N =R+R{tilde over (Ω)}. 
     In order to optimize the cost function on the product manifold SO 3   N , the product manifold can be locally parameterized at a neighborhood of a point R on the product manifold SO 3   N . If N(0)⊂R 3  denotes a sufficiently small neighborhood of the origin in R 3 , and R i εSO 3 , an exponential mapping μ is a local diffeomorphism from N(0) onto a neighborhood of R i  in SO 3 . This exponential mapping μ can be expressed as:
 
μ: N (0)⊂ R   3 →SO 3 ,ω i     R   i   e   Ω     i     (ω     i     ) .  (22)
 
     As expressed in Equation 22, the exponential mapping μ converts a three dimensional vector ω i  into a point on the SO 3  manifold in the neighborhood of R i  using skew symmetric matrix corresponding to the three dimensional vector ω i . Similarly, due to the above described isomorphism, an exponential mapping φ can be used to locally parameterize the product manifold SO 3   N  at RεSO 3   N . This exponential mapping φ can be expressed as:
 
φ: N (0)× . . . × N (0)⊂ R   3N →SO 3   N ,
 
     
       
         
           
             
               
                 
                   ω 
                   = 
                   
                     
                       
                         [ 
                         
                           
                             
                               
                                 ω 
                                 1 
                               
                             
                           
                           
                             
                               
                                 ω 
                                 2 
                               
                             
                           
                           
                             
                               ⋮ 
                             
                           
                           
                             
                               
                                 ω 
                                 N 
                               
                             
                           
                         
                         ] 
                       
                       ↦ 
                       
                         R 
                         ⁡ 
                         
                           ( 
                           
                             
                               ⅇ 
                               
                                 Ω 
                                 ⁡ 
                                 
                                   ( 
                                   
                                     ω 
                                     1 
                                   
                                   ) 
                                 
                               
                             
                             ⊕ 
                             
                               ⅇ 
                               
                                 Ω 
                                 ⁡ 
                                 
                                   ( 
                                   
                                     ω 
                                     2 
                                   
                                   ) 
                                 
                               
                             
                             ⊕ 
                             … 
                             ⊕ 
                             
                               ⅇ 
                               
                                 Ω 
                                 ⁡ 
                                 
                                   ( 
                                   
                                     ω 
                                     N 
                                   
                                   ) 
                                 
                               
                             
                           
                           ) 
                         
                       
                     
                     = 
                     
                       R 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         
                           ⅇ 
                           
                             
                               Ω 
                               ~ 
                             
                             ⁡ 
                             
                               ( 
                               ω 
                               ) 
                             
                           
                         
                         . 
                       
                     
                   
                 
               
               
                 
                   ( 
                   23 
                   ) 
                 
               
             
           
         
       
     
     As expressed in Equation 23, the exponential mapping φ converts a 3N dimensional vector to point on the product manifold SO 3   N  in the neighborhood of R. 
     In order to obtain optimal values for the rotation matrices, an iterative optimization method is performed to minimize the cost function while remaining on the product manifold SO 3   N , using the properties of the SO 3   N  manifold described above.  FIG. 2  further describes step  120  of  FIG. 1 . That is,  FIG. 2  illustrates a method of optimizing the cost function on the product manifold SO 3   N  according to an embodiment of the present invention. 
     At step  210 , a local approximation of the cost function f (Equation 11) in the neighborhood of R on the product manifold SO 3   N  is constructed. The local approximation of f can be calculated using a second order Taylor expansion. Instead of differentiating f, the local parameterization of SO 3   N  can be used to locally approximate the function f∘φ, whose domain is R 3N . The use of the local parameterization φ ensures that the optimization method stays on the manifold. During the first iteration of the optimization method, the R is the rotational matrices having the initial values determined at step  110 . For each subsequent iteration, the value of R is updated. 
     The second order Taylor approximation of f∘φ is given by the function: 
     
       
         
           
             
               
                 
                   
                     
                       
                         j 
                         0 
                         
                           ( 
                           2 
                           ) 
                         
                       
                       ⁡ 
                       
                         ( 
                         
                           f 
                           ∘ 
                           φ 
                         
                         ) 
                       
                     
                     : 
                     
                       
                         R 
                         
                           3 
                           ⁢ 
                           N 
                         
                       
                       → 
                       R 
                     
                   
                   , 
                   
                     
 
                   
                   ⁢ 
                   
                     
                       ω 
                       ↦ 
                       
                         ( 
                         
                           
                             
                               ( 
                               
                                 f 
                                 ∘ 
                                 φ 
                               
                               ) 
                             
                             ⁢ 
                             
                               ( 
                               
                                 t 
                                 ⁢ 
                                 
                                     
                                 
                                 ⁢ 
                                 ω 
                               
                               ) 
                             
                           
                           + 
                           
                             
                               ⅆ 
                               
                                 ⅆ 
                                 t 
                               
                             
                             ⁢ 
                             
                               ( 
                               
                                 f 
                                 ∘ 
                                 φ 
                               
                               ) 
                             
                             ⁢ 
                             
                               ( 
                               
                                 t 
                                 ⁢ 
                                 
                                     
                                 
                                 ⁢ 
                                 ω 
                               
                               ) 
                             
                           
                           + 
                           
                             
                               1 
                               2 
                             
                             ⁢ 
                             
                               
                                 ⅆ 
                                 2 
                               
                               
                                 ⅆ 
                                 
                                   t 
                                   2 
                                 
                               
                             
                             ⁢ 
                             
                               ( 
                               
                                 f 
                                 ∘ 
                                 φ 
                               
                               ) 
                             
                             ⁢ 
                             
                               ( 
                               
                                 t 
                                 ⁢ 
                                 
                                     
                                 
                                 ⁢ 
                                 ω 
                               
                               ) 
                             
                           
                         
                         ) 
                       
                     
                     ⁢ 
                     
                       | 
                       
                         t 
                         = 
                         0 
                       
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   24 
                   ) 
                 
               
             
           
         
       
     
     As with a univariate Taylor expansion, the above expression can be written in the form 
                   (     f   ∘   φ     )     ⁢     (     t   ⁢           ⁢   ω     )       +       ω   T     ⁢     ∇     +     1   2         ⁢     ω   T     ⁢   H   ⁢           ⁢   ω       ,         
where ∇ is the gradient and H is the Hessian of the function f∘φ. Accordingly, the first term of the cost function as expressed in Equation 11 is (f∘φ)(tω)| t=0 =tr (RMR T ). The second term is:
 
     
       
         
           
             
               
                 
                   
                     
                       
                         
                           
                             
                               ⅆ 
                               
                                 ⅆ 
                                 t 
                               
                             
                             ⁢ 
                             
                               ( 
                               
                                 f 
                                 ∘ 
                                 ω 
                               
                               ) 
                             
                             ⁢ 
                             
                               ( 
                               
                                 t 
                                 ⁢ 
                                 
                                     
                                 
                                 ⁢ 
                                 ω 
                               
                               ) 
                             
                           
                           ⁢ 
                           
                             | 
                             
                               t 
                               = 
                               0 
                             
                           
                         
                         = 
                           
                         ⁢ 
                         
                           2 
                           ⁢ 
                           
                             tr 
                             ⁡ 
                             
                               ( 
                               
                                 R 
                                 ⁢ 
                                 
                                     
                                 
                                 ⁢ 
                                 
                                   Ω 
                                   ~ 
                                 
                                 ⁢ 
                                 
                                     
                                 
                                 ⁢ 
                                 
                                   MR 
                                   T 
                                 
                               
                               ) 
                             
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                           
                         ⁢ 
                         
                           2 
                           ⁢ 
                           
                             ω 
                             T 
                           
                           ⁢ 
                           
                             ∇ 
                             
                               ( 
                               
                                 f 
                                 ∘ 
                                 φ 
                               
                               ) 
                             
                           
                           ⁢ 
                           
                             
                               ( 
                               0 
                               ) 
                             
                             . 
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   25 
                   ) 
                 
               
             
           
         
       
     
     Furthermore, tr(R{tilde over (Ω)}MR T ) can be written as vec T ({tilde over (Ω)}R T ) vec(MR T ). Thus, 
     
       
         
           
             
               
                 
                   
                     
                       
                         
                           
                             vec 
                             T 
                           
                           ⁡ 
                           
                             ( 
                             
                               
                                 Ω 
                                 ~ 
                               
                               ⁢ 
                               
                                   
                               
                               ⁢ 
                               
                                 R 
                                 T 
                               
                             
                             ) 
                           
                         
                         = 
                           
                         ⁢ 
                         
                           
                             [ 
                             
                               vec 
                               ⁡ 
                               
                                 ( 
                                 
                                   
                                     Ω 
                                     ~ 
                                   
                                   ⁢ 
                                   
                                       
                                   
                                   ⁢ 
                                   
                                     R 
                                     T 
                                   
                                 
                                 ) 
                               
                             
                             ] 
                           
                           T 
                         
                       
                     
                   
                   
                     
                       
                         = 
                           
                         ⁢ 
                         
                           
                             [ 
                             
                               vec 
                               ⁡ 
                               
                                 ( 
                                 
                                   
                                     I 
                                     
                                       3 
                                       ⁢ 
                                       N 
                                     
                                   
                                   ⁢ 
                                   
                                     Ω 
                                     ~ 
                                   
                                   ⁢ 
                                   
                                       
                                   
                                   ⁢ 
                                   
                                     R 
                                     T 
                                   
                                 
                                 ) 
                               
                             
                             ] 
                           
                           T 
                         
                       
                     
                   
                   
                     
                       
                         = 
                           
                         ⁢ 
                         
                           
                             
                               [ 
                               
                                 
                                   ( 
                                   
                                     R 
                                     ⊗ 
                                     
                                       I 
                                       
                                         3 
                                         ⁢ 
                                         N 
                                       
                                     
                                   
                                   ) 
                                 
                                 ⁢ 
                                 
                                   vec 
                                   ( 
                                   
                                     Ω 
                                     ~ 
                                   
                                   ⁢ 
                                   
                                       
                                   
                                   ) 
                                 
                               
                               ] 
                             
                             T 
                           
                           . 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   26 
                   ) 
                 
               
             
           
         
       
     
     At step  220 , the gradient ∇ of the local approximation of the cost function f∘φ is calculated at R. R is set to the initial values determined at step  110  during the first iteration of the optimization method, and is updated for each subsequent iteration. If 
                 Q   ~     :=       Q     e   1       ⊕     Q     e   2       ⊕   …   ⊕     Q     e   N           ,     
     ⁢   and                   Q     e   i       :=     [             e   i     ⊗     Q   x                   e   i     ⊗     Q   y                   e   i     ⊗     Q   z             ]       ,         
then using Equation 18 and the fact that vec({tilde over (Ω)})={tilde over (Ω)}ω, Equation 26 can be expressed as vec T ({tilde over (Ω)}R T )=ω T J, wherein J:=(R I 3N ){tilde over (Q)}. Substituting this back into Equation 25, the gradient ∇f the local approximation of the cost function f∘φ can be expressed as:
 
∇( f ∘φ)(0)= J   T vec( MR   T ).  (27)
 
     At step  230 , the Hessian H of the local approximation of the cost function f∘φ is calculated. The quadratic term in Equation 11 consists of a sum of two terms. The first term can be expressed as:
 
 tr ( R{tilde over (Ω)}M{tilde over (Ω)}   T   R   T )=ω T   Ĥ   (f∘φ)(0) ω.  (28)
 
     The second term can be expressed as: 
     
       
         
           
             
               
                 
                   
                     
                       
                         
                           tr 
                           ⁡ 
                           
                             ( 
                             
                               R 
                               ⁢ 
                               
                                   
                               
                               ⁢ 
                               
                                 
                                   Ω 
                                   ~ 
                                 
                                 2 
                               
                               ⁢ 
                               
                                 MR 
                                 T 
                               
                             
                             ) 
                           
                         
                         = 
                           
                         ⁢ 
                         
                           
                             vec 
                             T 
                           
                           ⁡ 
                           
                             ( 
                             
                               
                                 MR 
                                 T 
                               
                               ⁢ 
                               R 
                               ⁢ 
                               
                                   
                               
                               ⁢ 
                               
                                 Ω 
                                 ~ 
                               
                             
                             ) 
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                           
                         ⁢ 
                         
                           
                             ω 
                             T 
                           
                           ⁢ 
                           
                             
                               
                                 H 
                                 ~ 
                               
                               
                                 
                                   ( 
                                   
                                     f 
                                     ∘ 
                                     
                                         
                                     
                                     ⁢ 
                                     φ 
                                   
                                   ) 
                                 
                                 ⁢ 
                                 
                                   
                                     ( 
                                     0 
                                     ) 
                                   
                                   ω 
                                 
                               
                             
                             . 
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   29 
                   ) 
                 
               
             
           
         
       
     
     By applying similar methods as used to obtain the gradient ∇, the Hessian H of the local approximation of the cost function f∘φ can be evaluated at zero, as expressed below:
 
 H   (f∘φ)(0)   =Ĥ   (f∘φ)(0)   +{tilde over (H)}   (f∘φ)(0),   (30)
 
wherein,
 
 Ĥ   (f∘φ)(0)   =J   T ( I   3     M ) J≧ 0  (31)
 
 {tilde over (H)}   (f∘φ)(0)   =−{tilde over (Q)}   T ( I   3N     MR   T   R ) {tilde over (Q)}.   (32)
 
     As expressed in Equations 30-32, H is the sum of the positive semidefinite term Ĥ and the term {tilde over (H)}. 
     At step  240 , a descent direction ω opt  is calculated based on the gradient ∇ and the Hessian H of the local approximation of the cost function f∘φ. At step  250 , a step length λ opt  is calculated based on the descent direction ω opt . The optimization method comprises the iteration,
 
 s=π   2 ∘π 1 :SO 3   N →SO 3   N .  (33)
 
     In Equation 33, π 1  maps a point R on the product manifold SO 3   N  to an element in the tangent space at the point R that minimizes j 0   (2) (f∘φ)(0), and π 2  maps the element back to SO 3   N  using the parameterization φ. The mapping π 1  is an iterative scheme that uses a modified Newton method to determine the descent direction ω opt , and a line search to move along this direction. The steps of calculating the descent direction ω opt  and the step length λ opt  are described by the mapping:
 
π 1 =π 1   b ∘π 1   a :SO 3   N →R 3X×3N.   (34)
 
     The mapping π 1   a  is used to calculate the descent direction ω opt , wherein π 1   a :SO 3   N →R 3N×3N , R R+R{tilde over (Ω)}(ω opt ).  FIG. 3  further describes step  240  of  FIG. 2 . That is,  FIG. 3  illustrates a method of calculating the descent direction ω opt  according to an embodiment of the present invention. At step  310  it is determined whether the Hessian H of the local approximation of the cost function is positive definite. If H is determined to be positive definite at step  310 , at step  320 , the descent direction ω opt  is calculated as a Newton direction based H and ∇. In this case, the descent direction ω opt  can be expressed as:
 
ω opt (φ(ω))= −H   (f∘φ)(ω)   −1 ∇( f∘φ )(ω).  (35)
 
     If H is determined not to be positive definite at step  310 , at  330 , the descent direction is calculated as a Gauss direction based on Ĥ and ∇. In this case the descent direction can be expressed as:
 
ω opt (φ(ω))=− Ĥ   (f∘φ)(ω)   −1 ∇( f∘φ )(ω).  (36)
 
     Since H can be decomposed as a sum of a positive definite term (Ĥ) and another term ({tilde over (H)}), it is possible to use the only the positive definite term (Ĥ) to determine the descent direction ω opt  when H is not positive definite. 
     Returning to  FIG. 2 , once the descent direction ω opt  is calculated (step  240 ), the step length λ opt  is calculated based on the descent direction ω opt  (step  250 ). The step length λ opt  can be calculated using an approximate one dimensional line search carried out in the descent direction ω opt , and denoted by the mapping π 1   b . The line search can be a backtracking line search used to ensure that the cost function is reduced with every iteration. Since the backtracking line search is carried out in the descent direction ω opt , choosing a sufficiently small step size will ensure that the cost function is reduced. In order to select a sufficiently small step size, it is possible that the backtracking line search first try a step size of 1, and if this is unacceptable, reduce the step size until an acceptable step length λ opt  is found. Thus,
 
π 1   b   :R   3N×3N   →R   3N×3N,  
 
 R+R{tilde over (Q)}(ω   opt )   R+R {tilde over (Ω)}(λ opt ω opt )  (37)
         where λ opt  is the step length that reduces the cost function in the descent direction ω opt , and is found using a backtracking line search.       

     At step  260 , an updated value of R is calculated using the descent direction ω opt  and the step length λ opt  calculated at steps  240  and  250 , respectively. That is, a point (i.e., the updated value of R) on the product manifold SO 3   N  in the neighborhood of R, which reduces the cost function, is determined based on the descent direction ω opt  and the step length λ opt  via the parameterization π 2 :R 3N×3N →SO 3   N : 
     
       
         
           
             
               
                 
                   
                     
                       
                         R 
                         + 
                         
                           
                             Ω 
                             ~ 
                           
                           ⁡ 
                           
                             ( 
                             
                               
                                 λ 
                                 opt 
                               
                               ⁢ 
                               
                                 ω 
                                 opt 
                               
                             
                             ) 
                           
                         
                       
                       ↦ 
                       
                         R 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           ⅇ 
                           
                             
                               Ω 
                               ~ 
                             
                             ⁡ 
                             
                               ( 
                               
                                 
                                   λ 
                                   opt 
                                 
                                 ⁢ 
                                 
                                   ω 
                                   opt 
                                 
                               
                               ) 
                             
                           
                         
                       
                     
                     = 
                     
                       R 
                       ⁡ 
                       
                         ( 
                         
                           
                             
                               
                                 
                                   ⅇ 
                                   
                                     ( 
                                     
                                       
                                         Ω 
                                         1 
                                       
                                       ⁡ 
                                       
                                         ( 
                                         
                                           
                                             λ 
                                             opt 
                                           
                                           ⁢ 
                                           
                                             ω 
                                             1 
                                             opt 
                                           
                                         
                                         ) 
                                       
                                     
                                     ) 
                                   
                                 
                                 ⊕ 
                                 … 
                                 ⊕ 
                               
                             
                           
                           
                             
                               
                                 ⅇ 
                                 
                                   ( 
                                   
                                     
                                       Ω 
                                       N 
                                     
                                     ⁡ 
                                     
                                       ( 
                                       
                                         
                                           λ 
                                           opt 
                                         
                                         ⁢ 
                                         
                                           ω 
                                           N 
                                           opt 
                                         
                                       
                                       ) 
                                     
                                   
                                   ) 
                                 
                               
                             
                           
                         
                         ) 
                       
                     
                   
                   ⁢ 
                   
                     
 
                   
                   ⁢ 
                   Since 
                   ⁢ 
                   
                     
 
                   
                   ⁢ 
                   
                     
                       ω 
                       opt 
                     
                     = 
                     
                       
                         
                           [ 
                           
                             
                               ω 
                               1 
                               
                                 opt 
                                 T 
                               
                             
                             ⁢ 
                             … 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               ω 
                               N 
                               
                                 opt 
                                 T 
                               
                             
                           
                           ] 
                         
                         T 
                       
                       . 
                     
                   
                 
               
               
                 
                   ( 
                   38 
                   ) 
                 
               
             
           
         
       
     
     Accordingly, using Equation 38, an updated value for R is determined which is on the manifold based on the descent direction and the step length calculated to reduce the cost function. As the local approximation of the cost function f∘φ is reduced, the norm of the gradient ∇ of the local approximation of the cost function f∘φ approached zero. Thus, at step  270 , it is determined whether the norm of the gradient ∇ of the local approximation of the cost function f∘φ is greater than an error thresholds ε (i.e., ∥∇(f∘φ)(0)∥&gt;ε). If the norm of the gradient ∇ of the local approximation of the cost function f∘φ is greater than the error threshold ε, than the cost function must be reduced further, and the method returns to step  210 , using the updated value of R as R. If the norm of the gradient ∇ of the local approximation of the cost function f∘φ is not greater than an error threshold ε, then the method does to step  280 . At step  280 , the updated value of R is output as the optimal value of R for minimizing the cost function. Accordingly, optimal values are determined for each of the rotation matrices corresponding to the 3D point sets of multiple views. 
     Returning to  FIG. 1 , at step  130 , optimal values of the translation vectors corresponding to the 3D point sets of the multiple views are calculated based on the determined optimal values of the rotation matrices. That is, an optimal value of T is calculated based on the optimal value of R using Equation 10. Accordingly, using the optimal translation vectors and rotation matrices corresponding to each view, the 3D point sets of the views can be registered onto the reference coordinate frame to construct a 3D model of an object. 
     The global registration method of multiple 3D point sets, as described above, can be implemented on a computer using well known computer processors, memory units, storage devices, computer software, and other components. A high level block diagram of such a computer is illustrated in  FIG. 4 . Computer  402  contains a processor  404  which controls the overall operation of the computer  402  by executing computer program instructions which define such operation. The computer program instructions may be stored in a storage device  412  (e.g., magnetic disk) and loaded into memory  410  when execution of the computer program instructions is desired. Thus, the iterative optimization method used to determine optimal values of rotational matrices for the 3D point sets can be defined by the computer program instructions stored in the memory  410  and/or storage  412  and controlled by the processor  404  executing the computer program instructions. The computer  402  also includes one or more network interfaces  406  for communicating with other devices via a network. The computer  402  also includes input/output  408  which represents devices which allow for user interaction with the computer  402  (e.g., display, keyboard, mouse, speakers, buttons, etc.) One skilled in the art will recognize that an implementation of an actual computer will contain other components as well, and that  FIG. 4  is a high level representation of some of the components of such a computer for illustrative purposes. 
     The foregoing Detailed Description is to be understood as being in every respect illustrative and exemplary, but not restrictive, and the scope of the invention disclosed herein is not to be determined from the Detailed Description, but rather from the claims as interpreted according to the full breadth permitted by the patent laws. It is to be understood that the embodiments shown and described herein are only illustrative of the principles of the present invention and that various modifications may be implemented by those skilled in the art without departing from the scope and spirit of the invention. Those skilled in the art could implement various other feature combinations without departing from the scope and spirit of the invention.