sophus库的一些使用

首先是cmakelists:

[cpp] view plain copy

1. cmake_minimum_required( VERSION 2.8 )  
2. project( useSophus )  
3.   
4. # 为使用 sophus，您需要使用find_package命令找到它  
5. find_package( Sophus REQUIRED )  
6. include_directories( ${Sophus_INCLUDE_DIRS} )  
7.   
8. add_executable( useSophus useSophus.cpp )  
9. target_link_libraries( useSophus ${Sophus_LIBRARIES} )  

然后一些include：

[cpp] view plain copy

1. #include <iostream>  
2. #include <cmath>  
3. using namespace std;   
4.   
5. #include <Eigen/Core>//导入eigen库的核心组件  
6. #include <Eigen/Geometry>//导入eigen库的几何组件  
7.   
8. #include "sophus/so3.h"//导入sophus库的so3头文件  
9. #include "sophus/se3.h"//导入sophus库的se3头文件  

首先说明一下构造方式：

[cpp] view plain copy

1. Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();//由轴角构造旋转向量，用于后面的由旋转向量构造李群  
2.   
3. Sophus::SO3 SO3_R(R);               // Sophus::SO(3)可以直接从旋转矩阵构造，由上方的R构造  
4. Sophus::SO3 SO3_v( 0, 0, M_PI/2 );  // 这里注意，不是旋转向量的三个坐标值，有点像欧拉角构造。因为假设是旋转向量的三个坐标值构造的话，那么Sophus::SO3 SO3_111( 1, 1, 1 );构造输出出来的应该也是( 1, 1, 1 )  
5. Eigen::Quaterniond q(R);            // 或者四元数  
6. Sophus::SO3 SO3_q( q );  
7. // 上述表达方式都是等价的  
8. // 注意输出SO(3)时，以so(3)形式输出  
9. cout<<"SO(3) from matrix: "<<SO3_R<<endl;//本来这里的李群应该是矩阵，但是输出时都被转换成李代数进行输出了。所以结果形式上是一个三维向量。  
10. cout<<"SO(3) from vector: "<<SO3_v<<endl;  
11. cout<<"SO(3) from quaternion :"<<SO3_q<<endl;  

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

[cpp] view plain copy

1. SO(3) from matrix:      0      0 1.5708  
2.   
3. SO(3) from vector:      0      0 1.5708  
4.   
5. SO(3) from quaternion :     0      0 1.5708  

很明显，一样的，没毛病。从输出的形式可以看出，虽然SO3是李群，是旋转矩阵，但是输出形式还是向量（被转化成李代数输出）。
看一下源代码中重载的<<运算也很明显：

[cpp] view plain copy

1. inline std::ostream& operator <<(std::ostream & out_str,  
2.                                  const SO3 & so3)  
3. {  
4.   
5.   out_str << so3.log().transpose() << std::endl;  
6.   return out_str;  
7. }  

有一个.log()转化成李代数之后再输出的。

这里重点说一下这条构造，这条构造语句后面测试的时候，有坑：

[cpp] view plain copy

1.   

[cpp] view plain copy

1. Sophus::SO3 SO3_v( 0, 0, M_PI/2 );  

这条语句。第一次看想当然以为是旋转向量的三个坐标值进行构造的。因为输出的值跟输进去的值是一样的：

[cpp] view plain copy

1. 0 　0　 1.5708　＝　( 0, 0, M_PI/2 )  

假如，这条语句真的就是直接旋转向量构造，那么用（１，１，１）构造的话，输出也应该是１，１，１吧。下面测试：

[cpp] view plain copy

1. Sophus::SO3 SO3_111( 1, 1, 1 );  
2. cout<<"SO3_v( 1, 1, 1 )=\n"<<SO3_111<<endl;  

输出：

[cpp] view plain copy

1. SO3_v( 1, 1, 1 )　=　1.34255　 0.393915　  1.34255  

嗯？什么鬼～

扒一扒源代码吧，显示此条构造是这样：

[cpp] view plain copy

1. SO3  
2. ::SO3(double rot_x, double rot_y, double rot_z)  
3. {  
4.   unit_quaternion_  
5.       = (SO3::exp(Vector3d(rot_x, 0.f, 0.f))  
6.          *SO3::exp(Vector3d(0.f, rot_y, 0.f))  
7.          *SO3::exp(Vector3d(0.f, 0.f, rot_z))).unit_quaternion_;  
8. }  

这就有点思路了，显示的貌似是三个过程，先转Ｘ轴，再转Ｙ轴，再转Ｚ轴，完全跟旋转向量不搭边。瞅着过程有点像欧拉角的过程，三个轴分了三步。

