SLAM14讲第四讲--李群、李代数

最新推荐文章于 2024-08-26 17:50:01 发布

vigigo

最新推荐文章于 2024-08-26 17:50:01 发布

阅读量979

点赞数 1

分类专栏： SLAM c++ 文章标签： slam c++

本文链接：https://blog.csdn.net/qq_40574123/article/details/121545106

版权

SLAM 同时被 2 个专栏收录

13 篇文章 5 订阅

订阅专栏

c++

12 篇文章 1 订阅

订阅专栏

李群

对于这种只有一个运算的集合，我们把它叫做群。
在slam中有两种群：特殊正交群 SO(3)、特殊欧氏群 SE(3)。

图中R表示旋转矩阵

李代数

SO(3)

推导过程这里不赘述，详见式4.5~4.10。
在这里插入图片描述
ф就是SO(3)的李代数so(3)中的向量表示形式。

特别注意，李代数ф^是一个反对称阵。

SE(3)

在这里插入图片描述
特别注意，fai就是so(3)中的ф，rou表示的位移，为一个(3,1)的向量。

李括号

这个概念在第四章中并未使用到，只在BCH公式近似形式推导中有所使用，这里仅给出表达式。
在这里插入图片描述

指数映射

SO(3)上的指数映射

这一个将ф、ξ与SO(3)、SE(3)连接起来的一个概念或者方法。
假设ф=θa，这有点像欧拉角的写法。实际上就是欧拉角？这个问题，我们在文末来回来。
首先引出泰勒公式
在这里插入图片描述
对于a^由两个性质

显然可以得到如下的推导

这个公式让我们会想起罗德里格斯公式

这不就把exp(ф^)与R矩阵连接起来了
我们可以得到R=exp(ф^)
值得注意的是，SO(3)与so(3)并非一一对应的关系。

SE(3)上的指数映射

在这里插入图片描述
这里有一个以J为系数的对于rou的线性变化，在后面，我们会用在BCH近似中，它又被称为：左乘BCH近似雅可比Jl。

最后给出对应关系

4.3BCH 公式与近似形式

这里引出非常重要的两个概念：

矩阵的指数函数的乘积不等于矩阵加法后的指数函数

在这里插入图片描述
即这个等式并不成立.

引出当δR变化量极小时，存在：

同理可以得出当在李代数上做加法时的指数表达式：

这个式子将会用于李代数求导，其目的在于想找最优的旋转矩阵。
最后给出一个SE(3)上的指数乘法表达式

SO(3)上李代数的求导问题

来源？假设我们要得机器人得位姿T，已知一个世界坐标位于P得点和机器人产生的观测数据z，但是由于误差得存在，使得TP=z得关系并不像理论中的美好。事实中他们并不相等，可是我们的目的是使得这个式子看上去是成立的，那么我们就要使得z无限趋近于Tp得积。

因而定义损失函数：

在这里插入图片描述
计算损失函数涉及对于自变量的求导，但是T是矩阵，SO(3)和SE(3)并没有良好定义的加法，因此引出两种解决方法：

用李代数表示姿态，然后对根据李代数加法来对李代数求导
对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型。

李代数的求导

在这里插入图片描述

左乘扰动模型

是φ表示deltaR
在这里插入图片描述

SE(3)上李代数的求导问题

-(Tp)^=(Tp)⚙
在这里插入图片描述 ### 最后对文章指数映射提出的问题作出回答
ф=θa就是我们的欧拉角
在以下条件下

Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();

so3 =      0      0 1.5708
euler_angles =     -0      0 1.5708

实践Sophus的代码:

将sophus文件夹放在ch4的目录下
CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(useSophus)

set(CMAKE_BUILD_TYPE "Release") #  Debug也是其中一种
set(CMAKE_CXX_FLAGS "-O3")

# link library
add_library(so3 SHARED sophus/so3.cpp)
add_library(se3 SHARED sophus/se3.cpp)

#link directories
include_directories("/usr/include/eigen3")
include_directories("/usr/include/boost")
include_directories("/usr/local/include/NumCpp")

# add_h and cpp
add_executable(useSophus useSophus.cpp)
target_link_libraries(useSophus so3)
target_link_libraries(useSophus se3)

useSophus.cpp

// include diectories
#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace std;

// include liberary
#include "sophus/so3.h"
#include "sophus/se3.h"

int main(int argc, char** argv)
{
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
//    R.eulerAngles()
    Eigen::Vector3d euler_v = R.eulerAngles(0,1,2);
    cout << "euler_angles = " << euler_v.transpose() << endl;

    Sophus::SO3 SO3_R(R); //Sophus::SO(3)可以直接从旋转矩阵构造 (0,0,3.14/2)
    Sophus::SO3 SO3_V(0,0,M_PI/2); //亦可从旋转向量构造
    Eigen::Quaterniond q(R);
    Sophus::SO3 SO3_q(q); // 或者四元数
//
//    cout<<"SO(3) from matrix: "<<SO3_R<<endl;
//    cout<<"SO(3) from vector: "<<SO3_V<<endl;
//    cout<<"SO(3) from quaternion :"<<SO3_q<<endl;

    Eigen::Matrix3d SO3ToR = SO3_R.matrix(); // 一定是一个负对称矩阵,但这个矩阵不是我们要的矩阵，正确的表达如下：
//    cout << SO3ToR << endl;
    // 使用对数映射获得它的李代数
    Eigen::Vector3d so3 = SO3_R.log(); //log由李群到李代数，exp由李代数到李群
    cout<<"so3 = "<<so3.transpose()<<endl;
    // 再使用hat方法获取某一个so3（李代数）的反对称矩阵
    Eigen::Matrix3d hat_M = Sophus::SO3::hat(so3);
    cout<<"hat_M = "<<hat_M<<endl;
    // 相对的, vee 为反对称到向量
    cout << "so3 hat vee = " << Sophus::SO3::vee(hat_M) << endl;

    // 增量扰动模型的更新
    Eigen::Vector3d update_so3(1e-4,0,0); //这是delta R
    Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3)*SO3_R;
    cout << "SO3 updated = " << SO3_updated << endl;

    /******************** 萌萌的分割线 *****************************/
    cout<<"************ 我是分割线 *************"<<endl;

    Eigen::Vector3d t(1,0,0);
    Sophus::SE3 SE3_Rt(R, t); //SE3相比于SO3多了一个平移，为欧式旋转矩阵
    Sophus::SE3 SE3_qt(q, t); // 四元数构造法
    cout<<"SE3 from R,t= "<<endl<<SE3_Rt<<endl;
    cout<<"SE3 from q,t= "<<endl<<SE3_qt<<endl;

    Eigen::Matrix<double, 6,1> ksi = SE3_Rt.log(); //4X4
    cout << "ksi = " << ksi.transpose() << endl;
    cout<<"ksi Matrix= "<<endl<<Sophus::SE3::hat(ksi)<<endl;

    typedef Eigen::Matrix<double,6,1> Vector6d;
    Vector6d delta_ksi;
    delta_ksi.setZero();
    cout << "delta_ksi = " << delta_ksi.transpose() <<endl;
    delta_ksi(0,0) = 1e-4; //matrix
    cout << "delta_ksi = " << delta_ksi.transpose() <<endl;
    Sophus::SE3 SE3_updated = Sophus::SE3::exp(delta_ksi)*SE3_Rt;
    cout<<"SE3 updated = "<<endl<<SE3_updated.matrix()<<endl;

    return 0;
}