参考引用1、参考引用2、参考引用3、参考引用4、参考引用5、参考引用6
1. 旋转矩阵
1.1 点、向量和坐标系
-
点：点就是空间中的基本元素，没有长度，没有体积
-
向量：把两个点连接起来，就构成了向量，向量可以看成从某点指向另一点的一个箭头
- 只有当我们指定这个三维空间中的某个坐标系时，才可以谈论该向量在此坐标系下的坐标
- 默认向量就是列向量
-
坐标系：三根不共面的轴，坐标系的三根坐标轴的方向向量就是基，坐标系能用它的基来表示
-
标准正交基：两两正交、单位长度（基就是张成这个空间的一组线性无关的向量）
-
任意向量在基下的坐标（坐标 + 坐标系可以表示向量）
-
机器人中有各种各样的坐标系
- 世界系 World、机体系 Body、传感器参考系 Sensor
-
-
向量点乘和叉乘
-
点乘（内积）：结果是一个数
-
叉乘（外积）：结果是一个向量，方向垂直于这两个向量，大小为 |a||b|sin<a,b>
将 ^ 记作反对称符号，a^ 则为反对称矩阵，该符号是一个一一映射，任意向量都对应着唯一的一个反对称矩阵
-
1.2 位姿变换
-
两个坐标系之间的运动由一个旋转和一个平移组成，这种运动称为刚体运动
-
旋转变换 SO(3)
-
某个单位正交基（e1,e2,e3）经过一次旋转变成了（e1’,e2’,e3’）
-
左右同乘 [e1,e2,e3] 的转置，因为 [e1,e2,e3] 是正交阵，所以 A’A = E
正交矩阵的转置是正交矩阵
正交矩阵相乘还是正交矩阵
所以 R 是正交矩阵且行列式为1，称为旋转矩阵 -
将 n 维旋转矩阵的集合定义为
SO(n)是特殊正交群，SO(3)就是指三维空间的旋转
-
由于旋转矩阵为正交矩阵，它的逆（即转置）描述了一个相反的旋转，显然 R T R^T RT刻画了一个相反的旋转
出于计算复杂性考虑，常用R转置代替它的逆
-
-
欧式变换 SE(3)
-
在 SO(3) 基础上再加一个平移就是 SE(3)
- R12 表示把姿态由 2 系对齐到 1 系的旋转矩阵
- t12 表示把位置由 2 系对齐到 1 系的平移量
- 从右向左读，如： a 1 = R 12 R 23 R 34 a 4 a_1 = R_{12}R_{23}R_{34}a_4 a1=R12R23R34a4
实际对应的是坐标系 1 姿态到坐标系 2 姿态的旋转，我们之所以说 R12 角标从右往左读，那是对于坐标变化来说的，对坐标轴的姿态变换要从左往右读才是真实旋转方向
-
假设进行了两次变换： R 1 R_1 R1， t 1 t_1 t1 和 R 2 R_2 R2， t 2 t_2 t2
-
为保证简洁引入了齐次坐标和变换矩阵，在一个三维向量的末尾加 1 将其变成了四维向量，称为齐次坐标，T 称为变换矩阵
-
两次变换的叠加形式
-
-
SE(3)特殊欧式群
-
求解该矩阵的逆表示一个反向的变换
因为是先旋转后平移，所以平移量会受到旋转量的影响
-
2. 描述刚体旋转（姿态）
2.1 旋转矩阵 R
- SO(3)：三维空间的旋转
2.2 旋转向量/角轴
-
旋转矩阵描述的缺点
- S0(3) 旋转矩阵有 9 个量，但一次旋转只有 3 个自由度，故这种表达方式是冗余的；同理变换矩阵用 16 个量表达了 6 自由度的变换
- 旋转矩阵必须是正交矩阵且行列式为1，变换矩阵亦如此，当想估计或优化时这些约束条件让求解变得更困难
-
如何紧凑的描述旋转和平移？
- 事实上，任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量称为 旋转向量(或轴角/角轴，Axis-Angle) 只需一个三维向量即可描述旋转
- 可以用一个向量 θn 来表示这个旋转，其中 n 是转轴方向的单位向量，θ 是转过的角度
- 同样对于变换矩阵，我们使用一个旋转向量和一个平移向量即可表达一次变换
-
从旋转向量到旋转矩阵的转换：罗德里格斯公式
-
从旋转矩阵到旋转向量的转换
符号 ^ 是向量到反对称矩阵的转换符，对于转角 θ，取两边的迹（矩阵对角元素之和）
关于转轴 n，旋转轴上的向量在旋转后不发生改变： Rn = n
因此，转轴 n 是矩阵 R 特征值1对应的特征向量，求解此方程并归一化，就得到了旋转轴
2.3 欧拉角
-
无论是旋转矩阵还是旋转向量，它们虽然能描述旋转，但是非常不直观，欧拉角则提供了一种非常直观的方式来描述旋转：它使用了 3 个分离的转角，把一个旋转分解成 3 次绕不同轴的转
-
欧拉角通常用 “偏航-俯仰-滚转”(yaw-pitch-roll) 3 个角度来描述一个旋转。它等价于ZYX轴的旋转，因此就以 ZYX 为例。假设一个刚体的前方(朝向我们的方向)为 X 轴，右侧为 Y 轴，上方为 Z 轴，如下图所示。那么，ZYX 转角相当于把任意旋转分解成以下 3 个轴上的转角
- 绕物体的 Z 轴旋转，得到偏航角 yaw
- 绕旋转之后的 Y 轴旋转，得到俯仰角 pitch
- 绕旋转之后的 X 轴旋转，得到滚转角 roll
-
动轴定轴问题
- 绕着旋转之后的轴旋转（动轴），方便交流但有万向锁现象
- 绕着旋转前的轴来旋转（定轴），方便编程且无万向锁现象
-
奇异性问题（万向锁）
- 欧拉角一个最大缺点便是万向锁问题：在俯仰角为 ±90° 时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度(由 3 次旋转变成了 2 次旋转 )，这被称为奇异性问题
- 由于这个问题，欧拉角不适用于插值和迭代，往往只用于人机交互中
- 在某些主体主要为 2D 运动的场合 (例如扫地机、自动驾驶车辆)，我们也可以把旋转分解成三个欧拉角，然后把其中一个 (例如偏航角 yaw ) 拿出来作为定位信息输出
- 旋转向量也存在 0~2π 的跳变的奇异性问题
- 并不是说旋转向量、欧拉角这些量没有办法表示具体某一个位姿，而是它们的这种描述形式，很难用连续变化的参数描述连续变化的运动，这导致无法做插值、求导和迭代这些涉及范围概念的操作
-
欧拉角范围
- 车体坐标系（x-前，y-左，z-上），那么 roll 和 pitch 应该定义在（-90° ~ +90°），yaw 应该定义在（-180° ~ +180°）。
- 飞机坐标系，那么 roll、pitch 和 yaw 都应该定义在（-180° ~ +180°）。
- Eigen 中的默认范围 roll、pitch 和 yaw 都是（-180° ~ +180°）。
2.4 四元数
-
旋转矩阵用 9 个量描述 3 自由度的旋转，具有冗余性；欧拉角和旋转向量是紧凑的，但具有奇异性。事实上，我们找不到不带奇异性的三维向量描述方式
-
复数的乘法则表示复平面上的旋转：例如，乘上复数 i 相当于逆时针把一个复向量旋转 90
类似地，在表达三维空间旋转时，也有一种类似于复数的代数：四元数 (Quaternion)，它是 Hamilton 找到的一种扩展的复数，它既是紧凑的，也没有奇异性
-
2D 空间中的旋转可以用一个单位复数来表示
-
3D 空间的旋转也可以用复数表示（只不过虚部有三个维度组成，i j k类似于四元数的旋转操作）
-
也可用一个标量和一个向量来表示四元数
其中 s 称为四元数的实部，而 v 称为虚部，如果一个四元数虚部为0，则称为实四元数；若实部为0，则称为虚四元数
-
-
四元数基本运算
-
四元数加减
-
四元数乘法（通常没有交换律）
-
四元数模长
用来描述姿态的四元数是单位四元数，如果在运算中改变了四元数的模长，记得将其变为单位长度
-
四元数的共轭
-
四元数的逆
-
四元数数乘
-
-
用四元数表示旋转
-
假设空间中一个三维点 p = [ x , y , z ] ∈ R 3 p = [x,y,z] ∈ R^3 p=[x,y,z]∈R3，以及一个由单位四元数 q 指定的旋转，三维点 p 经过旋转后变为 p’，如果用旋转矩阵描述则有：p’ = Rp，如果用四元数描述呢？
-
首先，把三维空间点用一个虚四元数来描述
-
相当于把四元数的 3 个虚部与空间中的 3 个轴相对应。那么，旋转后的点 p’ 可表示为这样的乘积
- 这里的乘法均为四元数乘法，结果也是四元数，最后把 p 的虚部取出，即得旋转之后点的坐标，并且计算结果为纯虚四元数
- Eigen 已经将四元数的旋转运算重载，平时只需要计算：p’=q*p
-
-
-
四元数到其他旋转表示的转换
-
四元数乘法也可写成一种矩阵的乘法，设 q = [ s , v ] T q = [s,v]^T q=[s,v]T，那么定义如下符号
-
那么四元数乘法可以写成矩阵形式
-
四元数到旋转矩阵的变换
-
四元数到旋转向量的转换
-
3. Eigen 基本使用
-
Eigen 是一个 C++ 开源线性代数库，提供了快速的有关矩阵的线性代数运算等
-
与其他库相比，Eigen 是一个纯用头文件搭建起来的库，在使用时只需引入 Eigen 的头文件即可，不需要链接库文件
- #include <Eigen/Core> ：向量、矩阵、矩阵基本操作、矩阵分解
- #include <Eigen/Dense> ：求特征值、特征向量、稠密矩阵的一些操作
- #include <Eigen/Geometry> ：位姿变换相关的数据类型和操作
// Eigen基本操作 #include <iostream> #include <eigen3/Eigen/Core> #include <eigen3/Eigen/Dense> #define MATRIX_SIZE 50 int main(int argc, char *argv[]) { // Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列 // 声明一个2*3的float矩阵 Eigen::Matrix<float, 2 , 3> matrix_23; // 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix // 例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，即三维向量 Eigen::Vector3d v_3d; Eigen::Matrix<float, 3, 1> vd_3d; // 这是一样的 // Matrix3d 实质上是 Eigen::Matrix<double, 3, 3> Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //初始化为零 // 如果不确定矩阵大小，可以使用动态大小的矩阵 Eigen::Matrix<double, Dynamic, Dynamic> matrix_dynamic; Eigen::MatrixXd matrix_x; // 更简单的动态大小的矩阵 // 下面是对Eigen阵的操作 // 矩阵的行数与列数、访问矩阵元素 matrix_23.rows(); matrix_23.cols(); matrix_23(i, j); // 块操作：块的大小 (p,q), 块的索引 (i,j) matrix.block(i,j,p,q); // 动态大小块表达式 matrix.block<p,q>(i,j); // 固定大小块表达式 // 单位矩阵生成 MatrixXd::Identity(rows, cols); matrix_33.setIdentity(3, 3); // 输入数据（初始化） matrix_23 << 1, 2, 3, 4, 5, 6; std::cout << matrix_23 << std::endl; // 用()访问矩阵中的元素 for (int i = 0; i < 2; i++) { for (int j = 0; j < 3; j++) { std::cout << matrix_23(i, j) << "\t"; } std::cout << std::endl; } // 矩阵和向量相乘（实际上仍是矩阵和矩阵） v_3d << 3, 2, 1; vd_3d << 4, 5, 6; // 在Eigen里不能混合两种不同类型的矩阵，像这样是错的 // Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d; // 应该显式转换 matrix_23.cast<double>() Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d; std::cout << result.transpose() << std::endl; Matrix<float, 2, 1> result2 = matrix_23 * vd_3d; std::cout << result2.transpose() << std::endl; // 同样不能搞错矩阵的维度，像这样是错的 // Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d; // 一些矩阵运算 matrix_33 = Eigen::Matrix3d::Random(); // 随机数矩阵 std::cout << matrix_33 << std::endl; std::cout << matrix_33.transpose() << std::endl; // 转置 std::cout << matrix_33.adjoint() << std::endl; // 共轭转置 std::cout << matrix_33.sum() << std::endl; // 各元素和 std::cout << matrix_33.trace() << std::endl; // 迹 std::cout << matrix_33.inverse() << std::endl; // 逆 std::cout << matrix_33.determinant() << std::endl; // 行列式 std::cout << v_3d.norm() << std::endl; // 范数 std::cout << v_3d.squaredNorm() << std::endl; // 平方和 std::cout << v_3d.dot(v_3d) << std::endl; // 点积 std::cout << v_3d.cross(v_3d) << std::endl; // 交叉积（需要头文件 #include <Eigen/Geometry>） // 特征值 // 实对称矩阵可以保证对角化成功 // 计算自伴随矩阵的特征值和特征向量 // 自伴随矩阵(Hermitian matrices)，是一个具有复数的平方矩阵，其特征是等于其共轭转置 Eigen::SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33); std::cout << eigen_solver.eigenvalues() << std::endl; // 计算特征值 std::cout << eigen_solver.eigenvectors() << std::endl; // 计算特征值对应的特征向量 // 解方程 // 我们求解 matrix_NN * x = v_Nd 这个方程(Ax = b) // N的大小在前边的宏里定义，它由随机数生成 // 直接求逆自然是最直接的，但是求逆运算量大 Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE); matrix_NN = matrix_NN * matrix_NN.transpose(); // 保证半正定 Eigen::Matrix<double, MATRIX_SIZE, 1> v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE, 1); // 直接求逆 clock_t time_stt = clock(); // 计时 Eigen::Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd; std::cout << "time of normal inverse is " << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << std::endl; std::cout << "x = " << x.transpose() << std::endl; // LU分解求逆 time_stt = clock(); x = matrix_NN.lu().solve(v_Nd); std::cout << "time of lu decomposition is " << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << std::endl; std::cout << "x = " << x.transpose() << std::endl; // QR分解求逆，速度会快很多 time_stt = clock(); x = matrix_NN.colPivHouseholderQr().solve(v_Nd); std::cout << "time of Qr decomposition is " << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << std::endl; std::cout << "x = " << x.transpose() << std::endl; // Cholesky分解求逆，(前提是正定矩阵) time_stt = clock(); x = matrix_NN.llt().solve(v_Nd); std::cout << "time of llt decomposition is " << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << std::endl; std::cout << "x = " << x.transpose() << std::endl; // LDLT分解求逆，(前提是正定矩阵) time_stt = clock(); x = matrix_NN.ldlt().solve(v_Nd); std::cout << "time of ldlt decomposition is " << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << std::endl; std::cout << "x = " << x.transpose() << std::endl; // SVD分解 /* int main(int argc, char *argv[]) { Eigen::MatrixXf m = Eigen::MatrixXf::Zero(3, 2); m << 0,1,1,1,1,0; std::cout << "Here is the matrix m:" << std::endl << m << std::endl; Eigen::JacobiSVD<MatrixXf> svd(m, ComputeFullU | ComputeFullV); std::cout << "Its singular values are:" << std::endl << svd.singularValues() << std::endl; std::cout << "Its left singular vectors are the columns of the thin U matrix:" << std::endl << std::endl << svd.matrixU() << std::endl; std::cout << "Its right singular vectors are the columns of the thin V matrix:" << std::endl << std::endl << svd.matrixV() << std::endl; system("pause"); return 0; } */ return 0; }// Eigen几何操作 #include <iostream> #include <cmath> #include <eigen3/Eigen/Core> #include <Eigen/Geometry> #include <eigen3/Eigen/Dense> int main(int argc, char *argv[]) { // Eigen/Geometry 模块提供了各种旋转和平移的表示 // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f // 使用旋转矩阵的函数来初始化旋转矩阵 Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity(); // 旋转向量使用 AngleAxis, 它底层不直接是 Matrix，但运算可以当作矩阵（因为重载了运算符） Eigen::AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1)); // 沿 Z 轴旋转 45 度 std::cout.precision(3); // 设置输出值的有效位数为 3 std::cout << rotation_vector.matrix() << std::endl; // 用 matrix() 转换成矩阵 // 也可以直接赋值，旋转向量转换成矩阵 rotation_matrix = rotation_vector.toRotationMatrix(); // 用 AngleAxis 可以进行坐标变换 Vector3d v(1, 0, 0); Vector3d v_rotated = rotation_vector * v; std::cout << v_rotated.transpose() << std::endl; // 或者用旋转矩阵 v_rotated = rotation_matrix * v; std::cout << v_rotated.transpose() << std::endl; // 欧拉角：可以将旋转矩阵直接转换成欧拉角（旋转向量同理） Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX 顺序，即 yaw-pitch-roll 顺序 std::cout << euler_angles.transpose() << std::endl; // 欧氏变换矩阵使用 Eigen::Isometry Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); // 虽然称为3d，实质上是4＊4的矩阵 T.rotate(rotation_vector); // 按照 rotation_vector 进行旋转 T.pretranslate(Vector3d(1, 3, 4)); // 把平移向量设成(1,3,4) std::cout << T.matrix() << std::endl; // 用变换矩阵进行坐标变换 Eigen::Vector3d v_transformed = T * v; // 相当于R*v+t std::cout << v_transformed.transpose() << std::endl; // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可 // 四元数 // 可以直接把 AngleAxis 赋值给四元数，反之亦然 // 运用四元数来描述旋转，构造的时候按照（w,x,y,z）顺序，但在实际内存中，四元数的顺序却是x,y,z,w Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector); // 显示四元数数值：q.coeffs()，注意coeffs()的顺序是(x,y,z,w)，w 为实部，前三者为虚部 std::cout << q.coeffs().transpose() << std::endl; // 也可以把旋转矩阵赋给它 q = Eigen::Quaterniond(rotation_matrix); std::cout << q.coeffs().transpose() << std::endl; // 使用四元数旋转一个向量，使用重载的乘法即可 v_rotated = q * v; // 注意数学上是qvq^{-1} std::cout << v_rotated.transpose() << std::endl; // 用常规向量乘法表示，则应该如下计算 std::cout << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << std::endl; return 0; }
4. 坐标系变换
-
常用函数
- 旋转矩阵（3X3）:Eigen::Matrix3d
- 旋转向量（3X1）:Eigen::AngleAxisd
- 四元数（4X1）:Eigen::Quaterniond
- 欧拉角/平移向量（3X1）:Eigen::Vector3d
- （欧氏）变换矩阵（4X4）:Eigen::Isometry3d
-
基于 eigen 实现旋转向量、旋转矩阵、欧拉角(RPY)和四元数之间相互变换
#include <iostream> #include <Eigen/Core> #include <Eigen/Geometry> using namespace std; #define PI 3.1415926535897932346 int main(int argc, char **argv) { /**** 1. 旋转向量 ****/ cout << endl << "********** AngleAxis **********" << endl; //1.0 初始化旋转向量,沿Z轴旋转45度的旋转向量 Eigen::AngleAxisd rotation_vector1 (M_PI/4, Eigen::Vector3d(0, 0, 1)); //1.1 旋转向量转换为旋转矩阵 //旋转向量用matrix()转换成旋转矩阵 Eigen::Matrix3d rotation_matrix1 = Eigen::Matrix3d::Identity(); //单位矩阵初始化 rotation_matrix1 = rotation_vector1.matrix(); cout << "rotation matrix1 =\n" << rotation_matrix1 << endl; //或者由罗德里格公式进行转换 rotation_matrix1 = rotation_vector1.toRotationMatrix(); cout << "rotation matrix1 =\n" << rotation_matrix1 << endl; /*1.2 旋转向量转换为欧拉角*/ //将旋转向量转换为旋转矩阵,再由旋转矩阵转换为欧拉角,详见旋转矩阵转换为欧拉角 Eigen::Vector3d eulerAngle1 = rotation_vector1.matrix().eulerAngles(2,1,0); cout << "eulerAngle1, z y x: " << eulerAngle1 << endl; /*1.3 旋转向量转四元数*/ Eigen::Quaterniond quaternion1(rotation_vector1); //或者 Eigen::Quaterniond quaternion1_1; quaternion1_1 = rotation_vector1; cout << "quaternion1 x: " << quaternion1.x() << endl; cout << "quaternion1 y: " << quaternion1.y() << endl; cout << "quaternion1 z: " << quaternion1.z() << endl; cout << "quaternion1 w: " << quaternion1.w() << endl; cout << "quaternion1_1 x: " << quaternion1_1.x() << endl; cout << "quaternion1_1 y: " << quaternion1_1.y() << endl; cout << "quaternion1_1 z: " << quaternion1_1.z() << endl; cout << "quaternion1_1 w: " << quaternion1_1.w() << endl; /**** 2. 旋转矩阵 *****/ cout << endl << "********** RotationMatrix **********" << endl; //2.0 旋转矩阵初始化 Eigen::Matrix3d rotation_matrix2; rotation_matrix2 << 0.707107, -0.707107, 0, 0.707107, 0.707107, 0, 0, 0, 1; ; //或直接单位矩阵初始化 Eigen::Matrix3d rotation_matrix2_1 = Eigen::Matrix3d::Identity(); //2.1 旋转矩阵转换为欧拉角 //ZYX顺序，即先绕x轴roll,再绕y轴pitch,最后绕z轴yaw,0表示X轴,1表示Y轴,2表示Z轴 Eigen::Vector3d euler_angles = rotation_matrix2.eulerAngles(2, 1, 0); cout << "yaw(z) pitch(y) roll(x) = " << euler_angles.transpose() << endl; //2.2 旋转矩阵转换为旋转向量 Eigen::AngleAxisd rotation_vector2; rotation_vector2.fromRotationMatrix(rotation_matrix2); //或者 Eigen::AngleAxisd rotation_vector2_1(rotation_matrix2); cout << "rotation_vector2 " << "angle is: " << rotation_vector2.angle() * (180 / M_PI) << " axis is: " << rotation_vector2.axis().transpose() << endl; cout << "rotation_vector2_1 " << "angle is: " << rotation_vector2_1.angle() * (180 / M_PI) << " axis is: " << rotation_vector2_1.axis().transpose() << endl; //2.3 旋转矩阵转换为四元数 Eigen::Quaterniond quaternion2(rotation_matrix2); //或者 Eigen::Quaterniond quaternion2_1; quaternion2_1 = rotation_matrix2; cout << "quaternion2 x: " << quaternion2.x() << endl; cout << "quaternion2 y: " << quaternion2.y() << endl; cout << "quaternion2 z: " << quaternion2.z() << endl; cout << "quaternion2 w: " << quaternion2.w() << endl; cout << "quaternion2_1 x: " << quaternion2_1.x() << endl; cout << "quaternion2_1 y: " << quaternion2_1.y() << endl; cout << "quaternion2_1 z: " << quaternion2_1.z() << endl; cout << "quaternion2_1 w: " << quaternion2_1.w() << endl; /**** 3. 欧拉角 ****/ cout << endl << "********** EulerAngle **********" << endl; //3.0 初始化欧拉角(Z-Y-X，即RPY, 先绕x轴roll,再绕y轴pitch,最后绕z轴yaw) Eigen::Vector3d ea(0.785398, -0, 0); //3.1 欧拉角转换为旋转矩阵 Eigen::Matrix3d rotation_matrix3; rotation_matrix3 = Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(ea[2], Eigen::Vector3d::UnitX()); cout << "rotation matrix3 =\n" << rotation_matrix3 << endl; //3.2 欧拉角转换为四元数, Eigen::Quaterniond quaternion3; quaternion3 = Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(ea[2], Eigen::Vector3d::UnitX()); cout << "quaternion3 x: " << quaternion3.x() << endl; cout << "quaternion3 y: " << quaternion3.y() << endl; cout << "quaternion3 z: " << quaternion3.z() << endl; cout << "quaternion3 w: " << quaternion3.w() << endl; //3.3 欧拉角转换为旋转向量 Eigen::AngleAxisd rotation_vector3; rotation_vector3 = Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(ea[2], Eigen::Vector3d::UnitX()); cout << "rotation_vector3 " << "angle is: " << rotation_vector3.angle() * (180 / M_PI) << " axis is: " << rotation_vector3.axis().transpose() << endl; /**** 4.四元数 ****/ cout << endl << "********** Quaternion **********" << endl; //4.0 初始化四元素,注意eigen Quaterniond类四元数初始化参数顺序为w,x,y,z Eigen::Quaterniond quaternion4(0.92388, 0, 0, 0.382683); //4.1 四元数转换为旋转向量 Eigen::AngleAxisd rotation_vector4(quaternion4); //或者 Eigen::AngleAxisd rotation_vector4_1; rotation_vector4_1 = quaternion4; cout << "rotation_vector4 " << "angle is: " << rotation_vector4.angle() * (180 / M_PI) << " axis is: " << rotation_vector4.axis().transpose() << endl; cout << "rotation_vector4_1 " << "angle is: " << rotation_vector4_1.angle() * (180 / M_PI) << " axis is: " << rotation_vector4_1.axis().transpose() << endl; //4.2 四元数转换为旋转矩阵 Eigen::Matrix3d rotation_matrix4; rotation_matrix4 = quaternion4.matrix(); Eigen::Matrix3d rotation_matrix4_1; rotation_matrix4_1 = quaternion4.toRotationMatrix(); cout << "rotation matrix4 =\n" << rotation_matrix4 << endl; cout << "rotation matrix4_1 =\n" << rotation_matrix4_1 << endl; //4.4 四元数转欧拉角(Z-Y-X，即RPY) Eigen::Vector3d eulerAngle4 = quaternion4.matrix().eulerAngles(2,1,0); cout << "yaw(z) pitch(y) roll(x) = " << eulerAngle4.transpose() << endl; return 0; } -
坐标变换例子
p R 2 = T R 2 , W ∗ T W , R 1 ∗ p R 1 p_{R_2} = T_{{R_2},W} * T_{W,{R_1}} * p_{R_1} pR2=TR2,W∗TW,R1∗pR1
// coordinateTransform.cpp
#include <iostream>
#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
using namespace Eigen;
int main(int argc, char** argv) {
// 四元数表示的旋转矩阵
Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
// 四元数使用之前需要归一化（归一化后数字的精度和大小都会发生变化）
q1.normalize();
q2.normalize();
// 平移向量
Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
// R1 坐标系下点 p1 的坐标
Vector3d p1(0.5, 0, 0.2);
// 位姿的四元数表示和平移向量转化为欧式变换矩阵
Isometry3d T1w(q1), T2w(q2);
T1w.pretranslate(t1);
T2w.pretranslate(t2);
// Trw 下标从左往右读是 r 坐标系变换到 w 坐标系
// Trw 下标从右往左读是 w 坐标系下的坐标变换到 r 坐标系下的坐标
// 1、T1w.inverse() * p1：把 R1 坐标系的坐标 p1 转化为世界坐标系 w 下的坐标
// 2、T2w * T1w.inverse() * p1：世界坐标系 w 下面的坐标 p1 转化为 R2 坐标系下的坐标 p2
Vector3d p2 = T2w * T1w.inverse() * p1;
cout << endl << p2.transpose() << endl;
return 0;
}
// CMakeLists.txt
cmake_minimum_required(VERSION 2.8)
include_directories("/usr/include/eigen3")
add_executable(coordinateTransform coordinateTransform.cpp)
-
坐标变换例子2
参考引用- 有两个右手系1和2，其中2系的x轴与1系的y轴方向相同，2系的y轴与1系z轴方向相反，2系的z轴与1系的x轴相反，两个坐标系原点重合。求R12，求1系中(1,1,1)在2系中的坐标。请自己编写一个c++程序实现它，并用Cmake编译，得到能输出答案的可执行文件
// coordinateTransform2.cpp #include <iostream> #include <Eigen/Core> #include <Eigen/Geometry> #define PI 3.14159265358979323846 Eigen::Matrix3d RPY2Rotation(double, double, double); int main(int argc, char const *argv[]) { //hint 1: Find the Euler Angle from Frame1 to Frame2 //adopt: XYZ rotation order(i.e. roll-pitch-yaw, and ACTIVE axis as reference), //easy to see, Frame 1 ---[roll=-pi/2, pitch=-pi/2, yaw=0]---> Frame 2 double roll, pitch, yaw; roll = -PI/2; pitch = -PI/2; yaw = 0; //hint 2: Convert the Euler Angle into the rotation matrix Eigen::Matrix3d roation_matrix; // The total rotation is decomposed into three rotations about the axis // and then translates to the rotation matrix via AngleAxis Eigen::AngleAxisd rv_roll(roll, Eigen::Vector3d::UnitX()); //ACTIVE axis Eigen::AngleAxisd rv_pitch(pitch, rv_roll*Eigen::Vector3d::UnitY()); //ACTIVE axis Eigen::AngleAxisd rv_yaw(yaw, rv_pitch*rv_roll*Eigen::Vector3d::UnitZ()); //ACTIVE axis // we choose rpy order, so total_roate = y*p*r (Beginners may get confused) roation_matrix = (rv_yaw*rv_pitch*rv_roll).toRotationMatrix(); // R12 //hint 3: Try to convert a known point under the Frame1, like (1,1,1), to Frame2 using the rotation matrix, // and then you can check if your answer is correct intuitively. (The answer is (1, -1, -1)) Eigen::Vector3d point_frame1(1,1,1); Eigen::Vector3d point_frame2; point_frame2 = roation_matrix.transpose() * point_frame1; //p2 = R21 * p1 std::cout << "***************************************" << std::endl; std::cout << "point in frame1: " << point_frame1.transpose() << std::endl; std::cout << "point in frame2: " << point_frame2.transpose() << std::endl; std::cout << "R12: " << std::endl; std::cout << roation_matrix << std::endl; std::cout << "***************************************" << std::endl; std::cout << std::endl; // /*******************we can also use Fixed axis to solve******************/ // //adopt: XYZ rotation order(i.e. roll-pitch-yaw, and FIXED axis as reference) // //eazy to see, Frame 1 ---[roll=-pi/2, pitch=0, yaw=pi/2]---> Frame 2 // // or you can still use angldeAxis // Eigen::AngleAxisd rv_roll(-PI/2, Eigen::Vector3d::UnitX()); // FIXED axis // Eigen::AngleAxisd rv_pitch(0, Eigen::Vector3d::UnitY()); // FIXED axis // Eigen::AngleAxisd rv_yaw(PI/2, Eigen::Vector3d::UnitZ()); // FIXED axis // roation_matrix = (rv_yaw*rv_pitch*rv_roll).toRotationMatrix(); // R12 // roation_matrix = RPY2Rotation(-PI/2,0,PI/2); // FIXED axis R12 return 0; } // except for angleAxis, we can also get the rotation matrix directly from Euler angles Eigen::Matrix3d RPY2Rotation(double roll , double pitch , double yaw) { //roll , pitch , yaw // send EulerAngles and transform into RotationMatrix Eigen::Matrix3d R, Rx, Ry, Rz; Rx << 1.0, 0.0, 0.0, 0.0, cos(roll), -sin(roll), 0.0, sin(roll), cos(roll); Ry << cos(pitch), 0.0, sin(pitch), 0.0, 1.0, 0.0, -sin(pitch), 0.0, cos(pitch); Rz << cos(yaw), -sin(yaw), 0.0, sin(yaw), cos(yaw), 0.0, 0.0, 0.0, 1.0; R = Rz * Ry * Rx; return R; } // 你可能会疑惑，为什么把1系转到2系得到的旋转量是R12，按照我们说的从右往左读的习惯，我们应该写成R21啊？ // 我们所说的a1=R12a2+t12中，R12和t12确实是从右向左读没错，但这只是坐标变换时的较为合适的读法 // R12实际对应的是坐标系1姿态到坐标系2姿态的旋转，我们之所以说R12角标从右往左读，那是对于坐标变换来说的，对坐标轴的姿态变换要从左往右读才是真实旋转方向，这点很重要！ // 同理，t12实际对应的是坐标系1原点指向坐标系2原点的向量，我们之所以说t12角标从右往左读，那是对于坐标变换来说的，对坐标轴的位置变换要从左往右读才是真实平移向量方向，这点很重要！// CMakeLists.txt cmake_minimum_required(VERSION 2.8) project(ch3_homework) find_package(Eigen3) include_directories(${EIGEN3_INCLUDE_DIRS}) add_executable(answer coordinateTransform2.cpp) target_link_libraries(answer ${EIGEN3_LIBRARIES})*************************************** point in frame1: 1 1 1 point in frame2: 1 -1 -1 R12: 2.22045e-16 -5.55112e-17 -1 1 1.11022e-16 2.22045e-16 5.55112e-17 -1 1.11022e-16 *************************************** - 有两个右手系1和2，其中2系的x轴与1系的y轴方向相同，2系的y轴与1系z轴方向相反，2系的z轴与1系的x轴相反，两个坐标系原点重合。求R12，求1系中(1,1,1)在2系中的坐标。请自己编写一个c++程序实现它，并用Cmake编译，得到能输出答案的可执行文件
957
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
