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

Claim 7:
8. The pose estimation method for an unmanned aerial vehicle based on point, line and plane feature fusion according to claim 7, characterized in that the step S7 comprises: using the extracted feature points, plane and linear feature as a plurality of nodes of the association graph, setting the feature node as an inactive node, and setting the plane node and the linear node as active nodes;
constructing an edge eπ,p=[(φπ,p, index] between the feature point and the plane, φ
   
    π
    ,
    p
   
  
  =
  
   {
   
    
     
      
       
        0
        ,
       
      
      
       
        the
        ⁢
           
        feature
        ⁢
           
        point
        ⁢
           
        belongs
        ⁢
           
        to
        ⁢
           
        the
        ⁢
           
        plane
       
      
     
     
      
       
        1
        ,
       
      
      
       
        the
        ⁢
           
        feature
        ⁢
           
        point
        ⁢
           
        does
        ⁢
           
        not
        ⁢
           
        belong
        ⁢
           
        to
        ⁢
           
        the
        ⁢
           
        plane
       
      
     
    
    ;
   
  
 

index=r*10+c, the feature point is extracted from the cell of the rth row and the cth column of the plane;
constructing an edge eπ,π=[ω, θijππ,dijππ] between planes:, ω
  =
  
   {
   
    
     
      
       
        0
        ,
       
      
      
       
        two
        ⁢
           
        planes
        ⁢
           
        are
        ⁢
           
        parallel
        ⁢
           
        
         (
         
          
           θ
           ij
           ππ
          
          <
          
           θ
           th
          
         
         )
        
       
      
     
     
      
       
        1
        ,
       
      
      
       
        two
        ⁢
           
        planes
        ⁢
           
        are
        ⁢
           
        not
        ⁢
           
        parallel
       
      
     
    
    ;, θijππ=arccos(niT, nj), represents the angle between the normal vectors of two planes;, d
   ij
   ππ
  
  =
  
   {
   
    
     
      
       
        
         ❘
         "\[LeftBracketingBar]"
        
        
         
          d
          i
         
         -
         
          d
          j
         
        
        
         ❘
         "\[RightBracketingBar]"
        
       
      
      
       
        two
        ⁢
           
        planes
        ⁢
           
        are
        ⁢
           
        parallel
       
      
     
     
      
       
        0
        ,
       
      
      
       
        two
        ⁢
           
        planes
        ⁢
           
        are
        ⁢
           
        not
        ⁢
           
        parallel
       
      
     
    
    ;, dijππ represents the distance between plane i and plane j;
constructing an edge eπ,L=[I, θijLπ,dijLπ] between the plane and the line:, I
  =
  
   {
   
    
     
      
       
        0
        ,
       
      
      
       
        the
        ⁢
           
        line
        ⁢
           
        is
        ⁢
           
        parallel
        ⁢
           
        to
        ⁢
           
        the
        ⁢
           
        plane
        ⁢
           
        
         (
         
          
           
            ❘
            "\[LeftBracketingBar]"
           
           
            
             π
             2
            
            -
            
             θ
             ij
             
              L
              ⁢
              π
             
            
           
           
            ❘
            "\[RightBracketingBar]"
           
          
          <
          
           θ
           th
          
         
         )
        
       
      
     
     
      
       
        1
        ,
       
      
      
       
        the
        ⁢
           
        line
        ⁢
           
        is
        ⁢
           
        not
        ⁢
           
        parallel
        ⁢
           
        to
        ⁢
           
        the
        ⁢
           
        plane
       
      
     
    
    ;
   
  
 

θijLπ=arccos(viT,nj),, represents the angle between the line and the normal vector of the plane;, d
   ij
   
    L
    ⁢
    π
   
  
  =
  
   {
   
    
     
      
       
        
         ❘
         "\[LeftBracketingBar]"
        
        
         
          
           
            
             n
             j
             T
            
            [
            
             v
             i
            
            ]
           
           x
          
          ⁢
          
           u
           i
          
         
         +
         
          d
          j
         
        
        
         ❘
         "\[RightBracketingBar]"
        
       
       ,
      
     
     
      
       the
       ⁢
          
       plane
       ⁢
          
       is
       ⁢
          
       parallel
       ⁢
          
       to
       ⁢
          
       the
       ⁢
          
       line
      
     
    
    
     
      
       0
       ,
      
     
     
      
       the
       ⁢
          
       plane
       ⁢
          
       is
       ⁢
          
       not
       ⁢
          
       parallel
       ⁢
          
       to
       ⁢
          
       the
       ⁢
          
       line
      
     
    
   
  
 

 wherein dijLπ represents the distance between line i and plane j.