建一个在线商城网站,网站品牌推广,网站建设和前端开发的区别,信息流优化师培训如果对于某些线性代数的知识不太牢固#xff0c;可以看一下我的另一篇博客#xff0c;写了一些基础知识并推荐了一些视频。
旋转矩阵 单元所需的线代基础知识https://blog.csdn.net/Johaden/article/details/141023668 一、旋转矩阵
1.点、向量、坐标系 在数学中… 如果对于某些线性代数的知识不太牢固可以看一下我的另一篇博客写了一些基础知识并推荐了一些视频。
旋转矩阵 单元所需的线代基础知识https://blog.csdn.net/Johaden/article/details/141023668 一、旋转矩阵
1.点、向量、坐标系 在数学中特别是在线性代数领域一个三维空间中的坐标可以被视为一个向量与线性空间基的乘积这是因为线性空间的基提供了一种方式来表示该空间中的所有向量。为了理解这一点首先需要明确什么是线性空间的基。 线性空间的基是一组向量这组向量满足两个条件第一它们线性无关即没有向量可以通过其他向量的线性组合来表示第二它们能够生成整个线性空间(向量的数量不能少于空间的维数、秩与维数相等)即线性空间中的任何向量都可以通过这组基向量的线性组合来唯一表示。在三维空间中基通常由三个线性无关的向量组成这三个向量分别对应于空间中的三个坐标轴。若三个向量两两垂直就是一个标准正交基下面的就是组成如下的矩阵为正交矩阵A’AE两个基矩阵各自是正交矩阵相乘后仍是正交矩阵。 已知正交矩阵后可以推出转置逆即。 坐标的取值与向量本身有关也与坐标系线性空间的基选取有关。 机器人中有很多种坐标系①世界系/惯性系World ②机体系body ③传感器参考系Sensor。不同坐标系存在变换关系。传感器捕获的数据都在Sensor系里但是分析位姿时不关注传感器的移动更多关注机体的移动所以需要进行Sensorbodyworld的变换。第一个变换我们已知机器人在制作过程中就已知了人为给定。以及制作时传感器相对于机体的位置也已知可以进行转换。第二个translate需要估计需要在运动过程中估计机器人在世界的位置和姿态SLAM本质。
运算
内积定义 外积定义(在几何代数中“∧”符号表示把向量变为矩阵形式为反对称矩阵) 计算用到了行列式的拆分知识注意ijk都为单位向量模为1。写成相乘的格式利于后面的编程和推导。
几何意义方向满足右手定则大小为为平行四边形的有向面积。
坐标 当我们说一个三维空间中的坐标是一个向量乘以线性空间的基时实际上是在说任何一个点或向量在这个三维空间中的位置都可以通过其在三个基向量方向上的分量即坐标来唯一确定。换句话说如果我们用v表示一个向量用,,表示三维空间的一组基那么该向量可以表示为 其中x,y,z是该向量相对于基,,的坐标。这种表示方法说明了向量与基之间的关系即向量可以看作是其坐标与基向量的线性组合。
2.位姿变换和旋转矩阵
1位姿变换
旋转变换 坐标系e1e2e3发生一次旋转变为e1e2e3 对于一个固定向量a它的坐标如何变化 坐标关系 左乘得 我们把这样一个3×3的矩阵R称为旋转矩阵。 旋转矩阵有以下两个特性①正交矩阵 ②行列式为1充要 。 n为3时为三维空间的旋转特殊正交群可以看作一个集合。
同理R为正交矩阵转置就是逆出于复杂性考虑常用转置 示例从1位置以为旋转矩阵旋转到2位置R12表示把姿态由2系对齐到1系的旋转矩阵 进一步可以有三个坐标系的递推 欧式变换 表示把2系的坐标原点对齐到1系的平移量 在多层坐标系的变换时不得不写成 直观来看就是①旋转 ②平移 3.变换矩阵与齐次坐标 上面的形式在变换多次之后会过于复杂。因此我们要引入齐次坐标和变换矩阵重写 式。 中间红框的矩阵称为变换矩阵T。记为齐次坐标四维。
这样变换利于连续坐标系转换如 特殊欧式群(旋转平移) 此处不是简单的取反需要排除旋转的影响因为是先旋转后平移平移量受旋转量影响。 类比ykxb左右移动时不是直接调整b的值而是调整ykxb/k中b/k的值。 若世界坐标系为机器人坐标系为一个点的世界坐标为机器人坐标系下为那么满足 注意T的下标与R的下标一致“就近原则” 其中的意思就是从w到R进行平移如果把后面乘一个0向量与T一样三个0一个1结果就是矩阵的平移t很容易直接稍微算一下把R全消掉了只剩t了是世界坐标系原点在机器人坐标系下的表示反之可以自己思考一下。 4.旋转向量(角轴) 旋转向量由一个方向向量和一个标量组成其中方向向量定义了旋转的轴而标量则定义了绕该轴旋转的角度。旋转向量的长度等于旋转角度的弧度值。 旋转向量可以用一个三维向量 来表示其中 θx, θy, 和 θz 分别是绕 x, y, z 轴旋转的弧度数。但是实际上旋转向量的表示是规范化的即它的方向表示旋转轴的方向而它的模长表示旋转角度。所以一个旋转向量 r 可以表示为 其中θ 是旋转角度以弧度为单位 是单位向量指向旋转轴的方向。
转化: 旋转向量到旋转矩阵的过程由罗德里格斯公式Rodrigues’s Formula 表明 符号 ∧ 是向量到反对称的转换符 旋转矩阵到旋转向量的转换: 在三维空间中任何非平凡的旋转矩阵 R即不是恒等矩阵的旋转都有一个特殊的属性存在至少一个方向向量当这个向量被 R 作用时它仅仅可能改变长度但不会改变方向。这个方向就是旋转轴而旋转轴上的向量是旋转矩阵的一个特征向量对应于特征值 1。 二、实践部分-EIGEN
1.安装eigen通过eigen在C中进行矩阵运算 eigen安装在如下图路径中eigen的头文件库 2. Eigen 不是一个预先编译好的库而是许多头文件因此通常不需要在 CMake 或其他构建系统中显式地链接到任何库。只需要通过include_directories“目录”添加进来即可。 补充拓展CMAKE_BUILD_TYPE与CMAKE_CXX_FLAGS
CMAKE_BUILD_TYPE Release如果你将 CMAKE_BUILD_TYPE 设置为 Release编译器会在编译过程中应用各种优化技术这可能会显著提高程序的运行速度。同时它会移除一些不必要的调试信息使得可执行文件更小加载更快。但是这也意味着在程序崩溃或出现错误时调试起来会比较困难因为你没有那么多的上下文信息。 Debug与此相反设置为 Debug 会禁用大部分优化并包含大量的调试信息。这意味着程序运行时可能不会像 Release 版本那样快但是它会提供更多的信息来帮助开发者在出现问题时进行调试。对于开发阶段这是非常有用的。
CMAKE_CXX_FLAGS
这些标志可以用来指定编译器在编译时应该遵循的特定规则或应用的优化。 -O0这是默认的优化等级实际上不开启任何优化仅做最基本的处理保留所有的调试信息适合于调试。 -O1开启基本的优化比如常量传播、死代码消除等这些优化可以提高代码的执行效率但不会进行大规模的代码重组。 -O2在 -O1 的基础上增加了更多的优化包括函数内联、循环优化等它试图在编译时间和执行效率之间找到一个平衡。 -O3 会启用所有 -O2 优化加上其他更激进的优化比如循环展开、内联函数、寄存器重命名等这些都可以减少指令的数量和提高代码的执行速度。 -Os类似于 -O2但更注重于生成较小的二进制文件这在嵌入式系统或资源有限的环境下很有用。 -Ofast类似于 -O3但可能还会启用一些可能不遵守标准或可能产生错误结果的优化比如假设浮点运算满足结合律等因此在科学计算等领域使用时要谨慎。 选择哪个优化等级取决于你的具体需求。在开发阶段你可能更倾向于使用 -O0 或 -O1 来便于调试而在发布或性能关键的应用中-O2 或 -O3 会是更好的选择尽管它们可能会延长编译时间。-Os 适用于需要节省存储空间的情况而 -Ofast 则是在追求极致性能且可以接受潜在不精确计算时的选择。
—————————————————————————————— 3.编程头文件如下 要生成一个矩阵如下图第二行的提示Matrix3f意思是生成一个3*3的float精度的矩阵 而定义向量如下同理使用Vector 若不知道矩阵大小需要定义动态大小矩阵 其中的Dynamic的值为-1写-1也可以但是最好写Dynamic。MatrixXd也代表未知大小矩阵也就是动态矩阵。 Ⅰ.定义 Eigen 矩阵的常见方法
1. 使用 Eigen::Matrix 模板类
Eigen 的 Matrix 类是一个通用模板类允许你指定元素类型、行数、列数和可选的存储选项。这是定义矩阵的最通用方式。
#include Eigen/Denseusing namespace Eigen;// 定义一个动态大小的双精度矩阵
Matrixdouble, Dynamic, Dynamic mat1;
mat1.resize(3, 4); // 设置矩阵大小为 3x4// 定义一个固定大小的单精度矩阵
Matrixfloat, 2, 3 mat2;
2. 使用类型别名
Eigen 提供了一些类型别名简化了固定大小矩阵的定义。
Matrix2f mat2x2; // 2x2 浮点矩阵
Matrix3d mat3x3; // 3x3 双精度矩阵
VectorXd vec; // 动态大小的列向量元素类型为双精度
MatrixXd mat; // 动态大小的双精度矩阵
MatrixXf matXf; // 动态大小的单精度矩阵
3. 使用初始化列表
Eigen 支持使用初始化列表来定义矩阵这在初始化小型矩阵时特别方便。
Matrix2f mat2x2 { {1, 2}, {3, 4} };
4. 使用 setZero, setIdentity, setRandom 等方法初始化
Eigen 提供了一系列方法来快速初始化矩阵。
MatrixXd mat MatrixXd::Zero(3, 3); // 创建一个 3x3 的零矩阵
MatrixXd mat2 MatrixXd::Identity(3, 3); // 创建一个 3x3 的单位矩阵
MatrixXd mat3 MatrixXd::Random(3, 3); // 创建一个 3x3 的随机矩阵
Ⅱ.eigen阵的操作 10个
1. 基本矩阵创建和初始化
#include Eigen/Dense
using namespace Eigen;MatrixXd mat(2,3); // 动态大小矩阵 2x3 OR Matrix float,2,3 mat; 非动态确定矩阵
mat 1, 2, 3, 4, 5, 6;Matrix3f mat3f; // 固定大小 3x3 浮点矩阵
mat3f.setRandom(); // 随机初始化Vector3d vec; // 3维向量
vec 1, 2, 3;
PS使用取出第i行第j列如mat(i,j)以及v_3d[0],v_3d[1] 2. 矩阵和向量的数学运算
不能混合两种不同的矩阵包括维度、数字类型等比如float和double就不能往一起乘。
MatrixXd A MatrixXd::Random(3,3);
MatrixXd B MatrixXd::Random(3,3);MatrixXd C A B; // 加法
MatrixXd D A - B; // 减法
MatrixXd E A * B; // 矩阵乘法VectorXd v1 VectorXd::Random(3);
VectorXd v2 VectorXd::Random(3);double dot_product v1.dot(v2); // 向量点积
VectorXd v_sum v1 v2; // 向量加法//Matrix::Identity()创建一个单位矩阵其中主对角线上的元素为1其余元素为0。
//Matrix::Zero()创建一个零矩阵所有元素都初始化为0。
//Matrix::Ones()创建一个所有元素都设置为1的矩阵。 ::运算符在这里表示作用域解析运算符它用于访问类或命名空间的成员。在这个上下文中MatrixXd::Random意味着Random是MatrixXd类的一个静态成员函数。 Eigen 的矩阵和向量运算要求参与运算的对象具有相同的数据类型。例如你不能直接将一个 float 类型的矩阵与一个 double 类型的矩阵相乘因为它们的数据类型不匹配不遵循C的类型提升规则遵循Eigen的类型安全的原则。如果你需要将不同数据类型的矩阵进行运算通常需要先进行类型转换。例如
MatrixXf matF(2,2); // 浮点型矩阵
MatrixXd matD(2,2); // 双精度矩阵// 错误类型不匹配
// MatrixXd result matF * matD;// 正确将 float 矩阵转换为 double 矩阵
MatrixXd converted matF.castdouble();
MatrixXd result converted * matD; PS上图里有几个关键点
[1,2,3;4,5,6] 表示的是一个2x3的矩阵其中第一行为1,2,3第二行为4,5,6。[3,2,1] 如果被视为一个列向量则它是一个3x1的矩阵。 假设result是计算上述矩阵和向量相乘得到的结果那么result.transpose()就是将这个结果矩阵的行和列进行互换即转置。 几种运算 3. 访问和修改矩阵元素
MatrixXd mat MatrixXd::Random(2,2);
double element mat(0,1); // 访问第一行第二列元素
mat(0,1) 5.0; // 修改第一行第二列元素
4. 矩阵的转置和逆
MatrixXd A MatrixXd::Random(3,3);
MatrixXd At A.transpose(); // 转置
MatrixXd A_inv A.inverse(); // 逆矩阵 5. 行列和子矩阵提取
MatrixXd A MatrixXd::Random(3,3);
RowVectorXd row A.row(0); // 提取第一行
VectorXd col A.col(0); // 提取第一列
MatrixXd sub A.block(0,0,2,2); // 提取左上角的 2x2 子矩阵
6. 矩阵的拼接
MatrixXd A MatrixXd::Random(2,2);
MatrixXd B MatrixXd::Random(2,2);
MatrixXd C MatrixXd::Random(2,2);
MatrixXd D MatrixXd::Random(2,2);MatrixXd AB A.colwise() B; // 按列拼接
MatrixXd CD C.rowwise() D; // 按行拼接
MatrixXd full AB.colwise().append(CD); // 上下拼接
7. 矩阵的行列式和秩
MatrixXd A MatrixXd::Random(3,3);
double det A.determinant(); // 行列式
int rank A.fullPivLu().rank(); // 矩阵秩 LU分解是一种将矩阵分解为一个下三角矩阵Lower Triangular Matrix和一个上三角矩阵Upper Triangular Matrix的乘积的技术。对于一个n×n的矩阵A如果它可以被分解为ALU A.fullPivLu().rank()方法的原理是检查分解后的上三角矩阵U的对角线统计非零元素的数量从而计算出矩阵的秩。
8. 矩阵的特征值和特征向量
MatrixXd A MatrixXd::Random(3,3);
SelfAdjointEigenSolverMatrixXd es(A*A.transpose());
VectorXd eigenvalues es.eigenvalues(); // 特征值
MatrixXd eigenvectors es.eigenvectors(); // 特征向量 A*A.transpose()计算的是矩阵A与其转置矩阵的乘积。对于一般的矩阵 A 的结果将是一个对称矩阵 所以A*A.transpose()的结果是一个对称矩阵可以作为SelfAdjointEigenSolver的输入。SelfAdjointEigenSolver是Eigen库中的一个类用于求解自伴矩阵self-adjoint matrix的特征值问题类中包含特征值、特征向量、其他内部计算数据等。对于实数矩阵自伴矩阵就是对称矩阵即满足 。
9. 矩阵的比较和条件判断
MatrixXd A MatrixXd::Random(3,3);
MatrixXd B MatrixXd::Random(3,3);
bool isEqual (A B).all(); // 判断 A 和 B 是否完全相等 10. 矩阵的求解和分解
MatrixXd A MatrixXd::Random(3,3);
VectorXd b VectorXd::Random(3);
VectorXd x A.colPivHouseholderQr().solve(b); // 解线性方程 Ax bMatrixXd lu A.lu(); // LU 分解
MatrixXd qr A.householderQr(); // QR 分解
MatrixXd svd A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV); // SVD 分解 这里首先创建了一个3x3的随机矩阵A和一个3维随机向量b。接下来使用colPivHouseholderQr()方法对矩阵A进行QR分解其中包含了列主元的置换以增加分解的稳定性。solve(b)方法随后被用来解线性方程组Ax b这里的x就是所求的解向量 。 QR分解将矩阵A分解为A QR其中Q是一个正交矩阵意味着Q^T Q I。其中Q^T 表示矩阵Q的转置。R是一个上三角矩阵。QR分解在求解最小二乘问题和矩阵特征值问题时非常有用因为它保留了矩阵的秩信息同时保证了数值稳定性。 SVD分解将矩阵A分解为A UΣV^T其中U和V都是正交矩阵Σ是对角矩阵对角线上的元素是A的奇异值。SVD分解提供了矩阵的完整结构信息包括矩阵的秩、特征值、奇异向量等是处理线性代数问题的强有力工具尤其在数据分析和降维等领域。
Ⅲ.解方程 正半定矩阵是一个对称矩阵其所有特征值都是非负的。生成正半定矩阵的一个常见方法是将一个矩阵与其转置矩阵相乘。这是因为对于任意矩阵AA * A^T的结果总是一个正半定矩阵。在上面的代码中matrix_NN首先被赋值为其自身与自身转置的乘积。 最后成了一个MATRIX_SIZE x 1的随机列向量v_Nd。MatrixXd::Random(MATRIX_SIZE, 1)生成的向量同样具有在[-1, 1]区间均匀分布的随机元素。 解方程方法
1.直接求逆
Matrixdouble, MATRIX_SIZE, 1 x matrix_NN.inverse() * v_Nd; 这里使用了直接求逆的方法来解线性方程组。inverse() 函数计算矩阵 matrix_NN 的逆矩阵然后将逆矩阵与向量 v_Nd 相乘得到向量 x。这种方法简单直接但是当矩阵较大或接近奇异时求逆可能会非常慢且数值稳定性较差。
2.QR分解
X matrix_NN.colPivHouseholderQr().solve(v_Nd); 这里使用了QR分解来解方程。colPivHouseholderQr() 函数执行列主元Householder QR分解这是一种更稳定和更快的方法来解线性方程组尤其是对于非正交或接近奇异的矩阵。solve() 函数接收右侧向量 v_Nd 并返回解向量 X。QR分解将矩阵分解为一个正交矩阵Q和一个上三角矩阵R的乘积使得求解线性系统变得更为简单和快速。 原理一旦得到了矩阵 A 的QR分解 AQR解线性方程组 Axb 变得相当直接。由于 Q 是正交矩阵我们可以两边同时左乘 得到 由于 R 是上三角矩阵我们可以通过回代back substitution的方法很容易地解出 x。这就是 solve() 函数所做的工作它接收向量 v_Nd然后返回解向量 X。 PS回代
上三角矩阵的回代
假设我们有一个上三角矩阵 U 和相应的线性方程组 Uxb其中 U 是一个上三角矩阵x 是未知向量b 是常数向量。上三角矩阵 U 的形式如下 回代的过程从最后一行即 nn 行开始逐步向上进行 最后一行只有一个未知数 xn所以可以直接求解 然后移动到倒数第二行将已知的 xn 值代入解出 xn−1 重复这个过程直到第一行每次都将已解出的未知数代入更高行的方程中最终解出所有的未知数。 ————————————————————————————————————
3.Cholesky分解
X matrix_NN.ldlt().solve(v_Nd); 对于正定矩阵使用Cholesky分解可以提供一种快速且稳定的方法来解线性方程组。ldlt() 函数执行LDL^T分解这是Cholesky分解的一种广义形式适用于对称矩阵。即使矩阵不是严格正定的LDL^T分解仍然可以执行通过添加一个小的正数到对角线来确保正定性。solve() 函数同样接收右侧向量 v_Nd 并返回解向量 X。
如何判断正定矩阵尤其是下面推文中的第二条判定法则
矩阵正定性判定——来源博主爱学习的贝塔https://blog.csdn.net/qq_38048756/article/details/115108733
LDLT分解原理 LDLT分解是一种矩阵分解方法主要用于对称矩阵尤其是对称正定矩阵。对称正定矩阵是所有特征值都是正数的对称矩阵。这种分解将矩阵 A 分解为一个单位下三角矩阵 L一个对角矩阵 D 和 L 的转置 的乘积即 这里的 L 是一个单位下三角矩阵对角线元素为1的下三角矩阵D 是一个对角矩阵而 是 L 的转置。
LDLT解线性方程组原理 一旦有了 的分解求解线性方程组 Axb 变得相对简单可以通过以下两步完成 解 Lyb首先我们需要解出 y满足 Lyb。这是一个下三角系统的解可以通过前代forward substitution来解决因为L 是下三角矩阵。 解 Dzy接着我们解出 z满足 Dzy。由于 D 是对角矩阵这个步骤非常简单每个未知数 zi 可以直接通过 yi/dii 计算出来其中 dii 是 D 的对角线元素。 解 最后我们解出 x满足 。这是上三角系统的解可以通过回代back substitution来解决因为 是上三角矩阵。 超级强烈推荐的视频 如果缺少基础或不理解正交矩阵以及各种分解可以看下面的视频很清楚专栏里面的四个视频都很好
矩阵分解动画讲解其实也很简单https://www.bilibili.com/video/BV1mm4y1i7ex/?spm_id_from333.337.search-card.all.clickvd_source033a75d9cfb0b0df5347cafb8b033109
最后运行一下 可以看看上面三种方法哪个方法用时最少 三、欧拉角 欧拉角是由莱昂哈德·欧拉Leonhard Euler在18世纪提出的用于描述任意刚体相对于固定参考系的旋转。欧拉角通常涉及三个独立的角它们按顺序绕着三个轴旋转以达到所需的位置。 欧拉角通常定义为三个连续的旋转每个旋转都绕着一个坐标轴进行。最常见的欧拉角序列有三种
Z-X-Z或称 intrinsic Z-X-Z: 第一次绕 Z 轴旋转第二次绕新的 X 轴旋转第三次再绕新的 Z 轴旋转。X-Y-X或称 intrinsic X-Y-X: 类似于 Z-X-Z只是轴的顺序不同。Z-Y-X或称 Tait-Bryan angles: 第一次绕 Z 轴旋转第二次绕新的 Y 轴旋转第三次绕新的 X 轴旋转。这种序列在航空和航天中特别常见其中 Z 轴通常与地球的重力方向对齐。 而XrollYpitchZyaw的方向可以类比为飞机飞行的方向如下 注意旋转的次序比如先绕x转30°再绕y转20°和先绕y转20°再绕x转30°不同的顺序会导致不同的最终结果。这是因为每次旋转都会创建一个新的局部坐标系后续的旋转将基于这个新的坐标系进行轴也会跟着转所以轴变了。定义欧拉角时要先明确旋转轴顺序。 每个旋转都可以用一个角度来描述通常标记为 ϕphi、θtheta和 ψpsi。例如在 Z-Y-X 序列中
ψ航向角或Yaw角绕 Z 轴的旋转决定了刚体在水平面上的方向。θ俯仰角或Pitch角绕 Y 轴的旋转决定了刚体向上或向下倾斜的程度。ϕ滚转角或Roll角绕 X 轴的旋转决定了刚体侧向翻滚的程度。
动轴定轴
动轴旋转Intrinsic Rotation便于交流 在动轴旋转中每次旋转都是相对于物体当前的坐标系进行的。这意味着一旦物体绕一个轴旋转下一个旋转将绕着物体新的坐标轴进行。动轴旋转通常用于描述物体在其自身坐标系内的旋转例如飞机或航天器的姿态变化。 有万向锁现象。 万向锁现象 万向锁Gimbal Lock现象是指在使用欧拉角描述物体旋转时由于旋转轴的对齐导致物体的旋转自由度减少失去一个维度的控制能力的现象。这种情况在动轴intrinsic旋转中尤其常见尤其是在航空、航天及机械工程领域中使用欧拉角描述姿态时万向锁是一个需要特别注意的问题。 假设你有一架飞机使用 Z-Y-X 的欧拉角航向角、俯仰角、滚转角来描述其姿态。当飞机的俯仰角达到 ±90°即飞机头朝上或头朝下Y 轴飞机的纵向轴与地面平行时会发生万向锁。此时如果再尝试改变飞机的航向角或滚转角实际上只能改变飞机的姿态在地平面上的投影而无法改变飞机相对于地面的实际方向。也就是说原本独立的两个旋转操作绕 Z 轴的航向角和绕 X 轴的滚转角现在只能产生一个维度的效果。
定轴旋转Extrinsic Rotation便于编程 在定轴旋转中每次旋转都是相对于固定的外部坐标系进行的即使物体已经发生了旋转。这意味着不管物体如何旋转旋转轴始终保持在空间中的固定位置。定轴旋转通常用于描述物体相对于固定参考系的变化比如地球坐标系。 没有万向锁现象
奇异性 奇异性Singularity在数学和物理学中通常指的是一个点或状态在那里某些物理量或数学函数的行为变得不寻常通常表现为无穷大、不连续或无法定义。 比如在旋转向量中存在0到2π的跳变可以给定义区间打补丁等。 欧拉角中也存在0到2π的跳变。另外有万向锁理解万向锁具体可以看下面的视频无伤理解欧拉角中的“万向死锁”现象https://www.bilibili.com/video/av771397545/?vd_source033a75d9cfb0b0df5347cafb8b033109 根据上面的视频我们可知变换是由初始状态进行的即使在调整y90°后继续调整x当整体过程再次从初始状态开始时会在第一步直接调整x的值x10y90z0现在继续x旋转1度从初始开始x11y90z0由于y调整为90°与初始的x“重合”所以视觉上是调整了z轴而实际调整的仅为初始的x轴。 整体的一个现象可以抽象为一个规律前面的轴运动带动后面的轴运动而后面的轴无法带动前面的轴运动。每次旋转必须在初始状态下旋转而不是叠加旋转
例子如图我第一次以103050变换飞机第二次以113050变换。问能否在第一次后直接变换100 答不行因为轴会被带动后面转的x并不是之前最初的x轴。 无论r是多少只要p转了90度就一定会使x轴和z轴重合出现死锁现象。 定轴不存在死锁现象但是定轴的欧拉角也不能增量式旋转 虽然旋转向量和欧拉角这些量可以表示某个位姿但是在全局范围或者特定情况下它们的表示可能会遇到不连续性的问题难以描述连续变化的运动不能增量式调整每次都需要从头开始。 在死锁状态下物体仍然可以旋转但这并不意味着在数学模型中旋转的描述是连续的。在死锁状态下如果绕Z轴和X轴的旋转角连续变化物体的实际旋转状态可能会经历不连续的变化。这是因为在死锁点附近小的欧拉角变化可能会导致物体旋转状态的大变化或者相反大的欧拉角变化只引起微小的旋转状态变化。这种不连续性是由于欧拉角参数化与旋转状态空间之间映射的非线性造成的。导致无法进行插值或求导等操作。
四、四元数 四元数类似于复数由爱尔兰数学家威廉·哈密顿于1843年提出是复数的拓展用于表示三维空间中的旋转。四元数由一个实部和三个虚部组成通常表示为 qabicjdk其中 a,b,c,d实数而 i,j,k 是四元数的虚数单位它们满足特定的乘法规则。
欧拉公式二维 二维空间中的旋转可以用一个单位复数表示 欧拉公式证明 四元数的基本形式 四元数可以被视为扩展复数的概念但它们具有四个分量而不是两个。一个四元数 q 通常写作 其中 a,b,c,d 是实数而 i,j,k 是四元数单位它们满足以下性质 这意味着 ijk,jki,kij同时 ji−k,kj−i,ik−j。这些规则定义了四元数乘法的非交换性和非分配性。
可视化四元数务必看完视频超级好
四元数的在三维中的可视化与三维在二维空间的可视化纸片人十分相近。圆-球线-面。
四元数的可视化https://www.bilibili.com/video/BV1SW411y7W1/?spm_id_from333.337.search-card.all.clickvd_source033a75d9cfb0b0df5347cafb8b033109
四元数的表示
在实际应用中四元数通常被写作一个实部和一个虚部的组合其中虚部由三个独立的分量组成 这里 w 称为标量部分而 x,y,z 组成了矢量部分。四元数也可以写作一个四维向量的形式(s是实部v就是虚部) 单位四元数与旋转 四元数的一个重要应用是在三维空间中表示旋转。一个单位四元数其长度等于1可以用来表示一个旋转 其中 θ 是旋转角度而 n 是一个单位向量表示旋转轴的方向。单位四元数的一个优点是它们避免了欧拉角在某些特定旋转角度下出现的“万向节锁”gimbal lock问题。
四元数的运算
加减法 乘法 上面这个图符合”右手定则“也就是类似于”叉积“ijk分别为三个轴的基底。
四元数没有交换律
数乘与点乘 取模以形式 我们用来描述姿态的四元数是单位四元数如果我们在优化运算中如求导等改变了四元数的模长记得将其变为单位长度。
共轭 逆 矩阵相乘 四元数的使用 1.三维扩展到四维实部为零 2. 如何用一个四元数旋转一个空间点 设点p经过一次以q表示的旋转后得到了p将后三维去除就是三维坐标
(1).p的坐标用四元数表示虚四元数 (2).旋转之后的关系为 为什么不直接像二维中一样是qp呢 通过视频下面的三维转动的视频和交互网站可以知道当只进行一个q的时候会导致点 p 不仅旋转还会平移这是因为四元数乘法本质上是非交换的并且不保证旋转中心在原点。具体过程如下图所示 0°的时候 q为45° 这一步将点 p 从其原始位置旋转到一个新的位置。但这个新的位置并不一定是在我们想要的旋转轴周围。 q逆为45° 这一步再将点从第一步旋转后的位置“反转”回旋转轴周围。这确保了点最终在正确的旋转轴周围完成旋转。 我们知道当q1为45度时整体不仅仅发生了旋转发生了平移。通过这样先旋转然后“反转”旋转的过程实际上我们是在执行一个复合操作它确保了点围绕原点进行旋转而不会产生任何额外的平移。这是因为是 q 的逆操作它会抵消掉 q 引入的任何非旋转效果。平移的角度变化其实是2倍的关系后面有讲。此处只需要理解旋转的公式即可。后面推荐的视频里面会将的很清楚。 3.虽然数学实质如上所述但是Eigen已经将四元数的旋转运算重载平时只需要这么算就行了也就是三维直接乘四维也可以仅在eigen中。 注意旋转时候左乘和右乘是有区别的绝大多数人习惯左乘绕世界系旋转用左乘绕本体轴旋转用右乘
四元数和角轴的关系
角轴到四元数 四元数到角轴 首先必须了解四元数是如何使物体旋转的即为什么会有半角的出现 视频推荐理解四元数如何使三维转动的有助于理解
看下面的视频之前务必看完上面的四元数可视化的视频
四元数和三维转动可互动的探索式视频https://www.bilibili.com/video/BV1Lt411U7og/?spm_id_fromautoNextvd_source033a75d9cfb0b0df5347cafb8b033109 视频中的交互网站如下英文无翻译打开后不要翻译为中文
https://eater.net/quaternionshttps://eater.net/quaternions 因为如下图f(p)为左乘后右乘逆两次都是绕同一方向旋转了。按下图所示旋转的效果不是90°而是180°可以在上面的网址的最后一个交互视频里查看英文无汉译这也解释了为什么是半角的原因。 截图例子如下注意蓝色红框点随角度的变化 0度时 q1转为90°时 q2旋转到90° 通过上图我们发现当两个q都设定为90°时球体的旋转为180°。 视频中还操作了一些别的旋转轴和角度可以自己去试着玩一下有助于理解。 旋转矩阵R的推导过程
为了更加清晰地展示推导过程我们将使用以下符号
q w xi yj zk (四元数)q⁻¹ w - xi - yj - zk (q 的共轭假设 q 为单位四元数)p [0, x, y, z] (纯虚四元数代表三维向量)p [0, x, y, z] (旋转后的向量也用纯虚四元数表示)
目标 计算 p qpq⁻¹并将其结果表示成矩阵形式从而得到旋转矩阵 R。
步骤
1.计算 qp:
qp (w xi yj zk)(0 xi yj zk) -x² - y² - z² wxi wyj wzk xwi - yk zj ywk xk - zi zwi - xj - yk (-x² - y² - z²) (2wx)i (2wy)j (2wz)k (yz - xy)i (zx - yz)j (xy - zx)k [-x² - y² - z², 2wx yz - xy, 2wy zx - yz, 2wz xy - zx]
2.计算 (qp)q⁻¹:
(qp)q⁻¹ [-x² - y² - z², 2wx yz - xy, 2wy zx - yz, 2wz xy - zx] * (w - xi - yj - zk) 为了简化计算我们将 (qp)q⁻¹ 分成四部分进行计算 标量部分: (-x² - y² - z²)w - (2wx yz - xy)(-x) - (2wy zx - yz)(-y) - (2wz xy - zx)(-z)-wx² - wy² - wz² 2wx² xyz - x²y 2wy² yzx - y²z 2wz² xyz - xz²0 (因为 p 是纯虚四元数)i 部分: (-x² - y² - z²)(-x) (2wx yz - xy)w (2wy zx - yz)(-z) (2wz xy - zx)(y)x³ xy² xz² 2wx² wyz - wxy - 2wyz - z²x yz² 2wyz xy² - xyzx³ 2xy² xz² 2wx² - wxy - z²x yz² xy² - xyz(1 - 2y² - 2z²)x (2xy - 2zw)y (2xz 2yw)z xj 部分: (-x² - y² - z²)(-y) (2wx yz - xy)(z) (2wy zx - yz)w (2wz xy - zx)(-x)y³ yx² yz² 2wxz yz² - xyz 2wy² wzx - wyz - 2wxz - x²y xyzy³ yx² 2yz² 2wy² wzx - wyz - x²y(2xy 2zw)x (1 - 2x² - 2z²)y (2yz - 2xw)z y k 部分: (-x² - y² - z²)(-z) (2wx yz - xy)(-y) (2wy zx - yz)(x) (2wz xy - zx)wz³ zx² zy² - 2wxy - y²z xyz 2wxy x²z - xyz 2wz² wxy - wzxz³ zx² zy² - y²z x²z 2wz² wxy - wzx(2xz - 2yw)x (2yz 2xw)y (1 - 2x² - 2y²)z z3.构建旋转矩阵:
将上面计算得到的 x, y, z 写成矩阵形式
[ x ] [ 1 - 2y² - 2z² 2xy - 2zw 2xz 2yw ] [ x ]
[ y ] [ 2xy 2zw 1 - 2x² - 2z² 2yz - 2xw ] * [ y ]
[ z ] [ 2xz - 2yw 2yz 2xw 1 - 2x² - 2y² ] [ z ]因此旋转矩阵 R 为
[ 1 - 2y² - 2z² 2xy - 2zw 2xz 2yw ]
[ 2xy 2zw 1 - 2x² - 2z² 2yz - 2xw ]
[ 2xz - 2yw 2yz 2xw 1 - 2x² - 2y² ] 五、实践部分-Eigen的几何模块
注意如何区分类 若在代码里使用了 using namespace Eigen; 之后Eigen 命名空间中的所有类和函数都被引入到当前的命名空间中这意味着你可以直接使用这些类和函数而不需要在它们前面加上 Eigen:: 前缀。 要判断一个单词是否代表一个类可以根据以下几点来判断 是否有变量的声明查看该单词是如何被使用的。如果是作为一个类型来声明变量那么它很可能是一个类。例如Matrix3d A; 这里 Matrix3d 被用来声明一个变量 A所以 Matrix3d 很可能是一个类。 构造函数调用如果一个单词后面跟着括号并且可能包含参数那么它可能是一个类的构造函数。例如AngleAxis aa(angle, axis); 这里 AngleAxis 后面跟着括号和参数表明它可能是一个类。 成员函数调用如果一个单词后面跟着 . 操作符然后是函数调用那么这个单词可能是一个类。例如aa.matrix(); 这里 aa 后面的 . 表明 aa 可能是一个类的实例matrix() 是它的成员函数。
——————————————————————————————
1.创建一个旋转矩阵
Eigen Matrix3d ratation_matrix Eigen::Matrix3d::Identity();
//旋转矩阵直接用一个3*3的单位矩阵表示 轴角表示法用一个单位向量 n和一个角度 θ 来表示旋转。单位向量 n^表示旋转轴角度 θ 表示绕该轴旋转的角度。
2.使用AngleAxis类来表示这种旋转
// 设置旋转为绕Z轴旋转45度
Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1)); 3.将AngleAxis转换为矩阵
coutrotation matrix \nrotation_vector.matrix() endl;
rotation_matrix rotation_vector.toRotationMatrix(); matrix()成员函数是 Eigen::AngleAxis 类提供的一个方法它的作用是计算并返回这个旋转向量对应的旋转矩阵。当你写 rotation_vector.matrix() 时你就是在告诉程序去执行 rotation_vector 这个对象的 matrix() 方法并获取它返回的结果。这个结果是一个 Eigen::Matrix3d 类型的对象即一个3x3的矩阵它具体描述了由 rotation_vector 所定义的三维空间旋转。 matrix()函数其实就是实现罗德里格斯公式罗德里格斯公式是旋转向量和旋转矩阵之间的转换公式。它允许我们从一个旋转向量角度和轴计算出对应的旋转矩阵从而可以在三维空间中应用这个旋转矩阵来旋转任意向量。 也可以直接赋值利用toRotationMatrix.成员函数赋给一个旋转矩阵。
PS调用 cout.precision(val) 时你设置了 cout 在后续输出中使用的浮点数精度。这里的 val 是一个小数点后要保留的位数。
4.旋转某点时的乘法运算
// 用 AngleAxis 可以进行坐标变换Eigen::Vector3d v ( 1,0,0 );Eigen::Vector3d v_rotated rotation_vector * v;cout(1,0,0) after rotation v_rotated.transpose()endl;用 rotation_vector旋转v。输出的结果与下图中的旋转矩阵方法得到的结果一致。 // 或者用旋转矩阵v_rotated rotation_matrix * v;cout(1,0,0) after rotation v_rotated.transpose()endl;
编译后运行结果如图 注意rotation_vector 实际上并不是一个矩阵至少不是直接以矩阵形式存储的。在Eigen库中AngleAxis类封装了轴角表示法它由一个旋转轴一个单位向量和一个旋转角度组成。尽管AngleAxis对象并不直接存储为矩阵但它可以隐式地转换为一个旋转矩阵这是因为AngleAxis类重载了必要的运算符使其可以像矩阵那样与其他几何对象如向量进行操作。
5.从旋转矩阵获取欧拉角eulerAngles
// 欧拉角: 可以将旋转矩阵直接转换成欧拉角Eigen::Vector3d euler_angles rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序即roll pitch yaw顺序coutyaw pitch roll euler_angles.transpose()endl;输出结果yaw-pitch-rollz-y-x 也就是说z对应的是0.785 与上面我们的绕z轴旋转45度正好对应可以作为一个验证。
6.使用Isometry表示欧氏变换
// 欧氏变换矩阵使用 Eigen::IsometryEigen::Isometry3d TEigen::Isometry3d::Identity(); // 虽然称为3d实质上是44的矩阵T.rotate ( rotation_vector ); // 按照rotation_vector进行旋转T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) ); // 把平移向量设成(1,3,4)
//T(0,3)1;T(1,3)3;T(2,3)4;与上一行表达是一样的。cout Transform matrix \n T.matrix() endl; Isometry3d是一个4x4的矩阵用于表示三维空间中的欧式变换。这里初始化了一个单位变换然后设置了旋转和平移。一般形式如下 如下图所示还有很多种变形比如2d代表的就是二维空间的且精度为double。 其中因为上文的rotation_vector 是一个绕Z轴旋转45度的 AngleAxis 对象。所以 T.rotate ( rotation_vector ); 就是告诉 T 要绕Z轴旋转45度输出的也就是所谓的“旋转矩阵R” 。 Eigen::Vector3d ( 1,3,4 )表示在变换中所有的点都将在x轴方向移动1个单位在y轴方向移动3个单位在z轴方向移动4个单位。T(0,3)1; T(1,3)3; T(2,3)4;它们分别对应于矩阵的第1行第4列、第2行第4列、第3行第4列的值。也就是上面对应的t的部分。 T.matrix() 方法返回一个4x4的矩阵它完全表示了 T 中的刚体变换。这个矩阵包含了旋转和平移的信息可以用于将一个点或向量从一个坐标系转换到另一个坐标系。当你调用 T.matrix() 并打印输出时你会看到一个4x4的矩阵其中3x3的左上角部分是旋转矩阵右边一列是平移向量最后一行是齐次坐标系统中的标准化因子就是上文的标准形式。
输出结果 这时候这个T就是一个变换矩阵这个变换矩阵也可以直接去乘一个向量即使它是一个4x4的矩阵但是它是有定义的拿这个矩阵乘一个向量相当于用这个矩阵去变换这个向量。
7.使用变换矩阵进行坐标变换
// 用变换矩阵进行坐标变换Eigen::Vector3d v_transformed T*v; // 相当于R*vtcoutv tranformed v_transformed.transpose()endl; 通常一个4x4矩阵和一个3维向量直接相乘在数学上是没有定义的但是通过运算符重载Eigen库允许这种操作从而简化了几何变换的编程。T是一个Isometry3d对象它实际上是一个4x4的矩阵而v是一个3维向量。在数学上为了能够将一个向量与一个4x4矩阵相乘向量需要被扩展为一个齐次坐标也就是说向量后面需要附加一个1变成一个4维向量。Eigen库内部自动处理了这一点它将3维向量v扩展为齐次坐标形式然后与T相乘最后再丢弃结果向量的第四个元素即齐次坐标中的1只保留前三个元素得到变换后的3维向量v_transformed。 结果溯源相当于Rvt 8.使用四元数表示旋转Quaterniond // 四元数// 可以直接把AngleAxis赋值给四元数反之亦然Eigen::Quaterniond q Eigen::Quaterniond ( rotation_vector );coutquaternion \nq.coeffs() endl; // 请注意coeffs的顺序是(x,y,z,w),w为实部前三者为虚部// 也可以把旋转矩阵赋给它q Eigen::Quaterniond ( rotation_matrix );coutquaternion \nq.coeffs() endl; Eigen::Quaterniond是一个四元数类它使用双精度浮点数来存储四元数的四个系数。这里rotation_vector是一个AngleAxis对象表示一个特定的旋转。构造函数会将AngleAxis转换成等价的四元数表示。 coeffs()是Eigen::Quaterniond类的一个成员函数它返回一个固定大小的向量Eigen::Vector4d这个向量包含了四元数的四个系数。在Eigen中四元数的系数顺序是(x,y,z,w)(x,y,z,w)即虚部在前实部在后 。 对比下面四元数旋转的形式 注意第一张图的顺序为X,Y,Z,W。所以注意对应。 注意旋转矩阵也可以有相同的结果是因为这是同一个旋转。因为旋转矩阵也是通过前面的轴角生成的所以本质来讲这是一回事。
9.使用四元数旋转向量
// 使用四元数旋转一个向量使用重载的乘法即可v_rotated q*v; // 注意数学上是qvq^{-1}cout(1,0,0) after rotation v_rotated.transpose()endl;这里使用四元数来旋转向量v。在数学上正确的旋转应该是但由于Eigen内部的优化重载直接使用乘法就可以得到正确的结果。其实是先把v向量的坐标成为四元数的虚部而实部被设定为0后按公式进行计算。
