SLAM——之Sophus函数库

1. Sophus 示例

Eigen库是一个开源的C++线性代数库，它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。但是Eigen库提供了集合模块，但没有提供李代数的支持。一个较好的李群和李代数的库是Sophus库，它很好的支持了SO(3),so(3),SE(3)和se(3)。
Sophus库是基于Eigen基础上开发的，继承了Eigen库中的定义的各个类。因此在使用Eigen库中的类时，既可以使用Eigen命名空间，也可以使用Sophus命名空间。比如

Eigen：：Matrix3d 和 Sophus：：Matrix3d
Eigen：：Vector3d 和 Sophus：：Vector3d

此外，为了方便说明SE(4)和se(4)，Sophus库还typedef了Vector4d、Matrix4d、Vector6d和Matrix6d等，即：

Sophus：：Vector4d
Sophus：：Matrix4d
Sophus：：Vector6d
Sophus：：Matrix6d

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/se3.h" //原版本为 sophus/se3.hpp

using namespace std;
using namespace Eigen;

/// 本程序演示sophus的基本用法

int main(int argc, char **argv) {

    // 沿Z轴转90度的旋转矩阵
    Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
    // 或者四元数
    Quaterniond q(R);
    Sophus::SO3 SO3_R(R);              // Sophus::SO3可以直接从旋转矩阵构造
    Sophus::SO3 SO3_q(q);              // 也可以通过四元数构造
    // 二者是等价的
    cout << "SO(3) from matrix:\n" << SO3_R.matrix() << endl;
    cout << "SO(3) from quaternion:\n" << SO3_q.matrix() << endl;
    cout << "they are equal" << endl;

    // 使用对数映射获得它的李代数
    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;

    // 增量扰动模型的更新
    Vector3d update_so3(1e-4, 0, 0); //假设更新量为这么多
    Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3) * SO3_R;
    cout << "SO3 updated = \n" << SO3_updated.matrix() << endl;

    cout << "*******************************" << endl;
    // 对SE(3)操作大同小异
    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= \n" << SE3_Rt.matrix() << endl;
    cout << "SE3 from q,t= \n" << SE3_qt.matrix() << 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 = \n" << 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;

    return 0;
}

2. Sophus常见函数总结

Sophus库的各种形式的表示如下：

李代数so(3)：Sophus：：Vector3d //因为so(3)仅仅只是一个普通的3维向量

李代数se(3)：Sophus：：Vector6d //因为se(3)仅仅只是一个普通的6维向量

2.1、初始化的李群（SO3）的几种方式

  //沿着Z轴旋转90度的旋转矩阵
    Eigen::AngleAxisd A1(M_PI / 2, Eigen::Vector3d(0, 0, 1));//以（0,0,1）为旋转轴，旋转180度
    Eigen::Matrix3d R1 = A1.matrix();
    Eigen::Quaterniond Q1(A1);

    //1.使用旋转矩阵初始化李群
    Sophus::SO3 SO3_R(R1);
    //注意：尽管SO(3)是对应一个矩阵,但是输出SO(3)时,实际上是以so(3)形式输出,从输出的结果可以看到,其输出的值与旋转角对应的值相同,这也证证实了SO(3)对应的李代数so(3)就是旋转角。
    cout << "SO(3) SO3_R from Matrix" << SO3_R << endl << endl;

    //2.使用四元数初始化李群
    Sophus::SO3 SO3_Q(Q1);
    cout << "SO(3) SO3_Q from Quaterion" << SO3_Q << endl << endl;

    /****************************************************************************
     3.1 使用旋转角（轴角）的各个元素对应的代数值来初始化李群

     注意：直接使用旋转角AngleAxis或是旋转角度对应的向量(Vector3d=AngleAxis.axis()*AngleAxis.angle())对李群进行初始化是不行的，因为SO3李群没有对应的构造函数。
    也即是使用下列方法是错误的：

     Sophus::SO3 SO3_A(A1);//直接使用旋转角对李群初始化
     Sophus::SO3 SO3_A(A1.axis()*A1.angle());//直接使用旋转角度对应的向量(Vector3d=AngleAxis.axis()*AngleAxis.angle())对李群进行初始化

     只能使用旋转角对应的向量的每一个维度进行赋值，对应于SO3的这样一个构造函数SO3(double rot_x, double rot_y, double rot_z);

    *******************************************************************************/

    //3.1.1 使用旋转角度对应的向量(Vector3d=AngleAxis.axis()*AngleAxis.angle())中的各个元素对李群进行初始化
    Sophus::SO3 SO3_A1((A1.axis() * A1.angle())(0), (A1.axis() * A1.angle())(1), (A1.axis() * A1.angle())(2));
    cout << "SO(3) SO3_A1 from AngelAxis1" << SO3_A1 << endl << endl;

    //3.1.2 使用旋转角度对应的向量(Vector3d=AngleAxis.axis()*AngleAxis.angle())中的各个元素对李群进行初始化
    Sophus::SO3 SO3_A2(M_PI / 2 * 0, M_PI / 2 * 0, M_PI / 2 * 1);
    cout << "SO(3) SO3_A2 from AngleAixs2" << SO3_A2 << endl << endl;

    //3.2 由于旋转角（轴角）与李代数so(3)对应,所以直接使用旋转角的值获得se(3),进而再通过Sophus::SO3::exp()获得对应的SO(3)
    Eigen::Vector3d V1(0, 0, M_PI / 2);//so3在Eigen中用Vector3d表示
    Sophus::SO3 SO3_V1 = Sophus::SO3::exp(V1);
    cout << "SO(3) SO3_V1 from SO3::exp()" << SO3_V1 << endl << endl;

