视觉SLAM Eigen库的实践

下面是视觉slam14讲中的代码:

首先要知道Matrix意思是矩阵,Vector意思是向量。

#include <iostream>

using namespace std;

#include <ctime>
// Eigen 核心部分
#include <Eigen/Core>
// 稠密矩阵的代数运算(逆,特征值等)
#include <Eigen/Dense>

using namespace Eigen;

#define MATRIX_SIZE 50

/****************************
* 本程序演示了 Eigen 基本类型的使用
****************************/

        这段代码开头部分主要是引入了一些必要的头文件、进行了命名空间声明以及定义了相关的常量等基础操作,为后续利用 Eigen 库进行线性代数相关的演示代码编写做准备。


int main(int argc, char **argv) {
  // Eigen 中所有向量和矩阵都是Eigen::Matrix,它是一个模板类。
  //它的前三个参数为:数据类型,行,列

  // 声明一个两行三列的float矩阵叫matrix_23
  Matrix<float, 2, 3> matrix_23;

  // 同时,Eigen 通过 typedef 提供了许多内置类型,不过底层仍是Eigen::Matrix
  // 例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>,即三维向量
  Vector3d v_3d;
  // 这是一样的

  //声明一个三行一列的矩阵,也可以说是向量,名为vd_3d
  Matrix<float, 3, 1> vd_3d;

  // Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
  Matrix3d matrix_33 = Matrix3d::Zero(); //初始化为零

  // 如果不确定矩阵大小,可以使用动态大小的矩阵
  Matrix<double, Dynamic, Dynamic> matrix_dynamic;

  // 更简单的
  MatrixXd matrix_x;

  // 这种类型还有很多,我们不一一列举
  // 下面是对Eigen阵的操作
  // 输入数据(初始化)
  matrix_23 << 1, 2, 3, 4, 5, 6;

  // 输出
  cout << "matrix 2x3 from 1 to 6: \n" << matrix_23 << endl;

 这段代码最后输出的内容是

matrix 2x3 from 1 to 6: 
1 2 3
4 5 6

// 用()访问矩阵中的元素
  cout << "print matrix 2x3: " << endl;
  for (int i = 0; i < 2; i++)

{
    for (int j = 0; j < 3; j++) cout << matrix_23(i, j) << "\t";
    cout << endl;
  }

 两个for循环为嵌套关系,一个外循环,一个内循环。注意内层for循环大括号省略了,如果循环体(或条件判断后的执行语句块)只有一条语句时,大括号是可以省略的。


 // 矩阵和向量相乘(实际上仍是矩阵和矩阵)
  v_3d << 3, 2, 1;
  vd_3d << 4, 5, 6;

  // 但是在Eigen里你不能混合两种不同类型的矩阵,像这样是错的
  // Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;
  // 应该显式转换
  Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
  cout << "[1,2,3;4,5,6]*[3,2,1]=" << result.transpose() << endl;

  Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
  cout << "[1,2,3;4,5,6]*[4,5,6]: " << result2.transpose() << endl;

        这是先把数据分别传给v_3d矩阵和vd_3d矩阵,然后分别与matrix_23矩阵相乘,但由于类型不一致不能直接相乘,所以要先统一类型(各自什么类型程序中有说明)。分别把结果存在了result和result2,最后再输出result的转置。transpose()就是转置的意思。

  // 同样你不能搞错矩阵的维度
  // 试着取消下面的注释,看看Eigen会报什么错
  // Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() *v_3d; 


 // 一些矩阵运算
  // 四则运算就不演示了,直接用+-*/即可。
  matrix_33 = Matrix3d::Random();    // 随机数矩阵

  cout << "random matrix: \n" << matrix_33 << endl;
  cout << "transpose: \n" << matrix_33.transpose() << endl;      // 转置
  cout << "sum: " << matrix_33.sum() << endl;            // 各元素和
  cout << "trace: " << matrix_33.trace() << endl;          // 迹
  cout << "times 10: \n" << 10 * matrix_33 << endl;               // 数乘
  cout << "inverse: \n" << matrix_33.inverse() << endl;        // 逆
  cout << "det: " << matrix_33.determinant() << endl;    // 行列式

       这段也十分好理解,就是先创建了一个随机数矩阵matrix_33,然后分别输出这个矩阵本身,转置,各元素和,迹,数乘,逆,行列式。


// 特征值
  // 实对称矩阵可以保证对角化成功
  SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);
  cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
  cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;

SelfAdjointEigenSolver 是 Eigen 库中定义的一个用于求解自伴矩阵(对于实矩阵来说,也就是实对称矩阵)特征值分解的类模板。这里通过 <Matrix3d> 指定了模板参数,意味着它将作用于 3×3 大小的双精度(double 类型)矩阵(Matrix3d 是 Eigen 库中表示 3×3 双精度矩阵的类型)。

声明了一个名为 eigen_solver 的对象,其类型为 SelfAdjointEigenSolver<Matrix3d>

实对称矩阵就是矩阵本身乘矩阵的转置,即为(matrix_33.transpose() * matrix_33)

