Eigen库：线性计算及坐标变换

最新推荐文章于 2023-03-16 16:39:36 发布

Jason.Li_0012

最新推荐文章于 2023-03-16 16:39:36 发布

阅读量1.8k

点赞数

分类专栏： C++\CMake 《视觉SLAM十四讲》笔记文章标签： c++

本文链接：https://blog.csdn.net/weixin_45929038/article/details/122830433

版权

《视觉SLAM十四讲》笔记同时被 2 个专栏收录

21 篇文章 19 订阅

订阅专栏

C++\CMake

13 篇文章 4 订阅

订阅专栏

线性方程组分解

对于未知数为 $x$ 的线性方程组 $A\cdot x = b$ ，其中 A与 b为已知矩阵（b一般为向量）。求解此类问题，可以使用不同的分解方法进行求解。

若此时该方程组为超定方程组，则方程组无解，可在 $error=A\cdot x-b$ 尽可能小的处寻找最小二乘解。线性方程组的求解使用Eigen库中Dense模块中内容进行。

矩阵分解API

在Eigen中内置了许多矩阵分解API，每种方法对矩阵的要求不同，针对的矩阵大小也不同。各个分解方法都提供了solve() 方法用于求解线性方程组。

分解方式	API	矩阵要求	速度（小型）	速度（大型）	正确率
PartialPivLU	partialPivLu()	可逆矩阵	++	++	+
FullPivLU	fullPivLu()	无	- -	- -	+++
HouseholderQR	householderQr()	无	++	++	+
ColPivHouseholderQR	colPivHouseholderQr()	无	+	- -	+++
FullPivHouseholderQR	fullPivHouseholderQr()	无	- -	- -	+++
CompleteOrthogonalDecomposition	completeOrthogonalDecomposition()	无	+	- -	+++
LLT	llt()	正定矩阵	+++	+++	+
LDLT	ldlt()	正定、负定矩阵	+++	+	++
BDCSVD	bdcSvd()	无	- -	- -	+++
JacobiSVD	jacobiSvd()	无	- -	- - -	+++

SVD分解

在Eigen库提供了两种实现SVD分解的算法，采用BDCSVD分解算法可以很好地解决较大的问题，并自动退回到 JacobiSVD分解算法以解决较小的问题。

#include <iostream>
#include <Eigen/Dense>
 
using namespace std;
using namespace Eigen;
 
int main()
{
    MatrixXf A = MatrixXf::Random(3, 2);
	VectorXf b = VectorXf::Random(3);
     
	cout << "The least-squares solution is:\n"
         << A.bdcSvd(ComputeThinU | ComputeThinV).solve(b) << endl;
    return 0;
}

QR分解

例如使用ColPivHouseholderQR法分解矩阵：

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main()
{
    //  定义方程组AX=b
    Matrix3f A;
    Vector3f b;
    Vector3f x;
    //  赋值
    A << 1, 2, 3,
         4, 5, 6,
         7, 8,10;
    b << 3, 3, 4;
    
    //  colPivHouseholderQr分解矩阵
    x = A.colPivHouseholderQr().solve(b);
    //  输出结果
    cout << "The solution is:\n" << x << endl;
    
    return 0;
}

输出结果如下：

The solution is:
      -2
0.999997
       1

LLT分解

对正定矩阵可以使用LLT分解方式进行求解：

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main()
{
    //  定义数据并赋值
    Matrix2f A, b;
    Matrix2f x;
    A << 2, -1, -1, 3;
    b << 1, 2, 3, 1;
    
    //  矩阵分解与求解方程组
    x = A.ldlt().solve(b);
    cout << "The solution is:\n" << x << endl;
    
    return 0;
}

输出如下：

The solution is:
1.2 1.4
1.4 0.8

计算偏差

使用分解法进行计算求解线性方程组存在则一定的误差，可以使用如下方式计算该误差的具体大小。

#include <iostream>
#include <Eigen/Dense>
 
using namespace std;
using namespace Eigen;
 
int main()
{
    MatrixXd A = MatrixXd::Random(100,100);
    MatrixXd b = MatrixXd::Random(100,50);
    MatrixXd x = A.fullPivLu().solve(b);
    double relative_error = (A*x - b).norm() / b.norm(); // norm() is L2 norm
    cout << "The relative error is:\n" << relative_error << endl;
    
    return 0;
}

计算出偏差值后即可判断是否在可接受范围内，若不再范围内则需要重新使用其他分解方法进行求解。

分离分解、求解步骤

