sophus库的一些使用

最新推荐文章于 2025-02-20 11:43:43 发布

robinhjwy

最新推荐文章于 2025-02-20 11:43:43 发布

阅读量1.6w

点赞数 11

分类专栏： SLAM库使用

本文链接：https://blog.youkuaiyun.com/robinhjwy/article/details/77334189

版权

SLAM库使用专栏收录该内容

5 篇文章

订阅专栏

首先是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;

整体思路很明显，由同一个轴角，构造出旋转矩阵，四元数。再去构造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_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<<"SO3 updated = "<<SO3_updated<<endl;
代码看出来：



.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;


输出：



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


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