后面为输出矩阵的特征值和特征向量, eigen_solver.eigenvalues() 为特征值,eigen_solver.eigenvectors()为特征向量。


 // 解方程
  // 我们求解 matrix_NN * x = v_Nd 这个方程
  // N的大小在前边的宏里定义,它由随机数生成
  // 直接求逆自然是最直接的,但是求逆运算量大
  Matrix<double,MATRIX_SIZE,MATRIX_SIZE>matrix_NN=MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
  matrix_NN = matrix_NN * matrix_NN.transpose();  // 保证半正定
  Matrix<double, MATRIX_SIZE, 1> v_Nd = MatrixXd::Random(MATRIX_SIZE, 1);

  clock_t time_stt = clock(); // 计时
  // 直接求逆
  Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;
  cout << "time of normal inverse is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
  cout << "x = " << x.transpose() << endl;

clock_t 是 C++ 中定义的一种用于表示时间的数据类型(通常是整数类型的别名),它用于存放从程序开始运行到当前时刻的时钟周期数等相关计时信息。

clock() 是 <ctime> 头文件中提供的一个函数(在代码开头已经引入了 <ctime> 头文件),该函数返回从程序启动开始到调用这个函数时所经过的时钟周期数。这里将 clock() 的返回值赋给 time_stt 变量,其目的就是记录下当前这个时刻的时间点,以便后续通过与其他时间点对比来计算程序执行某个操作所花费的时间,实现计时的功能。

cout << "time of normal inverse is " << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;

  • 首先输出字符串 "time of normal inverse is " 到标准输出(屏幕),用于提示接下来要输出的内容是进行矩阵求逆操作所花费的时间信息。
  • clock() - time_stt 计算了从之前记录的时间点 time_stt 到当前时刻(也就是执行完矩阵求逆及相关操作后)所经过的时钟周期数,得到这个操作过程所花费的时钟周期差值。
  • (double) CLOCKS_PER_SEC 将 CLOCKS_PER_SEC(这是 <ctime> 头文件中定义的一个常量,表示每秒包含的时钟周期数)强制转换为 double 类型,以便进行后续的浮点数运算。
  • 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC 通过先将时钟周期差值乘以 1000(将单位从秒转换为毫秒,因为 CLOCKS_PER_SEC 对应的是每秒的时钟周期数,乘以 1000 就换算成了每毫秒的时钟周期数),再除以 CLOCKS_PER_SEC(进行单位换算和归一化),最终得到的结果就是矩阵求逆操作所花费时间的毫秒数,然后将这个毫秒数输出到屏幕上,最后再输出字符串 "ms" 进一步明确时间单位,完成整个时间花费情况的展示。

       这段代码主要实现了对前面定义的矩阵 matrix_NN 进行求逆,并利用求逆结果求解一个线性方程组(从线性代数意义上理解)得到向量 x,同时还通过记录时间点并计算差值的方式统计了进行矩阵求逆操作所花费的时间,并将这些结果(时间信息和 x 的值)输出显示到标准输出设备(屏幕)上,便于分析和评估相关操作的性能以及查看计算得到的具体结果。


 // 通常用矩阵分解来求,例如QR分解,速度会快很多
  time_stt = clock();
  x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
  cout << "time of Qr decomposition is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
  cout << "x = " << x.transpose() << endl;

  // 对于正定矩阵,还可以用cholesky分解来解方程
  time_stt = clock();
  x = matrix_NN.ldlt().solve(v_Nd);
  cout << "time of ldlt decomposition is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
  cout << "x = " << x.transpose() << endl;

  return 0;
}

x = matrix_NN.colPivHouseholderQr().solve(v_Nd);

  • 矩阵分解操作
  • matrix_NN.colPivHouseholderQr() 调用了 matrix_NN 矩阵(前面已定义的方阵)的 colPivHouseholderQr() 方法。在 Eigen 库中,这是执行列主元豪斯霍尔德(Householder)QR 分解的操作,它会将矩阵 matrix_NN 分解为正交矩阵 Q 和上三角矩阵 R 的乘积形式,并且考虑了列主元的选取,这种分解方式在数值稳定性等方面有良好的表现,常用于求解线性方程组等场景。
  • .solve(v_Nd) 紧接着在上述 QR 分解的结果基础上,调用 solve() 函数用于求解线性方程组。这里相当于利用 QR 分解后的结果来求解方程 matrix_NN * x = v_Nd,将求得的解向量赋值给 x 变量。相比于直接求逆来求解线性方程组,QR 分解这种方式在一些情况下(尤其是对于大型矩阵或者数值稳定性要求较高的场景)可能具有更好的计算效率和数值准确性。

x = matrix_NN.ldlt().solve(v_Nd);

  • Cholesky 分解相关操作
  • matrix_NN.ldlt() 调用了 matrix_NN 矩阵的 ldlt() 方法,在 Eigen 库中,对于正定矩阵,ldlt() 执行的是 LDLT 分解(一种类似 Cholesky 分解的变体,它将矩阵分解为下三角矩阵 L、对角矩阵 D 和 L 的转置的乘积形式,在处理一些特殊的正定矩阵时更具优势,比如处理有零主元等情况时更灵活)。
  • 同样,后续的 .solve(v_Nd) 是基于 LDLT 分解的结果来求解线性方程组 matrix_NN * x = v_Nd,并把求得的解向量赋值给 x 变量,这是利用 Cholesky 分解相关方法来求解线性方程组的具体实现方式。

后面输出时间的操作和上述一样。

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值