sophus库的一些使用

首先是cmakelists:

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：

#include 
#include 
using namespace std; 

#include //导入eigen库的核心组件
#include //导入eigen库的几何组件

#include "sophus/so3.h"//导入sophus库的so3头文件
#include "sophus/se3.h"//导入sophus库的se3头文件

首先说明一下构造方式：

    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，理论上构造出来的应该都一样。
输出结果如下：

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是李群，是旋转矩阵，但是输出形式还是向量（被转化成李代数输出）。
看一下源代码中重载的<<运算也很明显：

inline std::ostream& operator <<(std::ostream & out_str,
                                 const SO3 & so3)
{

  out_str << so3.log().transpose() << std::endl;
  return out_str;
}

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

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

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

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

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

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

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

输出：

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

嗯？什么鬼～

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

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)。

ＯＫ，是这么玩的：

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

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

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

SO3　=　1 1 1

叵费～～

切记SO3是矩阵，是李群。

试验继续：

    // 使用对数映射获得它的李代数
    Eigen::Vector3d so3 = SO3_R.log();
    cout<<"so3 = "<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)操作：

    // 对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= "<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

对齐格式有点膈应，要啥自行车，能通就行。