那么问题来了，我就有一个(1, 1, 1)旋转向量，如何构造成SO3呢？也就是让它输出(1, 1, 1)。

ＯＫ，是这么玩的：

[cpp] view plain copy

1. Eigen::Vector3d so33 (1, 1, 1);  
2. Sophus::SO3 SO3 =Sophus::SO3::exp(so33);  
3. cout<<"SO3=\n"<<SO3<<endl;  

sophus库中，压根就没有so3这么一说，就是SO3类，所以，so3的本质就是个三维向量，直接Eigen::Vector3d定义。

定义完了之后由指数映射成矩阵，赋值给SO3，这样就好了。少废话，看输出：

[cpp] view plain copy

1. SO3　=　1 1 1  

叵费～～

切记SO3是矩阵，是李群。

试验继续：

[cpp] view plain copy

1. // 使用对数映射获得它的李代数  
2. Eigen::Vector3d so3 = SO3_R.log();  
3. cout<<"so3 = "<<so3.transpose()<<endl;  
4. // hat 为向量到反对称矩阵,相当于　^　运算。  
5. cout<<"so3 hat=\n"<<Sophus::SO3::hat(so3)<<endl;  
6. // 相对的，vee为反对称矩阵到向量，相当于下尖尖运算  
7. cout<<"so3 hat vee= "<<Sophus::SO3::vee( Sophus::SO3::hat(so3) ).transpose()<<endl; // transpose纯粹是为了输出美观一些  
8.   
9. // 增量扰动模型的更新  
10. Eigen::Vector3d update_so3(1e-4, 0, 0); //创建一个增量扰动，形式为旋转向量形式。数值上可以看出扰动非常小。  
11. Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3)*SO3_R;//将扰动用对数映射到旋转矩阵形式，并对原矩阵左乘进行扰动，然后看输出结果，发现也变化很小  
12. cout<<"SO3 updated = "<<SO3_updated<<endl;  

代码看出来：

.log运算将李群转化成李代数。

hat运算将向量转成反对称阵。

vee运算将反对称阵转成向量。

输出：

[cpp] view plain copy

1. so3 =  0   0 　1.5708  
2. so3 hat　=  
3. 0　             -1.5708       0  
4. 1.5708       0                 -0  
5. -0                0                  0  
6. so3 hat vee=      0      0       1.5708  
7. SO3 updated =  7.85398e-05      -7.85398e-05       1.5708  

看起来没啥毛病～


继续SE(3)操作：

[cpp] view plain copy

1. // 对SE(3)操作大同小异  
2. Eigen::Vector3d t(1,0,0);           // 沿X轴平移1  
3. Sophus::SE3 SE3_Rt(R, t);           // 从R,t构造SE(3)  
4. Sophus::SE3 SE3_qt(q,t);            // 从q,t构造SE(3)  
5. cout<<"SE3 from R,t= "<<endl<<SE3_Rt<<endl;  
6. cout<<"SE3 from q,t= "<<endl<<SE3_qt<<endl;  
7. // 李代数se(3) 是一个六维向量，方便起见先typedef一下  
8. typedef Eigen::Matrix<double,6,1> Vector6d;  
9. Vector6d se3 = SE3_Rt.log();  
10. cout<<"se3 = "<<se3.transpose()<<endl;  
11. // 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后.  
12. // 同样的，有hat和vee两个算符  
13. cout<<"se3 hat = "<<endl<<Sophus::SE3::hat(se3)<<endl;  
14. cout<<"se3 hat vee = "<<Sophus::SE3::vee( Sophus::SE3::hat(se3) ).transpose()<<endl;  
15.   
16. // 最后，演示一下更新  
17. Vector6d update_se3; //更新量  
18. update_se3.setZero();  
19. update_se3(0,0) = 1e-4d;  
20. Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3)*SE3_Rt;  
21. cout<<"SE3 updated = "<<endl<<SE3_updated.matrix()<<endl;  

输出：

[cpp] view plain copy

1. SE3 from R,t=   
2.      0      0 1.5708  
3. 1 0 0  
4.   
5. SE3 from q,t=   
6.      0      0 1.5708  
7. 1 0 0  
8.   
9. se3 =  0.785398 -0.785398         0         0         0    1.5708  
10. se3 hat =   
11.         0   -1.5708         0  0.785398  
12.    1.5708         0        -0 -0.785398  
13.        -0         0         0         0  
14.         0         0         0         0  
15. se3 hat vee =  0.785398 -0.785398         0         0         0    1.5708  
16. SE3 updated =   
17. 2.22045e-16          -1           0      1.0001  
18.           1 2.22045e-16           0           0  
19.           0           0           1           0  
20.           0           0           0           1
21. 转载 http://blog.csdn.net/robinhjwy/article/details/77334189