视觉SLAM理论与实践2

最新推荐文章于 2024-05-31 10:29:59 发布

尹扣0801

最新推荐文章于 2024-05-31 10:29:59 发布

阅读量551

点赞数

分类专栏：公式推导 C++实践文章标签：视觉SLAM C++ Eigen

本文链接：https://blog.csdn.net/whut_chengjun/article/details/103300333

版权

公式推导同时被 2 个专栏收录

4 篇文章 1 订阅

订阅专栏

C++实践

4 篇文章 0 订阅

订阅专栏

视觉SLAM理论与实践2-三维空间刚体运动-附代码链接

一、熟悉Eigen矩阵运算

Eigen是常用的C++ 矩阵运算库，具有很高的运算效率。大部分需要在C++ 中使用矩阵运算的库，都会选用Eigen 作为基本代数库，例如Google Tensorflow，Google,Ceres，GTSAM 等。
设线性方程Ax = b，在A 为方阵的前提下，请回答以下问题：
1）在什么条件下，x 有解且唯一?
当方程组的系数矩阵的秩与方程组增广矩阵的秩相等且均等于方程组中未知数个数n的时候，方程组有唯一解。
2）高斯消元法的原理是什么？
若用初等行变换将增广矩阵化为，则AX = B与CX = D是同解方程组。通过用初等行变换将增广矩阵化为行阶梯阵，然后通过回带求解线性方程组的解
可参考以下例子理解，计算线性方程组的解如下：
在这里插入图片描述解算过程如下：

3）QR分解的原理是什么？
QR分解就是把矩阵分解成一个正交矩阵和一个上三角矩阵，通常用来求解线性最小二乘问题。
4）Cholesky分解的原理是什么？
Cholesky分解法又叫平方根法，是求解对称正定线性方程组最常用的方法之一。对于一般矩阵，为了消除LU分解的局限性和误差的过分积累，采用了选主元的方法，但对于对称正定矩阵而言，选主元是不必要的。
定理：若 $\ A\isin R\raisebox{0.25em}{nxn}$ 对称正定，则存在一个对角元为正数的下三角矩阵 $\ L\isin R\raisebox{0.25em}{nxn}$ ，使得 $\ A= R\raisebox{0.5em}{nxn}$ 成立。 $A=LL^{T}$ 成立。
5）编程实现 A 为 100 × 100 随机矩阵时，⽤ QR 和 Cholesky 分解求 x 的程序。
useEigen.cpp代码如下：

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <ctime>
#define MATRIX_SIZE 100
using namespace std;
int main() {
    //Ax=B 用QR和Cholesky分解求x
    
    Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> A;//100*100的动态矩阵
    A = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);//随机产生
    A = A.transpose() * A;//产生对称矩阵
    cout<<"Matrix_A:"<<endl;
    cout<<A<<endl;//随机矩阵
    Eigen::Matrix<double,Eigen::Dynamic,1> B;
    B = Eigen::MatrixXd::Random(MATRIX_SIZE,1);
    Eigen::Matrix<double,Eigen::Dynamic,1> x;
    clock_t time_stt = clock();  //计时
    //QR分解
    x =A.colPivHouseholderQr().solve(B);//QR分解的结果
    cout << "time use QR is:" << 1000* (clock()-time_stt)/(double) CLOCKS_PER_SEC << " ms " << endl;
    cout<<"QR result:"<<endl;
    cout<<x<<endl;
    time_stt = clock();
    x = A.llt().solve(B);//LL分解的结果
    cout << "time use LLT is:" << 1000* (clock()-time_stt)/(double) CLOCKS_PER_SEC << " ms " << endl;
    cout<<"LLT result:"<<endl;
    cout<<x<<endl;
    return 0;
}

CmakeLists.txt:

cmake_minimum_required( VERSION 2.8 )
project( useEigen )
//set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-O3" )
set( CMAKE_BUILD_TYPE "Debug" )
# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )
add_executable( useEigen useEigen.cpp )

此部分代码可点击这里获取

二、几何运算练习