2.2、SO(3)与so(3)的相互转换

Eigen::Vector3d so3_V1 = SO3_V1.log();//so(3)在Sophus(Eigen)中用vector3d表示,使用对数映射获得李群对应的李代数
    cout << "so(3) so3_V1 from SO3_V1" << so3_V1.transpose() << endl << endl;

    Sophus::SO3 SO3_V2 = Sophus::SO3::exp(so3_V1);//使用指数映射将李代数转化为李群
    cout << "SO(3) so3_V2 from so3_V1" <<SO3_V2 << endl << endl;

2.3、SO3&so3对应的hat和vee操作

   Eigen::Matrix3d M_so3_V1 = Sophus::SO3::hat(so3_V1);//hat为向量到其对应的反对称矩阵
    cout << "so3 hat=\n" << M_so3_V1 << endl << endl;

    Eigen::Vector3d V_M = Sophus::SO3::vee(M_so3_V1);//vee为反对称矩阵对应的向量
    cout << "so3 vee=\n" << V_M << endl << endl;

2.4、增量扰动模型

    Eigen::Vector3d update_so3(1e-4,0,0);//假设更新量为这么多
    Eigen::Matrix3d update_matrix=Sophus::SO3::exp(update_so3).matrix();//将李代数转为李群，然后转成旋转矩阵
    cout<<"SO3 update Matrix=\n"<<update_matrix<<endl<<endl;

    Sophus::SO3 SO3_updated=Sophus::SO3::exp(update_so3)*SO3_R;
    cout<<"SO3 updated = \n"<<SO3_updated<<endl;

    Eigen::Matrix3d SO3_updated_matrix=SO3_updated.matrix();//将李群转换为旋转矩阵
    cout<<"SO3 updated Matrix = \n"<<SO3_updated_matrix<<endl<<endl;

