李群、李代数 基本示例程序

#include    <iostream>
#include    <cmath>

#include    <Eigen/Dense>
#include    <sophus/common.hpp>
#include    <sophus/geometry.hpp>

using namespace  std;
using namespace  Eigen;



int main(int argc, const char *argv[])
{
    Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
    Quaterniond q(R);
    // ************  SO(3) ************
    Sophus::SO3d SO3d_R(R); 
    Sophus::SO3d SO3d_q(q); // Result should be the same as SO3d_R
    cout << "SO(3) from rotation matrix = \n" << SO3d_R.matrix() << endl;
    cout << "SO(3) from quaternion = \n" << SO3d_q.matrix() << endl;
    // Logarithmic map to get the lie algebra
    Vector3d so3 = SO3d_R.log();
    cout << "so3 = \n" << so3.transpose() << endl;
    // Hat is from vector to skew-symmetric matrix
    cout << "so3 hat = \n" << Sophus::SO3d::hat(so3) << endl;
    // Vee is from skew-symmetric matrix to vector
    cout << "so3 vee = \n" << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;
    // Update by perturbation model
    Vector3d update_so3(1e-4, 0, 0);
    Sophus::SO3d SO3d_updated = Sophus::SO3d::exp(update_so3) * SO3d_R;
    cout << "SO3 updated = \n" << SO3d_updated.matrix() << endl;
    cout << "****************************" << endl;
    return 0;
}