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

最新推荐文章于 2024-08-21 20:20:33 发布

陆洋山芋

最新推荐文章于 2024-08-21 20:20:33 发布

阅读量421

点赞数 1

分类专栏：视觉SLAM十四讲文章标签：视觉SLAM十四讲

本文链接：https://blog.csdn.net/qq_40709702/article/details/98766200

版权

本文深入探讨了李代数在视觉SLAM中的作用，讲解了BCH公式及其线性近似在SO(3)上的应用。通过分析机器人位姿与观测数据之间的关系，阐述了如何利用李代数进行位姿优化，同时介绍了在SO(3)和SE(3)上的左扰动和右扰动模型。文章还提到了实践中使用Sophus库进行相关计算的例子。

摘要由CSDN通过智能技术生成

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}