2.5 、SE3 & se3 示例

    Eigen::AngleAxisd A2(M_PI/2,Eigen::Vector3d(0,0,1));
    Eigen::Matrix3d R2=A2.matrix();
    Eigen::Quaterniond Q2(A2);
    Sophus::SO3 SO3_2(R2);

    //对应第一点，初始化李代数的几种方式
    Eigen::Vector3d t(1,0,0);

    //1. 使用旋转矩阵和平移向量来初始化SE3
    Sophus::SE3 SE_Rt(R2,t);
    cout<<"SE3 SE_Rt from  Rotation_Matrix and Transform=\n"<<SE_Rt<<endl<<endl;//注意尽管SE(3)是对应一个4*4的矩阵,但是输出SE(3)时是以一个六维向量输出的,其中前前三位为对应的so3,后3维度为实际的平移量t，而不是se3中的平移分量

    //2. 使用四元数和平移向量来初始化SE3
    Sophus::SE3 SE_Qt(Q2,t);
    cout<<"SE3 SE_Qt from  Quaterion and Transform=\n"<<SE_Qt<<endl<<endl;
    //3. 使用SO3和平移向量来初始化SE3
    Sophus::SE3 SE_St(SO3_2,t);
    cout<<"SE3 SE_St from  SO3 and Transform=\n"<<SE_St<<endl<<endl;

    //对应第二点，SE(3)与se(3)的相互转换
    Sophus::Vector6d se3_Rt=SE_Rt.log();//se(3)在Sophus中用Vector6d表示,使用对数映射获得李群对应的李代数
    cout<<"se(3) se3_Rt from SE3_Rt\n"<<se3_Rt<<endl<<endl;//se3输出的是一个六维度向量,其中前3维是平移分量,后3维度是旋转分量

    Sophus::SE3 SE3_Rt2=Sophus::SE3::exp(se3_Rt);//使用指数映射将李代数转化为李群
    cout<<"SE(3) SO3_Rt2 from se3_Rt"<<SE3_Rt2<<endl<<endl;
    //对应第三点，se3对应的hat和vee操作
    Sophus::Matrix4d M_se3_Rt=Sophus::SE3::hat(se3_Rt);
    cout<<"se(3) hat=\n"<<M_se3_Rt<<endl<<endl;

    Sophus::Vector6d V_M_se3=Sophus::SE3::vee(M_se3_Rt);
    cout<<"se(3) vee=\n"<<V_M_se3<<endl<<endl;

    //对应第四点，增量扰动模型

    Sophus::Vector6d update_se3=Sophus::Vector6d::Zero();
    update_se3(0)=1e-4d;

    cout<<"update_se3\n"<<update_se3.transpose()<<endl<<endl;

    Eigen::Matrix4d update_matrix2=Sophus::SE3::exp(update_se3).matrix();//将李群转换为旋转矩阵
    cout<<"update matrix=\n"<<update_matrix2<<endl<<endl;

    Sophus::SE3 SE3_updated=Sophus::SE3::exp(update_se3)*SE3_Rt2;
    cout<<"SE3 updated=\n"<<SE3_updated<<endl<<endl;

    Eigen::Matrix4d SE3_updated_matrix=SE3_updated.matrix();//将李群转换为旋转矩阵
    cout<<"SE3 updated Matrix=\n"<<SE3_updated_matrix<<endl<<endl;

    return 0;
}

3.Sophus 常见的 se & SE的转换以及自转换

3. 1 g2o、Eigen、Mat矩阵类型转换

3.2 Eigen与Mat基础操作及其互相转换： Eigen矩阵赋值/Mat类型操作

1） 使用row或者col

Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
R.row(0) = Eigen::Vector3d(-0.0134899,-0.997066,0.0753502);
R.row(1) = Eigen::Vector3d(-0.0781018,-0.0740761,-0.99419);
R.row(2) = Eigen::Vector3d(0.996854,-0.0192965,-0.0768734);

2）Mat类型操作 copyTo和colRange使用

Mat Rcd = Mat::zeros(3,3,CV_32FC1);
rx.copyTo(Rcd.colRange(0,1));
ry.copyTo(Rcd.colRange(1,2));
rz.copyTo(Rcd.colRange(2,3));

3）Mat->Eigen

Eigen::MatrixXd toEigenMatrixXd(const cv::Mat &cvMat) 
{
    Eigen::MatrixXd eigenMat;`在这里插入代码片`
    eigenMat.resize(cvMat.rows, cvMat.cols);
    for (int i=0; i<cvMat.rows; i++)
        for (int j=0; j<cvMat.cols; j++)
            eigenMat(i,j) = cvMat.at<float>(i,j);
    return eigenMat;
}

3.3 g2o 本质g2o::Matrix3D是typedef Eigen::Matrix<double,3,3,Eigen::ColMajor> Matrix3D;即Eigen矩阵

1）使用block
注意：g2o中使用的g2o::Matrix3D是typedef Eigen::Matrix<double,3,3,Eigen::ColMajor> Matrix3D;，即使用的Eigen矩阵

g2o::Matrix3D R = g2o::Matrix3D::Identity();
R.block<2,2>(0,0) << cos(theta),-sin(theta),sin(theta),cos(theta);

2）Se3<->SE3Quat

Matrix3D Rbc=toEigenMatrix3d(se3bc.R());
Vector3D tbc=toG2oVector3D(se3bc.tvec);
g2o::SE3Quat Tbc=g2o::SE3Quat(Rbc, tbc);

3）Se2->SE3Quat

///方法一：
Eigen::AngleAxisd rotz(measure_se2.theta, Eigen::Vector3d::UnitZ());
g2o::SE3Quat relativePose_SE3Quat(rotz.toRotationMatrix(), Eigen::Vector3d(measure.x, measure_se2.y, 0));

///方法二：