上述例子中，都一步进行分离与求解。如若需要，将其分开可以使用如下方式。（以LLT分解法为例）

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main()
{
    //  定义数据并赋值
    Matrix2f A, b;
    A << 2, -1,
        -1,  3;
    b << 1, 2,
         3, 1;
    
    // 声明分解法为LLT，处理的对象为Matrix2f
    LLT<Matrix2f> llt;
    //	LLT分解
    llt.compute(A);
    //  求解方程组Ax=b
    cout << "The solution is:\n" << llt.solve(b) << endl;
}

输出如下：

The solution is:
1.2 1.4
1.4 0.8

坐标变换

机器人运动中，经常需要使用到坐标变换。Eigen库中的Geometry模块提供了许多API便于计算旋转矩阵、变换矩阵等内容，主要分为两种不同的几何变换：

抽象变换： 如旋转（用角轴（旋转向量） 或四元数 表示）、平移、缩放。
- 这些转换未表示为矩阵
射影或仿射变换： 此类转换的类型为矩阵。

坐标变换API

Geometry模块中常用的表达坐标变换的方式有如下几类：

2D平面选择

Rotation2D<typename _Scalar> rot(_Scalar m_angle);

其中参数如下：

_Scalar：数据类型
m_angle：旋转角度，_Scalar类型

旋转向量

AngleAxis<typename _Scalar> angleaxis(Vector3 m_axis, _Scalar m_angle);

其中参数如下：

_Scalar：角度的数据类型
m_axis：旋转轴坐标，三维向量类型
m_angle：旋转角度，_Scalar类型

四元数

Quaternion<typename _Scalar> q;

其中参数如下：

_Scalar：角度的数据类型

此外，对于旋转矩阵，可直接使用Matrix3d矩阵进行表示

实战一：例程

此处博主正在学习高博的《视觉SLAM十四讲》，学习了书中的例程。

#include <iostream>
#include <cmath>

#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;

// 本程序演示了 Eigen 几何模块的使用方法

int main(int argc, char **argv) {

  // Eigen/Geometry 模块提供了各种旋转和平移的表示
  // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
  Matrix3d rotation_matrix = Matrix3d::Identity();
  // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
  AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));     //沿 Z 轴旋转 45 度
  cout.precision(3);      // 输出的浮点类型保留三位小数
  cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;   //用matrix()转换成矩阵
  // 也可以直接赋值
  rotation_matrix = rotation_vector.toRotationMatrix();
  // 用 AngleAxis 可以进行坐标变换
  Vector3d v(1, 0, 0);
  Vector3d v_rotated = rotation_vector * v;
  cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;
  // 或者用旋转矩阵
  v_rotated = rotation_matrix * v;
  cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;

  // 欧拉角: 可以将旋转矩阵直接转换成欧拉角
  Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即roll pitch yaw顺序
  cout << "yaw pitch roll = " << euler_angles.transpose() << endl;

  // 欧氏变换矩阵使用 Eigen::Isometry
  Isometry3d T = Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
  T.rotate(rotation_vector);                                     // 按照rotation_vector进行旋转
  T.pretranslate(Vector3d(1, 3, 4));                     // 把平移向量设成(1,3,4)
  cout << "Transform matrix = \n" << T.matrix() << endl;

  // 用变换矩阵进行坐标变换
  Vector3d v_transformed = T * v;                              // 相当于R*v+t
  cout << "v tranformed = " << v_transformed.transpose() << endl;

  // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

  // 四元数
  // 可以直接把AngleAxis赋值给四元数，反之亦然
  Quaterniond q = Quaterniond(rotation_vector);
  cout << "quaternion from rotation vector = " << q.coeffs().transpose()
       << endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
  // 也可以把旋转矩阵赋给它
  q = Quaterniond(rotation_matrix);
  cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;
  // 使用四元数旋转一个向量，使用重载的乘法即可
  v_rotated = q * v; // 注意数学上是qvq^{-1}
  cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
  // 用常规向量乘法表示，则应该如下计算
  cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;

  return 0;
}

实战二：作业

题目与提示

代码实现

#include <iostream>

#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;


int main(int argc, char** argv) {
    Quaterniond q1(0.55, 0.3, 0.2, 0.2), q2(-0.1, 0.3, 0.7, 0.2);
    // 四元数归一化
    q1.normalize();
    q2.normalize();

    Vector3d t1(0.7, 1.1, 0.2), t2(-0.1, 0.4, 0.8);
    
    // 从四元数获取到对应的旋转向量并初始化变换矩阵
    Isometry3d Tc1w(q1), Tc2w(q2);
    // 设置变换矩阵的平移向量部分
    Tc1w.pretranslate(t1);
    Tc2w.pretranslate(t2);

    Vector3d p1(0.5, -0.1, 0.2);
    // 现求解p1点在世界坐标系下的坐标，再将其变换至小萝卜二号坐标系下
    Vector3d p2 = Tc2w * (Tc1w * p1);

    cout << endl << p2.transpose() << endl;
    
    return 0;
}