Patent ID: 11875534
Assignee: GUANGDONG UNIVERSITY OF TECHNOLOGY
Field: Computer technology (Electrical engineering)
Classification: CPC G  B  Y | IPC B  G

Claim 9:
10. The pose estimation method for an unmanned aerial vehicle based on point, line and plane feature fusion according to claim 9, characterized in that, after the step S9, the method further comprises:
S10, estimating a plane pose and performing degradation detection, comprising: according to the plane feature pair set Oπ={πac,πbr} after removing the mismatched plane pairs, calculating a three-dimensional rotation transformation matrix R∈SO(3) of the RGB-D camera and a translation transformation matrix t∈R3, and minimizing the error of a plane feature matching;, E
   π
   R
  
  =
  
   
    ∑
    
     O
     π
    
   
   
    
     
     
      
       n
       
        π
        ⁢
        a
       
       c
      
      -
      
       R
       ⁢
       
        n
        
         π
         ⁢
         b
        
        r
       
      
     
     
    
    2
   
  
 

 
  
   E
   π
   t
  
  =
  
   
    ∑
    
     O
     π
    
   
   
    
     (
     
      
       d
       
        π
        ⁢
        a
       
       c
      
      -
      
       (
       
        
         d
         
          π
          ⁢
          b
         
         r
        
        -
        
         
          
           (
           
            n
            
             π
             ⁢
             a
            
            c
           
           )
          
          T
         
         ⁢
         t
        
       
       )
      
     
     )
    
    2
   
  
 

in the formula, EπR represents a rotation error of the plane feature matching; Eπt represents a translation error of the plane feature matching; nπac represents the normal vector of the plane in the current frame in a Cartesian coordinate system; nπbr represents the normal vector of the plane in the reference frame in the Cartesian coordinate system; R represents a rotation transformation matrix from the reference frame to the current frame; dπac represents the distance from the plane πac in the current frame to the origin point of the coordinate system in the Cartesian coordinate system; dπbr represents the distance from the plane πbc in the reference frame to the origin point of the coordinate system in the Cartesian coordinate system; and t represents a translation transformation matrix from the reference frame to the current frame;
in order to detect whether the degeneration has occurred, constructing a matrix HE=ΣOπnπbr(nπac)t and performing SVD decomposition, if the matrix HE is a singular matrix and all three singular values are positive singular values, the degeneration having not occurred, and if there are two positive singular values and one zero singular value, degenerating to a solution with 5 degrees of freedom, otherwise degenerating to a solution with less than 5 degrees of freedom, wherein
5 degree of freedom degeneration pose estimation comprises: when degenerating to 5 degree of freedom, adding a linear feature matching error, E
   
    L
    ⁢
    π
   
   t
  
  =
  
   
    E
    π
    t
   
   +
   
    
     ∑
     
       
      
       i
       =
       1
      
     
     
       
      
       N
       L
      
     
    
    
     
      
      
       
        v
        i
        c
       
       ×
       q
      
      
     
     ·
     
      
       
       
        
         u
         i
         c
        
        -
        
         (
         
          
           R
           ⁢
           
            u
            i
            r
           
          
          +
          
           
            
             [
             
              v
              i
              c
             
             ]
            
            ×
           
           ⁢
           t
          
         
         )
        
       
       
      
      2
     
    
   
  
 

 while minimizing the translation error,
wherein, NL is the number of the matched linear feature pair, ELπt represents the translation error of combining plane feature matching and linear feature matching, which comprising: a translation error Eπt calculated according to the plane feature matching and translation error, ∑
   
     
    
     i
     =
     1
    
   
   
     
    
     N
     L
    
   
  
  
   
    
    
     
      v
      i
      c
     
     ×
     q
    
    
   
   ·
   
    
     
     
      
       u
       i
       c
      
      -
      
       (
       
        
         R
         ⁢
         
          u
          i
          r
         
        
        +
        
         
          
           [
           
            v
            i
            c
           
           ]
          
          ×
         
         ⁢
         t
        
       
       )
      
     
     
    
    2
   
  
 

 calculated according to the linear matching; vic represents the unit normal vector of the linear Lic in the current frame; q is a right singular vector of the zero singular value after the SVD decomposition of the matrix HE; uic represents the normal vector of the plane composed of the line Lic and the origin point of the camera in the current frame; uir of represents the normal vector of the plane composed of the line Ljr and the origin point of the camera in the reference frame; t represents the translation transformation matrix from the reference frame to the current frame, and R represents the rotation transformation matrix from the reference frame to the current frame; and [vic]× is a cross product matrix corresponding to a vector vic;
the lower degree of freedom pose estimation comprises: when degenerating to a lower degree of freedom less than 5 degrees of freedom, calculating an essential matrix by directly using the matching point feature point pair, and according to an eight-point method, performing SVD decomposition on the essential matrix and re-projecting by combining triangularization to obtain a projection matrix, and finally performing RO on the projection matrix to obtain a solution pose.