SLAM学习~ch4

最新推荐文章于 2023-12-10 01:29:11 发布

闲某欧.

最新推荐文章于 2023-12-10 01:29:11 发布

阅读量156

点赞数

分类专栏：个人学习 SLAM 文章标签： c++

本文链接：https://blog.csdn.net/m0_54225834/article/details/118789292

版权

个人学习同时被 2 个专栏收录

17 篇文章 0 订阅

订阅专栏

SLAM

12 篇文章 0 订阅

订阅专栏

理论部分：

太多公式了，写在草稿上就直接上传图片了。

李群	李代数
R	$\phi =\theta n$ (旋转向量)
T	$\xi =\begin{pmatrix} \rho \\ \phi \end{pmatrix}$ （p为平移）

二. Sophus库的使用

1、定义

1）SO(3)的定义

Sophus::SO3d SO3(R);//R为旋转矩阵

或

Sophus::SO3d SO3_1(q);//q为四元数

2）so(3)的定义

Vector3d so3；

3）SE(3)的定义

SO3d SO3(R,t);

4）se(3)的定义

Vector6d se3;

2、补充：
1). 李代数是Vector类型,李群是SO3d或者SE3d类型
2). 反对称矩阵是SO3d类型，对应的向量(尽管也是李代数)类型相同，也是SO3d

只有在反对称矩阵和向量互相转换中李代数可以看成SO3d类型。

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/se3.hpp"

using namespace std;
using namespace Eigen;
using namespace Sophus;
/*
1. 李代数是Vector类型
2. 反对称矩阵是SO3d类型，对应的向量类型相同
*/
int main(void)
{
    AngleAxisd lidaishu(M_PI/2,Vector3d(0,0,1));
    Matrix3d R=lidaishu.matrix();
    Quaterniond q(R);
    //两种定义方式
    SO3d SO3(R);
    SO3d SO3_1(q);
    cout<<"特殊正交群是"<<endl<<SO3.matrix()<<endl;
    cout<<"特殊正交群是"<<endl<<SO3_1.matrix()<<endl;
    //李代数
    Vector3d so3=SO3.log();
    cout<<"李代数是"<<endl<<so3.transpose()<<endl;
    //向量到反对称矩阵
    cout<<"向量到反对称矩阵"<<endl<<SO3d::hat(so3)<<endl;
    //反对称矩阵到向量
    cout<<"反对称矩阵到向量"<<endl<<SO3d::vee(SO3d::hat(so3)).transpose()<<endl;

    Vector3d change(1e-5,0,0);
    SO3d SO3_changed=SO3d::exp(change)*SO3;
    cout<<"变换之后"<<endl<<SO3_changed.matrix()<<endl;

    ///SE3
    Vector3d t(1,0,0);
    SE3d SE3(R,t);
    cout<<"特殊欧氏群是"<<endl<<SE3.matrix()<<endl;
    Vector6d se3=SE3.log();
    cout<<"李代数是"<<endl<<se3.transpose()<<endl;
    //向量到反对称矩阵
    cout<<"向量到反对称矩阵"<<endl<<SE3d::hat(se3)<<endl;
    //反对称矩阵到向量
    cout<<"反对称矩阵到向量"<<endl<<SE3d::vee(SE3d::hat(se3)).transpose()<<endl;

    Vector6d update;
    update.setZero();
    update(0,0)=1e-4;
    SE3d updated=SE3d::exp(update)*SE3;
    cout<<"变换之后"<<endl<<updated.matrix()<<endl;
    return 0;
}