Sophus李群，李代数

  
  

   
   #include <iostream>
  
  
  
  

   
   #include<cmath>
  
  
  
  

   
   #define  PI  3.14159265358979323846
  
  
  
  

   
   using  
   
   namespace 
   
   std
   
   ;
  
  
  
  


  
  
  
  

   
   #include<Eigen/Core>
  
  
  
  

   
   #include<Eigen/Geometry>
  
  
  
  

   
   #include "sophus/so3.h"
  
  
  
  

   
   #include "sophus/se3.h"
  
  
  
  

   
   int 
   
   main
   
   (
   
   int 
   
   argc
   
   , 
   
   char 
   
   **
   
   argv
   
   ) 
   
   {
  
  
  
  
    
   
   Eigen
   
   ::
   
   Matrix3d 
   
   R 
   
   = 
   
   Eigen
   
   ::
   
   AngleAxisd
   
   (
   
   PI
   
   /
   
   2
   
   , 
   
   Eigen
   
   ::
   
   Vector3d
   
   (
   
   0
   
   , 
   
   0
   
   , 
   
   1
   
   )).
   
   toRotationMatrix
   
   ();
  
  
  
  
   
   
   // toRotationMatrix();
  
  
  
  
        
  
  
  
  
    
   
   Sophus
   
   ::
   
   SO3   
   
   SO3_R
   
   (
   
   R
   
   );
  
  
  
  
    
   
   Sophus
   
   ::
   
   SO3  
   
   SO3_v
   
   (
   
   0
   
   , 
   
   0 
   
   , 
   
   PI
   
   /
   
   2
   
   );
  
  
  
  
    
   
   Eigen
   
   ::
   
   Quaterniond  
   
   q
   
   (
   
   R
   
   );
  
  
  
  
    
  
  
  
  
    
   
   Sophus
   
   ::
   
   SO3 
   
   SO3_q
   
   (
   
   q
   
   );
  
  
  
  
    
   
   cout 
   
   << 
   
   SO3_R
   
   <<
   
   endl
   
   ;
  
  
  
  
    
   
   cout 
   
   << 
   
   SO3_v
   
   <<
   
   endl
   
   ;
  
  
  
  
    
   
   cout 
   
   << 
   
   SO3_q
   
   <<
   
   endl
   
   ;
  
  
  
  
    
  
  
  
  
    
   
   Eigen
   
   ::
   
   Vector3d 
   
   so3 
   
   = 
   
   SO3_R
   
   .
   
   log
   
   ();
  
  
  
  
    
   
   cout 
   
   <<
   
   "so3 ="
   
   <<
   
   so3
   
   .
   
   transpose
   
   ()
   
   <<
   
   endl
   
   ;
  
  
  
  
    
   
   cout 
   
   <<
   
   "so3 hat = "
   
   <<
   
   Sophus
   
   ::
   
   SO3
   
   ::
   
   hat
   
   (
   
   so3
   
   )
   
   <<
   
   endl
   
   ;
  
  
  
  
    
   
   cout 
   
   <<
   
   "so3 hat vee = "
   
   <<
   
   Sophus
   
   ::
   
   SO3
   
   ::
   
   vee
   
   (
   
   Sophus
   
   ::
   
   SO3
   
   ::
   
   hat
   
   (
   
   so3
   
   )).
   
   transpose
   
   ()
   
   <<
   
   endl
   
   ;
  
  
  
  
    
  
  
  
  
    
   
   Eigen
   
   ::
   
   Vector3d 
   
   update_so3
   
   (
   
   1e-4
   
   , 
   
   0
   
   ,
   
   0
   
   );
  
  
  
  
    
   
   Sophus
   
   ::
   
   SO3  
   
   so3_updated 
   
   = 
   
   Sophus
   
   ::
   
   SO3
   
   ::
   
   exp
   
   (
   
   update_so3
   
   ) 
   
   * 
   
   SO3_R
   
   ;
  
  
  
  
    
   
   cout 
   
   << 
   
   "so3_updated ="
   
   <<
   
   so3_updated
   
   <<
   
   endl
   
   ;
  
  
  
  
    
  
  
  
  
    
   
   cout 
   
   << 
   
   SO3_R
   
   <<
   
   endl
   
   ;
  
  
  
  
    
  
  
  
  
    
   
   Eigen
   
   ::
   
   Vector3d 
   
   t
   
   (
   
   1
   
   , 
   
   0
   
   , 
   
   0
   
   );
  
  
  
  
    
   
   Sophus
   
   ::
   
   SE3 
   
   SE3_Rt
   
   (
   
   R
   
   , 
   
   t
   
   );
  
  
  
  
    
   
   Sophus
   
   ::
   
   SE3 
   
   SE3_qt
   
   (
   
   q
   
   , 
   
   t
   
   );    
  
  
  
  
    
   
   cout
   
   << 
   
   "SE3_Rt"
   
   <<
   
   SE3_Rt
   
   <<
   
   endl
   
   ;
  
  
  
  
    
   
   cout
   
   << 
   
   "SE3_qt"
   
   <<
   
   SE3_qt
   
   <<
   
   endl
   
   ;
  
  
  
  
    
   
   return 
   
   0
   
   ;
  
  
  
  

   
   }