g2o::SE3Quat toG2oSE3Quat(const g2o::SE2& se2) 
{
g2o::Vector3D v2 = se2.toVector();
double x = v2(0);
double y = v2(1);
double theta = v2(2);
g2o::Matrix3D R = g2o::Matrix3D::Identity();
R.block<2,2>(0,0) << cos(theta),-sin(theta),sin(theta),cos(theta);
g2o::Vector3D t;
t << x,y,0;
return g2o::SE3Quat(R,t);
}

Se3->Isometry3D
```cpp
g2o::Isometry3D toG2oIsometry3D(const cv::Mat &T){
    Eigen::Matrix<double,3,3> R;
    R << T.at<float>(0,0), T.at<float>(0,1), T.at<float>(0,2),
            T.at<float>(1,0), T.at<float>(1,1), T.at<float>(1,2),
            T.at<float>(2,0), T.at<float>(2,1), T.at<float>(2,2);
    g2o::Isometry3D ret = (g2o::Isometry3D) Eigen::Quaterniond(R);
    Eigen::Vector3d t(T.at<float>(0,3), T.at<float>(1,3), T.at<float>(2,3));
    ret.translation() = t;
    return ret;
}

g2o::Isometry3D toG2oIsometry3D(const Se3& _se3) {
    return toG2oIsometry3D(_se3.T());
}

3.4 Isometry3D与SE3Quat互相转换

1）SE3Quat 类型
SE3Quat 是g2o中老版本相机位姿的表示，内部使用四元数+平移向量存储位姿，同时支持李代数上的运算，例如对数映射（log函数）、李代数上增量（update函数）、指数映射（exp函数）、伴随矩阵（adj函数）等操作

g2o中定义顶点 VertexSE3Expmap 中的 oplusImpl 函数实现就是使用左乘更新
g2o SE3Quat —>Eigen::Matrix<double,4,4>

class  VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
public:
  virtual void oplusImpl(const double* update_)  {
    Eigen::Map<const Vector6d> update(update_);
//    Vector6d update;
//    update[1] = update_[1]; update[3] = update_[3]; update[5] = update_[5];
    setEstimate(SE3Quat::exp(update)*estimate());
  }
  ......

3.5 Isometry3D <-----> SE3Quat相互转换

1. 使用SE3Quat成员函数 to_homogeneous_matrix() 将 SE3Quat->Eigen::Matrix<double,4,4>

/// Isometry3D --> SE3Quat
/**
* convert an Isometry3D to the old SE3Quat class
*/
G2O_TYPES_SLAM3D_API SE3Quat toSE3Quat(const Isometry3D& t);

/// SE3Quat->Isometry3D
/**
* convert from an old SE3Quat into Isometry3D
*/
G2O_TYPES_SLAM3D_API Isometry3D fromSE3Quat(const SE3Quat& t);

2. Mat->Se3 注意对Mat操作使用_T.colRange(0,3).rowRange(0,3).clone()获取矩阵块

Se3::Se3(const cv::Mat &_T) 
{
    assert(_T.type() == CV_32FC1);
    Mat R = _T.colRange(0,3).rowRange(0,3).clone();
    Rodrigues(R, rvec);
    tvec = _T.col(3).rowRange(0,3).clone();
}

3.6 SE2和SE3的相互转换：

SE3—>SE2

 Sophus::Vector6d se3_Rt = SE3_Rt.log();//se(3)在Sophus中用Vector6d表示,使用对数映射获得李群对应的李代数
 Eigen::Quaterniond q2(se3_Rt.matrix())
 yaw = atan2(2.0*(q2[1]*q2[2] + q2[3]*q2[0]), q2[3]*q2[3] - q2[0]*q2[0] - q2[1]*q2[1] + q2[2]*q2[2]);
 Sophus::Vector3d update_se2 (se3_Rt[4], se3_Rt[5], yaw);//x,y,yaw
 Sophus::SE2 SE2_Rt2  = Sophus::SE2::exp(update_se2 )

SE2—>SE3

 Sophus::Vector3d se2_Rt = SE2_Rt.log();//se(2)在Sophus中用Vector2d表示,使用对数映射获得李群对应的李代数
 Eigen::Vector3d euler_angles1 = se3_Rt.matrix().eulerAngles ( 2,1,0 ); // ZYX顺序，即roll pitch yaw顺序
 Sophus::Vector6d update_se3 (se2_Rt[0], se2_Rt[1], 0, 0, 0 euler_angles1[2]);
 Sophus::SE3 SE3_Rt3  = Sophus::SE3::exp(update_se3 )