sophus库的一些使用

转载自https://blog.csdn.net/robinhjwy/article/details/77334189

首先是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 <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头文件

首先说明一下构造方式：

    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&lt;&lt;"SO(3) from quaternion :"&lt;&lt;SO3_q&lt;&lt;endl;</code></pre><br />整体思路很明显，由同一个轴角，构造出旋转矩阵，四元数。再去构造SO3，理论上构造出来的应该都一样。<br />输出结果如下：<br /><pre><code class="language-cpp">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_111<<endl;

输出：

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”<<SO3<<endl;

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


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

SO3　=　1 1 1

叵费～～

切记SO3是矩阵，是李群。


试验继续：

    // 使用对数映射获得它的李代数

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&lt;&lt;"SO3 updated = "&lt;&lt;SO3_updated&lt;&lt;endl;</code></pre>代码看出来：<br /><br />

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


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


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


输出：

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= "<<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&lt;&lt;"SE3 updated = "&lt;&lt;endl&lt;&lt;SE3_updated.matrix()&lt;&lt;endl;</code></pre><br />

输出：

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

有个疑问:
Sophus::SE3::exp(update_se3)中前三项是"平移向量",这个平移向量到底是什么?如何通过前三项来计算出真正的平移向量?