目次
1 初期化
回転するには 2 つの方法があります:
(1) 回転行列: a = Ra'、a' = R^T a、回転行列の特徴: 行列式が 1 の直交行列です。 3 次元空間の回転は 3x3 の行列で、 9 つの変数によって制御されます。
(2) 回転ベクトル: 方向は回転軸と一致し、長さは回転角度に等しく、回転を記述するために必要な 3 次元ベクトルは 1 つだけです。
// Eigen/Geometry 模块提供了各种旋转和平移的表示
// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
Matrix3d rotation_matrix = Matrix3d::Identity();
// 1, 初始化旋转向量使用 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()转换成矩阵
// 2, 初始化旋转矩阵，也可以直接赋值
rotation_matrix = rotation_vector.toRotationMatrix(); 回転行列 =
0.707 -0.707 0
0.707 0.707 0
0 0 1
回転空間内の 2 つのベクトル
// 3, 旋转，用 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;
// 4, 旋转，或者用旋转矩阵
v_rotated = rotation_matrix * v;
cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;回転後 (1,0,0) (角度軸による) = 0.707 0.707 0
回転後 (1,0,0) (行列による) = 0.707 0.707 0
3 オイラー角
回転行列であっても回転ベクトルであっても、人間にとっては直観的ではなく、オイラー角は回転を異なる軸の周りの 3 つの回転に分解します。3 軸上のコーナー。
回転行列による -> オイラー角
// 5, 欧拉角: 可以将旋转矩阵直接转换成欧拉角
Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即yaw-pitch-roll(ypr)顺序
cout << "yaw pitch roll = " << euler_angles.transpose() << endl;ヨーピッチロール = 0.785 -0 0
4 変換行列
回転と平行移動
// 6, 欧氏变换 矩阵使用 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;
// 7, 用变换矩阵进行坐标变换
Vector3d v_transformed = T * v; // 相当于R*v+t
cout << "v tranformed = " << v_transformed.transpose() << endl;変換行列 =
0.707 -0.707 0 1
0.707 0.707 0 3
0 0 1 4
0 0 0 1
vtransformed = 1.71 3.71 4
5 四つの要素
回転行列は 9 つの量を使用して 3 自由度の回転を記述しますが、これは冗長です。オイラー角と回転ベクトルはコンパクトですが、特異です。4 つの要素はいずれもコンパクトであり、単一ではありません。デメリットとしては直感的ではなく、操作がやや複雑なことです。
q は回転を指定する 4 つの要素です。翻訳が必要な場合は、数式の後に +t を入力してください。
// 8, 四元数
// 初始化1，可以直接把选择向量AngleAxis赋值给四元数，反之亦然
Quaterniond q = Quaterniond(rotation_vector);
// 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
cout << "quaternion from rotation vector = " << q.coeffs().transpose() << endl;
// 初始化2，也可以把旋转矩阵赋给它
q = Quaterniond(rotation_matrix);
cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;回転ベクトルからの四元数 = 0 0 0.383 0.924
回転行列からの四元数 = 0 0 0.383 0.924
// 使用四元数旋转一个向量，使用重载的乘法即可
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;回転後の (1,0,0) = 0.707 0.707 0 は
0.707 0.707 0 0 と等しくなります。
完全なプログラム
#include <iostream>
#include <cmath>
using namespace std;
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
using namespace Eigen;
// 本程序演示了 Eigen 几何模块的使用方法
int main(int argc, char **argv) {
// Eigen/Geometry 模块提供了各种旋转和平移的表示
// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
Matrix3d rotation_matrix = Matrix3d::Identity();
// 1, 初始化旋转向量使用 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()转换成矩阵
// 2, 初始化旋转矩阵，也可以直接赋值
rotation_matrix = rotation_vector.toRotationMatrix();
// 3, 旋转，用 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;
// 4, 旋转，或者用旋转矩阵
v_rotated = rotation_matrix * v;
cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;
// 5, 欧拉角: 可以将旋转矩阵直接转换成欧拉角
Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即yaw-pitch-roll(ypr)顺序
cout << "yaw pitch roll = " << euler_angles.transpose() << endl;
// 6, 欧氏变换 矩阵使用 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;
// 7, 用变换矩阵进行坐标变换
Vector3d v_transformed = T * v; // 相当于R*v+t
cout << "v tranformed = " << v_transformed.transpose() << endl;
// 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略
// 8, 四元数
// 初始化1，可以直接把选择向量AngleAxis赋值给四元数，反之亦然
Quaterniond q = Quaterniond(rotation_vector);
// 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
cout << "quaternion from rotation vector = " << q.coeffs().transpose() << endl;
// 初始化2，也可以把旋转矩阵赋给它
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;
}