sophus库的一些使用

参考博客：http://blog.csdn.net/robinhjwy/article/details/77334189

一：SO3

Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();//由轴角构造旋转向量
Sophus::SO3 SO3_R(R);               // Sophus::SO3 由旋转矩阵构造  
Sophus::SO3 SO3_v( 0, 0, M_PI/2 );  // Sophus::SO3 由向量构造(不是旋转向量)
Eigen::Quaterniond q(R);            // 旋转矩阵转四元数
Sophus::SO3 SO3_q( q );  　　　　　　 // Sophus::SO3 由四元数构造 
// 注意输出SO(3)时，以so(3)形式输出  
cout<<"SO(3) from matrix: "<<SO3_R<<endl;// 李群应该是矩阵，但输出时都被转换成李代数(.log())进行输出了,所以结果形式上是一个三维向量。  
cout<<"SO(3) from vector: "<<SO3_v<<endl;  
cout<<"SO(3) from quaternion :"<<SO3_q<<endl;  

整体思路很明显，由同一个轴角，构造出旋转矩阵，四元数。再去构造SO3，理论上构造出来的应该都一样。

输出结果如下：

SO(3) from matrix:      0      0 1.5708  
  
SO(3) from vector:      0      0 1.5708  
  
SO(3) from quaternion : 0      0 1.5708  

二：SE3

// 对SE(3)操作大同小异  
Eigen::Vector3d t(1,0,0);           // 构造平移向量
Sophus::SE3 SE3_Rt(R, t);           // 旋转矩阵构造SE3 
Sophus::SE3 SE3_qt(q,t);            // 四元数构造SE3
cout<<"SE3 from R,t= "<<endl<<SE3_Rt<<endl;  
cout<<"SE3 from q,t= "<<endl<<SE3_qt<<endl;   
typedef Eigen::Matrix<double,6,1> Vector6d;  
Vector6d se3 = SE3_Rt.log(); 　　　　// 转李代数形式输出 
cout<<"se3 = "<<se3.transpose()<<endl;  

输出：

SE3 from R,t=   
     0      0 　　1.5708  
　　　1 　　　0　 　0  
  
SE3 from q,t=   
     0      0 　　1.5708  
　　　1 　　　0 　　0  
  
se3 =  0.785398   -0.785398     0       0      0     1.5708  //平移在前，旋转在后