sophus库的一些使用

77 阅读 0 评论 51 点赞

我是靠谱客的博主满意盼望，最近开发中收集的这篇文章主要介绍sophus库的一些使用，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

首先是cmakelists:

[cpp]  
   view plain 
    copy 
   
 cmake_minimum_required( VERSION 2.8 )  
 project( useSophus )  
   
 # 为使用 sophus，您需要使用find_package命令找到它  
 find_package( Sophus REQUIRED )  
 include_directories( ${Sophus_INCLUDE_DIRS} )  
   
 add_executable( useSophus useSophus.cpp )  
 target_link_libraries( useSophus ${Sophus_LIBRARIES} )  

然后一些include：

[cpp]  
   view plain 
    copy 
   
 #include <iostream>  
 #include <cmath>  
 using namespace std;   
   
 #include <Eigen/Core>//导入eigen库的核心组件  
 #include <Eigen/Geometry>//导入eigen库的几何组件  
   
 #include "sophus/so3.h"//导入sophus库的so3头文件  
 #include "sophus/se3.h"//导入sophus库的se3头文件  

首先说明一下构造方式：

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

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

[cpp]  
   view plain 
    copy 
   
 SO(3) from matrix:      0      0 1.5708  
   
 SO(3) from vector:      0      0 1.5708  
   
 SO(3) from quaternion :     0      0 1.5708  

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

[cpp]  
   view plain 
    copy 
   
 inline std::ostream& operator <<(std::ostream & out_str,  
                                  const SO3 & so3)  
 {  
   
   out_str << so3.log().transpose() << std::endl;  
   return out_str;  
 }  

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

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

[cpp]  
   view plain 
    copy 
   
   

[cpp]  
   view plain 
    copy 
   
 Sophus::SO3 SO3_v( 0, 0, M_PI/2 );  

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

[cpp]  
   view plain 
    copy 
   
 0 　0　 1.5708　＝　( 0, 0, M_PI/2 )  

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

[cpp]  
   view plain 
    copy 
   
 Sophus::SO3 SO3_111( 1, 1, 1 );  
 cout<<"SO3_v( 1, 1, 1 )=n"<<SO3_111<<endl;  

输出：

[cpp]  
   view plain 
    copy 
   
 SO3_v( 1, 1, 1 )　=　1.34255　 0.393915　  1.34255  

嗯？什么鬼～

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

[cpp]  
   view plain 
    copy 
   
 SO3  
 ::SO3(double rot_x, double rot_y, double rot_z)  
 {  
   unit_quaternion_  
       = (SO3::exp(Vector3d(rot_x, 0.f, 0.f))  
          *SO3::exp(Vector3d(0.f, rot_y, 0.f))  
          *SO3::exp(Vector3d(0.f, 0.f, rot_z))).unit_quaternion_;  
 }  

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

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

ＯＫ，是这么玩的：

[cpp]  
   view plain 
    copy 
   
 Eigen::Vector3d so33 (1, 1, 1);  
 Sophus::SO3 SO3 =Sophus::SO3::exp(so33);  
 cout<<"SO3=n"<<SO3<<endl;  

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

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

[cpp]  
   view plain 
    copy 
   
 SO3　=　1 1 1  

叵费～～

切记SO3是矩阵，是李群。

试验继续：

[cpp]  
   view plain 
    copy 
   
 // 使用对数映射获得它的李代数  
 Eigen::Vector3d so3 = SO3_R.log();  
 cout<<"so3 = "<<so3.transpose()<<endl;  
 // hat 为向量到反对称矩阵,相当于　^　运算。  
 cout<<"so3 hat=n"<<Sophus::SO3::hat(so3)<<endl;  
 // 相对的，vee为反对称矩阵到向量，相当于下尖尖运算  
 cout<<"so3 hat vee= "<<Sophus::SO3::vee( Sophus::SO3::hat(so3) ).transpose()<<endl; // transpose纯粹是为了输出美观一些  
   
 // 增量扰动模型的更新  
 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;  

代码看出来：

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

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

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

输出：

[cpp]  
   view plain 
    copy 
   
 so3 =  0   0 　1.5708  
 so3 hat　=  
 0　             -1.5708       0  
 1.5708       0                 -0  
 -0                0                  0  
 so3 hat vee=      0      0       1.5708  
 SO3 updated =  7.85398e-05      -7.85398e-05       1.5708  

看起来没啥毛病～

继续SE(3)操作：

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

输出：

[cpp]  
   view plain 
    copy 
   
 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  
 se3 hat =   
         0   -1.5708         0  0.785398  
    1.5708         0        -0 -0.785398  
        -0         0         0         0  
         0         0         0         0  
 se3 hat vee =  0.785398 -0.785398         0         0         0    1.5708  
 SE3 updated =   
 2.22045e-16          -1           0      1.0001  
           1 2.22045e-16           0           0  
           0           0           1           0  
           0           0           0           1
 转载 http://blog.csdn.net/robinhjwy/article/details/77334189