视觉SLAM十四讲---04李群和李代数2

3、李代数求导与扰动模型

（1）BCH公式及近似形式

BCH公式展开式的前几项，其中[]为李括号：
在这里插入图片描述
考虑SO(3)上的李代数，且φ₁或φ₂为小量时，忽略小量二次以上的项，BCH线性近似为：

其中，

当一个旋转矩阵R₂（李代数为φ₂）左乘一个微小旋转矩阵JR₁（李代数为φ₁）时，可以近似地看作，在原有的李代数φ₂上，加上了一项J_l(φ₂)^-1φ₁

总结：SO(3)和SE(3)上的BCH近似公式，及李代数上的加法对应于李群上带左右雅可比的乘法。
在这里插入图片描述

（2）SO(3)李代数上的求导

重要意义：在实际SLAM过程中，经常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。
问题描述：假设某个时刻机器人的位姿为T，观察到一个世界坐标位于p的点，产生了一个观测数据z，则有：z = Tp + w，其中w是观测噪声。
我们通常计算理想与实际数据间的误差：e = z - Tp，假设有N个这样的路标点和观测，
则对机器人位姿的估计相当于找一个最优的T**，使得整体误差最小化**：
问题解决：需要计算目标函数J关于变换矩阵T的导数
用李代数解决求导问题的思路有：
- 用李代数表示姿态，然后对根据李代数加法对李代数求导
- 对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型
具体推导
- 李代数求导
- 扰动模型（左乘）
  SO(3)
  
  SE(3)

4、实践：Sophus

CMakeLists.txt

cmake_minimum_required(VERSION 2.6)
project(usesophus)

set(CMAKE_BUILD_TYPE "Debug")

include_directories("/usr/include/eigen3")

find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})

add_executable(usesophus main.cpp)
target_link_libraries(usesophus ${Sophus_LIBRARIES})

install(TARGETS usesophus RUNTIME DESTINATION bin)

useSophus.cpp

#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

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

int main(int argc, char **argv) {
    //沿Z轴转90度的旋转矩阵
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
    
    //(1)SO(3)
    Sophus::SO3 SO3_R(R); //Sophus::SO(3)可以直接从旋转矩阵构造
    Sophus::SO3 SO3_v(0,0,M_PI/2); //也可以从旋转向量构造
    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 << endl;
    
    //使用对数映射获得它的李代数fai
    Eigen::Vector3d so3 = SO3_R.log();
    cout << "so3 = " << so3.transpose() << endl << endl;
    //hat为向量到反对称矩阵φ^
    cout << "so3 hat = " << Sophus::SO3::hat(so3) << endl << endl;
    //vee为反对称矩阵到向量φ
    cout << "so3 hat vee = " << Sophus::SO3::vee(Sophus::SO3::hat(so3)).transpose() << endl << endl;
    
    //增量扰动模型的更新
    Eigen::Vector3d update_so3(1e-4,0,0); //假设更新量有这么多
    Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3)*SO3_R; //左乘更新 exp(△fai^)exp(fai^)
    cout << "SO3 updated = " << SO3_updated << endl << endl;
   
    //(2)SE(3)
    Eigen::Vector3d t(1,0,0);
    Sophus::SE3 SE3_Rt(R,t);//从R，t构造SE（3）
    Sophus::SE3 SE3_qt(q,t);//从q，t构造SO（3）
    cout << "SE3 from R,t = " << endl << SE3_Rt << endl;
    cout << "SE3 from q,t = " << endl << SE3_qt << endl << endl;
    //李代数se(3)是一个六维向量ξ
    typedef Eigen::Matrix<double,6,1> Vector6d;
    Vector6d se3 = SE3_Rt.log();
    cout << "se3 = " << se3.transpose() << endl; //se(3)输出中，平移在前，旋转在后
    //hat为ξ^
    cout << "se3 hat = " << endl << Sophus::SE3::hat(se3) << endl;
    //vee从ξ^得到ξ
    cout << "se3 hat vee = " << Sophus::SE3::vee(Sophus::SE3::hat(se3)).transpose() << endl << 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; //T = [R t, 0T 1]
    cout << "SE3 updated = " << endl << SE3_updated<< endl; //ξ= [ρ φ]T
    
    return 0;
}

来源：https://blog.csdn.net/qq_40709702/article/details/98766200

标签

李群

李代数

include

eigen

代数