转载自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<<"SO(3) from quaternion :"<<SO3_q<<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 )
假如,这条语句真的就是直接旋转向量构造,那么用(1,1,1)构造的话,输出也应该是1,1,1吧。下面测试:
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_;
}
这就有点思路了,显示的貌似是三个过程,先转X轴,再转Y轴,再转Z轴,完全跟旋转向量不搭边。瞅着过程有点像欧拉角的过程,三个轴分了三步。
那么问题来了,我就有一个(1, 1, 1)旋转向量,如何构造成SO3呢?也就是让它输出(1, 1, 1)。
OK,是这么玩的:
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<<"SO3 updated = "<<SO3_updated<<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<<"SE3 updated = "<<endl<<SE3_updated.matrix()<<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)中前三项是"平移向量",这个平移向量到底是什么?如何通过前三项来计算出真正的平移向量?