设有小萝卜1一号和小萝卜二号位于世界坐标系中。小萝卜一号的位姿为：q1 = [0:55; 0:3; 0:2; 0:2]; t1 =[0:7; 1:1; 0:2]T（q 的第一项为实部）。这里的q 和t 表达的是Tcw，也就是世界到相机的变换关系。小萝卜二号的位姿为q2 = [-0:1; 0:3;-0:7; 0:2]; t2 = [-0:1; 0:4; 0:8]T 。现在，小萝卜一号看到某个点在自身的坐标系下，坐标为p1 = [0:5;-0:1; 0:2]T ，求该向量在小萝卜二号坐标系下的坐标。
xlb.cpp:

#include <iostream>
//稠密矩阵的代数运算（逆、特征值等）
#include<cmath>
using namespace std;
#include<Eigen/Core>
#include<Eigen/Dense>
//Eigen几何模块
#include<Eigen/Geometry>
int main(int argc,char** argv)
{
Eigen::Vector3d p1,t1,t2;
p1<<0.5,-0.1,0.2;
t1<<0.7,1.1,0.2;
t2<<-0.1,0.4,0.8;
//四元素Eigen::Quaterniond 的正确初始化顺序为Eigen::Quaterniond(w,x,y,z)
//而coeffs的顺序是（x,y,z,w）,w为实部，x,y,z为虚部
//因为要表示相反的旋转，故输入为q1的共轭，即实部不变，虚部为相反数
Eigen::Quaterniond q1=Eigen::Quaterniond(0.55,-0.3,-0.2,-0.2).normalized();//将q1归一化处理
cout<<"q1:"<<endl<<q1.coeffs().transpose()<<endl;
Eigen::Quaterniond q2=Eigen::Quaterniond(-0.1,0.3,-0.7,0.2).normalized();//将q2归一化处理
cout<<q2.coeffs().transpose()<<endl;
Eigen::Vector3d pw =q1*(p1-t1); //数学上是q*p*q-1
Eigen::Vector3d p2 =q2*pw+t2;
cout<<"p2:"<<endl<<p2<<endl;
return 0;
}

CmakeLists.txt:

cmake_minimum_required(VERSION 2.8)
project(xlb)
#设置编译模式
//set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-O3" )
set(CMAKE_BUILD_TYPE "Debug")
#添加头文件
include_directories("/usr/include/eigen3")
add_executable(xlb xlb.cpp)

终端打印结果如下：
在这里插入图片描述
此部分代码可点击这里获取
注：参考答案是P2=[1.08228,0.663509,0.686957],显然答应结果与参考答案一致。

三、旋转的表达

1）设有旋转矩阵 R，证明 $R^{T}R=I$ 且 $d e t R = + 1$
答：在“罗德里格斯公式证明”第2）小节中已经利用罗德里格斯公式证明了 ,所以这里也间接得证。
2）设有四元素q，我们把虚部记为，实部记为，那么。请说明 $\varepsilon$ 和 $\eta$ 的维度。
答：设四元素
在这里插入图片描述
则： $\varepsilon$ 和 $\eta$ 的维度分别为3维和1维
3）手动推导见下图：

四、罗德里格斯公式证明

1）手动推导如下：
在这里插入图片描述
2）请用此式证明 $R^{-1}=R^{T}$
引入罗德里格斯公式： $R=cos\theta I+(1-cos\theta )nn^{T}+sin\theta n^{\wedge}$
它表示从旋转向量到旋转矩阵的转换过程（其中旋转向量是旋转轴为n以及旋转角度为 $\theta$ ）,那么 $R^{-1}$ 相当于沿着旋转轴n旋转了角度 $-\theta$ ，代入罗德里格斯公式得到如下公式：
$R=cos(-\theta) I+(1-cos(-\theta ))nn^{T}+sin(-\theta) n^{\wedge}$

=> $R=cos\theta I+(1-cos\theta )nn^{T}-sin\theta n^{\wedge}$
而
$R^{T}=(cos\theta I+(1-cos\theta )nn^{T}+sin\theta n^{\wedge})^{T}$
=> $R^{T}=(cos\theta I)^{T}+((1-cos\theta )nn^{T})^{T}+(sin\theta n^{\wedge})^{T}$
=> $R^{T}=cos\theta I^{T}+(1-cos\theta )n^{T}n+sin\theta n^{\wedge T}$
=> $R^{T}=cos\theta I^{T}+(1-cos\theta )nn^{T}-sin\theta n^{\wedge }$
注：因为 $n^{\wedge}$ 为反对称矩阵，所以 $n^{\wedge T}=-n^{\wedge }$
得到：
$R^{-1}=R^{T}$
公式得证！