SLAM -4讲

最新推荐文章于 2024-06-01 17:50:53 发布

雁瑜彭

最新推荐文章于 2024-06-01 17:50:53 发布

阅读量182

点赞数 1

分类专栏： SLAM14讲文章标签： c++ 算法

本文链接：https://blog.csdn.net/A2000613/article/details/127931037

版权

SLAM14讲专栏收录该内容

5 篇文章 1 订阅

订阅专栏

一、下载Sophus库

Sophus库是在Eigen的基础上进行开发的，分为模板/非模板类版本两种，SLAM14讲开源代码使用的为非模板类

1.获取(非模板)方式

目录：slam14/slambook2/3rdparty 下有 Sophus.tar.gz 解压

// 在上面基础
cd Sophus
mkdir build
cd build
cmake ..
make

会出现以下ERROR:

在这里插入图片描述

错误解决：

中文翻译：错误：左值需要作为赋值的左操作数 例如： 3=b

将so2.cpp中 33 lines 改为：

SO2::SO2()
{
        unit_complex_.real(1);
        unit_complex_.imag(0);
}

2.获取(模板)方式

git clone https://github.com/strasdat/Sophus  # 模板Sophus库

二、实践Sophus

mian.cpp

#include <iostream>
#include <cmath>
// 创建矩阵（矩阵的表达）
#include <Eigen/Core>
// 矩阵的几何运算（旋转向量、欧拉角、四元数等）
#include <Eigen/Geometry>

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

using namespace std;


int main(int argc,char* argv[]) {
    // Z轴旋转90度的旋转向量
    Eigen::AngleAxisd rotation_vector(M_PI/2,Eigen::Vector3d(0,0,1));
    // 将沿着Z轴旋转90度的旋转向量转换为旋转矩阵
    Eigen::Matrix3d R = rotation_vector.toRotationMatrix();

    /********************  区块1   **********************/
    // 通过SO3类的构造函数，将旋转矩阵赋值给特殊正交群(SO3李群)---SO(3)实际是旋转矩阵R的集合
    // 函数原型：SO3::SO3(const Matrix3d & _R);
    Sophus::SO3 SO3_R(R);

    // 也可以通过旋转向量进行构造
    // 函数原型：SO3::SO3(double rot_x,double rot_y,double rot_z);
    // 沿着Z轴旋转90的旋转向量赋值为 李群SO3
    Sophus::SO3 SO3_V(0,0,M_PI/2);

    // 四元数  旋转矩阵转四元数
    Eigen::Quaterniond q(R);
    // 通过四元数对李群SO3进行构造
    Sophus::SO3 SO3_q(q);
    /********************  区块1 的表达式均等价  **********************/


    /********  区块2  输出SO(3)时，以so(3)  李代数(定义在R3三维向量空间上的三维向量)  形式输出  *******/
    // 也可以通过 SO3_R.matrix()   SO3_V.matrix()  SO3_q.matrix() 将李群以三阶矩阵的形式输出
    cout << "SO(3) from matrix: \n" << SO3_R << endl;
    cout << "SO(3) from vector: \n" << SO3_V << endl;
    cout << "SO(3) from quaternion: \n" << SO3_q <<endl;
    /********  区块2  ******************/


    /********* 区块3   ************/
    // 使用对数映射获得其李代数(定义在R3三维向量空间上的三维向量)
    // 李代数-->李群 指数映射   李群-->李代数 对数映射
    Eigen::Vector3d so3 = SO3_R.log();
    cout << "so(3) = \n" << so3.transpose() << endl << endl;

    // Antisymmetric_matrix 为李代数（三维向量）到反对称矩阵(R 3*3)
    // 通过函数 static Matrix3d SO3::hat(const Vector3d & omega) 实现转换;
    Eigen::Matrix3d Antisymmetric_matrix = Sophus::SO3::hat(so3);
    cout << "Antisymmetric_matrix(李代数 转 反对称矩阵) = " << endl << Antisymmetric_matrix << endl << endl;

    // 反对称矩阵到向量 (李代数so3)
    // 通过函数  static Vector3d SO3::vee(const Matrix3d & Omega) 实现转换
    Eigen::Vector3d Antisymmetric_matrix_to_vector = Sophus::SO3::vee(Antisymmetric_matrix).transpose();
    cout << "Antisymmetric_matrix_to_vector(反对称矩阵 转 向量) = " << endl << Antisymmetric_matrix_to_vector << endl << endl;
    /********* 区块3   ************/


    /********  区块4 增量扰动模型的更新  *******/
    // update_so3 为一个三维向量 以此表示李代数 为更新量
    // Sophus::SO3::exp(update_so3) 表示该李代数所对应的李群，对原来的旋转矩阵左乘一个细微的扰动
    // 最后得到结果是一个更新的旋转矩阵以李代数的格式输出
    Eigen::Vector3d update_so3(1e-4,0,0);
    // 左乘更新
    Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3) * SO3_R;
    cout << "SO3 updated = \n" << SO3_updated << endl;
    /********  区块4 ************************/



    /******************  以下是对SE(3)的操作   ************************/
    /********  区块5 ************************/
    // 创建平移向量
    Eigen::Vector3d t(1,0,0);
    // 从 R t 构建特殊欧式群 SE(3) 实际是变换矩阵T的集合
    Sophus::SE3 SE3_Rt(R,t);
    // 从 q t 构建 SE(3)
    Sophus::SE3 SE3_qt(q,t);
    // SE(3)  六维数据(结果显示前三维是旋转分量即se(3) 后三维是平移向量t)
    cout << "SE3 from R t = \n" << SE3_Rt << endl;
    cout << "SE3 from q t = \n" << SE3_qt << endl;
    // 将SE(3) 以矩阵的形式输出得到 4*4的变换矩阵T
    cout << "SE3 以矩阵的形式输出T = " << endl << SE3_Rt.matrix() << endl << endl;

    // 李代数 se(3) 是一个六维向量
    typedef Eigen::Matrix<double,6,1> Vector6d;
    // SE(3)李群 通过对数映射转化为 李代数se3
    Vector6d se3 = SE3_Rt.log();
    // se(3) 输出 前三维为平移(此处的平移与平移向量t之间存在一个J雅可比矩阵因子) 后三维度为旋转
    cout << "se3 = " << se3.transpose() << endl << endl;

    // se3李代数 到 四维矩阵R4*4（这里不再表示反对称矩阵）
    Eigen::Matrix<double,4,4> se3_to_matrix = Sophus::SE3::hat(se3);
    cout << "se3_to_matrix = " << endl << se3_to_matrix << endl << endl;


    // 四维矩阵 到 李代数se3
    Eigen::Matrix<double,1,6> se3_to_matrix_toVector = Sophus::SE3::vee(se3_to_matrix).transpose();
    cout << "se3_to_matrix_toVector = " << endl << se3_to_matrix_toVector << endl << endl;
    /********  区块5 ************************/


    /********  区块6 ************************/
    Vector6d update_se3;
    // 全部元素变为o
    update_se3.setZero();
    // 赋值对矩阵(6*1)的(0,0)处赋值为 1e-4
    update_se3(0,0) = 1e-4;
    // 左乘
    Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3) * SE3_Rt;
    cout << "SE3 updated(se(3)) = " << endl << SE3_updated << endl;
    cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;
    //
    /********  区块5 ************************/
    return 0;
}

