高翔视觉slam14讲学习笔记（3-李群、李代数 Sophus库基础学习）

高翔视觉slam14讲学习笔记（3-李群、李代数 Sophus库基础学习）

Sophus 库练习

sophus主要用于 李群、李代数的使用，在视觉SLAM十四讲实践系列之Sophus下的位姿表示和更新优化和修正 已经有详细的介绍，跟着代码打一遍就清楚。

useSophus 练习代码

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

using     namespace  std;
using     namespace  Eigen;

//本程序演示sophus的基本用法

int main(int argc, char const *argv[])
{
  //使用旋转向量构建旋转矩阵
    Eigen::Matrix3d rotation_martix  =  Eigen::AngleAxisd(M_PI/4,Eigen::Vector3d(0,0,1)).toRotationMatrix();
    cout   <<  "用沿Z轴旋转90度的Eigen旋转向量构造Eigen旋转矩阵:\n"  << rotation_martix   << endl;

  //使用Eigen的旋转矩阵构造Sophus李群旋转矩阵SO3
    Sophus::SO3d  SO3_rotation_martix(rotation_martix);
    cout <<  "用Eigen的旋转矩阵构造Sophus李群旋转矩阵SO3:\n"  << SO3_rotation_martix.matrix()  << endl;

    //对数映射求 李群 SO3 的李代数 so3
    Eigen::Vector3d so3  = SO3_rotation_martix.log();
    cout   << "对数映射求李群SO3的李代so3=  "   << so3.transpose()  << endl;

    // 指数映射
    Sophus::SO3d SO3_rotation_martix1 =   Sophus::SO3d::exp(so3);
    cout  << "指数映射求李代数so3的李群SO3 = \n "  <<  SO3_rotation_martix1.matrix()  << endl; 

    //so3  hat 求向量到反对称矩阵
    cout << "hat求向量到反对称矩阵 = \n"  << Sophus::SO3d::hat(so3)  << endl;

    // so3 hat  vee求向量到反对称矩阵
    cout <<   "vee  求反对称矩阵到向量=  \n"   <<  Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose()  <<   endl;

    //更新李群的旋转矩阵SO3
    Eigen::Vector3d  update_so3(1e-4,0,0);
    Sophus::SO3d  SO3d_updated   =   Sophus::SO3d::exp(update_so3)*SO3_rotation_martix;
    cout  <<  "更新李群SO3的旋转矩阵 =  \n"  << SO3d_updated.matrix()  << endl;

    // 用旋转矩阵 R，平移向量t构造Sophus 表示李群变换矩阵SE3
    Eigen::Vector3d t(1,0,0);
    Sophus::SE3d  SE3_rotation_translation(rotation_martix,t);   // 旋转矩阵、平移向量  转换为  变换矩阵
    cout <<  "用旋转矩阵 R，平移向量t构造Sophus表示的李群变换矩阵SE3 =   \n"   <<  SE3_rotation_translation.matrix()   << endl; 

    typedef  Eigen::Matrix<double,6,1> Vector6d;
    Vector6d se3 =  SE3_rotation_translation.log();
    cout <<  "李代数se3 是一个六维向量  李群SE3的对数映射求李代数se3  \n"  <<  se3.transpose()  << endl;

    //se3 hat 求向量到反对称矩阵
    cout  <<  "se3 hat  = \n"  << Sophus::SE3d::hat(se3)  << endl;

    // 更新李群的变换矩阵SE3
    Vector6d  update_se3;
    update_se3.setZero();
    update_se3(0,0) = 1e-4;
    Sophus::SE3d  SE3_updated =  Sophus::SE3d::exp(update_se3) * SE3_rotation_translation;
    cout  <<  "SE3 updated  = "   << endl   << SE3_updated.matrix()  << endl; 

  return 0;
}

#CMakeLists.txt
cmake_minimum_required(VERSION 3.0)
project(useSophus)

# 为使用 sophus，需要使用find_package命令找到它
find_package(Sophus REQUIRED)

# Eigen
include_directories("/usr/include/eigen3")
add_executable(useSophus src/useSophus.cpp)
target_link_libraries(useSophus Sophus::Sophus)

add_subdirectory(example)

vslam 第三章作业

个人觉得以下两篇博文已经把作业说的很清晰，可以认真参考
深蓝学院视觉slam十四讲第3章作业
视觉SLAM十四讲（第三章作业）

eg.

在完成第六章作业，轨迹的描绘时，轨迹文件为由若干个 Twc 表达，
Twc 的格式为 [t,tx,ty,tz,qx,qy,qz,qw]
Twc的物理意义：某点从相机坐标系到世界坐标的变换矩阵。

Twc中的t平移部分的物理意义：
当该点为相机坐标系原点时该矩阵表示了相机在世界坐标系中的位姿，其平移部分即为相机在世界坐标系中的坐标。画出Twc的平移部分 (相机在世界坐标系下坐标) 便画出了机器人的轨迹。

Twc中的R四元数旋转部分的物理意义：
R部分表征 当前世界坐标系(W系) 与 不断移动的相机系(C系) 的坐标变换关系。