CMakeLists.txt版本1

cmake_minimum_required(VERSION 3.16)
project(useSophus)

# c++14
set(CMAKE_CXX_STANDARD 14)
# 针对C++编译器进行，对源代码进行优化
set(CMAKE_CXX_FLAGS "-O3")


# 添加Eigen的头文件，用于矩阵的创建运算
include_directories("/usr/local/include/eigen3")

# 使用 sophus 需要使用find_package找到他
find_package(Sophus REQUIRED)
# 添加Sophus的头文件，用于李代数的计算
include_directories(${Sophus_INCLUDE_DIRS})

# 生成可执行文件
add_executable(useSophus main.cpp)
# 链接Sophus源代码编译生成的库
target_link_libraries( useSophus ${Sophus_LIBRARIES} )

CMakeLists.txt版本2

cmake_minimum_required(VERSION 3.16)
project(useSophus)

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


# 添加库源文件 生成 动态库
add_library(so3 SHARED ~/slam14_m/slambook/3rdparty/Sophus/sophus/so3.cpp)
add_library(se3 SHARED ~/slam14_m/slambook/3rdparty/Sophus/sophus/se3.cpp)

# 添加头文件
include_directories("/usr/local/include/eigen3")
include_directories("~/slam14_m/slambook/3rdparty/Sophus")

# 添加执行文件
add_executable(useSophus main.cpp)

# 链接库
target_link_libraries(useSophus so3)
target_link_libraries(useSophus se3)

运行结果

/home/zxz/my_slam14/ch4/useSophus/cmake-build-debug/useSophus
SO(3) from matrix: 
     0      0 1.5708

SO(3) from vector: 
     0      0 1.5708

SO(3) from quaternion: 
     0      0 1.5708

so(3) = 
     0      0 1.5708

Antisymmetric_matrix(李代数 转 反对称矩阵) = 
      0 -1.5708       0
 1.5708       0      -0
     -0       0       0

Antisymmetric_matrix_to_vector(反对称矩阵 转 向量) = 
     0
     0
1.5708

SO3 updated = 
 7.85398e-05 -7.85398e-05       1.5708

SE3 from R t = 
     0      0 1.5708
1 0 0

SE3 from q t = 
     0      0 1.5708
1 0 0

SE3 以矩阵的形式输出T = 
2.22045e-16          -1           0           1
          1 2.22045e-16           0           0
          0           0           1           0
          0           0           0           1

se3 =  0.785398 -0.785398         0         0         0    1.5708

se3_to_matrix = 
        0   -1.5708         0  0.785398
   1.5708         0        -0 -0.785398
       -0         0         0         0
        0         0         0         0

se3_to_matrix_toVector = 
 0.785398 -0.785398         0         0         0    1.5708

SE3 updated(se(3)) = 
     0      0 1.5708
1.0001      0      0

SE3 updated = 
2.22045e-16          -1           0      1.0001
          1 2.22045e-16           0           0
          0           0           1           0
          0           0           0           1

进程已结束，退出代码为 0