向量的运算复习

1. 向量的内积

   $$ a b = [e_1, e_2, e_3] \left[ \begin{array}{c} a_1 \\ a_2 \\ a_3\end{array}\right] = a_1e_1+a_2e_2+a_3e_3 $$

2. 向量的外积

$$ a \times b = \left|\left| \begin{array}{c}i&j&k\\a_1&a_2&a_3 \\ b_1&b_2&b_3 \end{array}\right| \right| = \left[ \begin{array}{c}a_2b_3-a_3b_2 \\ a_3b_1-a_1b_3\\a_1b_2-a_2b_1 \end{array}\right] = \left[\begin{array}{c} 0&-a_3&a_2 \\ a_3&0&-a_1 \\ -a_2&a_1&0\end{array} \right] b \equiv a^\wedge b $$

可以把向量的外积 $a \times b$ 写成一个矩阵与向量之间的乘法运算 $a^{\wedge}b$ , 表示向量的旋转。

• 符号 $^{\wedge}$ 是向量到反对称矩阵的转换符

  $$ a^{\wedge} = \left[\begin{array}{c} 0&-a_3&a_2 \\ a_3&0&-a_1 \\ -a_2&a_1&0\end{array} \right] $$

旋转矩阵

旋转矩阵 $R$ 描述了旋转前后同一个向量的坐标变换关系，只要旋转是一样的，那么这个旋转矩阵也是一样的。

$$ \left[ \begin{array}{c} a_1 \\ a_2 \\ a_3\end{array}\right] = \left[ \begin{array} { c c c } \boldsymbol { e } _ { 1 } ^ { T } \boldsymbol { e } _ { 1 } ^ { \prime } & \boldsymbol { e } _ { 1 } ^ { T } \boldsymbol { e } _ { 2 } ^ { \prime } & \boldsymbol { e } _ { 1 } ^ { T } \boldsymbol { e } _ { 3 } ^ { \prime } \\ \boldsymbol { e } _ { 2 } ^ { T } \boldsymbol { e } _ { 1 } ^ { \prime } & \boldsymbol { e } _ { 2 } ^ { T } \boldsymbol { e } _ { 2 } ^ { \prime } & \boldsymbol { e } _ { 2 } ^ { T } \boldsymbol { e } _ { 3 } ^ { \prime } \\ \boldsymbol { e } _ { 3 } ^ { T } \boldsymbol { e } _ { 1 } ^ { \prime } & \boldsymbol { e } _ { 3 } ^ { T } \boldsymbol { e } _ { 2 } ^ { \prime } & \boldsymbol { e } _ { 3 } ^ { T } \boldsymbol { e } _ { 3 } ^ { \prime } \end{array} \right] \left[ \begin{array} { c } a _ { 1 } ^ { \prime } \\ a _ { 2 } ^ { \prime } \\ a _ { 3 } ^ { \prime } \end{array} \right]= R a' $$

• 旋转矩阵是一个正交矩阵，其行列式的值为1. 同样的，行列式的值为1的正交矩阵也是旋转矩阵

• 旋转矩阵的逆矩阵（即其转置矩阵）描述了一个相反的旋转

  $$ a'=R^{-1}a=R^Ta $$

• 欧式变换可以描述为一次旋转变换加上一次平移，则

  $$ a' = Ra+t $$

变换矩阵

使用齐次坐标形式的矩阵重写欧式变换的表达式，将平移和旋转写在同一个矩阵里，将这个矩阵 $T$ 称为变换矩阵。

$$ \left[ \begin{array}{c} a' \\ 1\end{array}\right] = \left[ \begin{array}{c} R & t \\ 0^T &1\end{array}\right] \left[ \begin{array}{c} a \\ 1\end{array}\right] \equiv T \left[ \begin{array}{c} a \\ 1\end{array}\right] \\ T = \left[ \begin{array}{c} R & t \\ 0^T &1\end{array}\right] $$

• 齐次坐标: 在三维向量 $a$ 的末尾添加一个1, 使其编变成四维向量, 称之为齐次坐标 $\tilde a$.

• 齐次坐标使用4个实数来描述一个三维向量, 多了一个自由度. 因此, 在齐次坐标当中, 某个点 $x$ 的每个分量乘以一个非零常数 $k$ 后, 任然表示同一个点.

  $$ \tilde x = [x, y, z, w]^T = [x/w, y/w, z/w, 1]^T $$

• 如果不使用齐次坐标来表示对一个点进行两次变换的话, $c = R_2(R_1a+t_1)+t_2$, 而使用齐次坐标的形式的话, 就可以简单的描述为 $\tilde c = T_2T_1 \tilde a$

• 变换矩阵的逆矩阵 $T ^{-1}$ 表示与原变换矩阵 $T$ 相反的变换

  $$ T^{-1} = \left[ \begin{array}{c} R^T&-R^Tt \\ 0^T&1\end{array}\right] $$

Eigen 的使用

矩阵的创建与运算

#include <iostream>
#include <ctime>

// Eigen
#include "Eigen/Core"
#include "Eigen/Dense"

#define MATRIX_SIZE 50

using namespace std;

int main() {
    Eigen::Matrix<float, 2, 3> matrix_23;
//    Eigen::Matrix<double, 3, 1> v_3d;
    Eigen::Vector3d v_3d;
//    Eigen::Matrix<double, 3, 3> matrix_33 = Eigen::Matrix<double, 3, 3>::Zero();
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();
//    Eigen::Matrix<double, -1, -1> matrix_dynamic;
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;
    Eigen::MatrixXd matrix_x;

    // 输入数据
    matrix_23 << 1, 2, 3, 4, 5, 6;
    v_3d << 3, 2, 1;
    // 访问数据
    cout << matrix_23 << endl;
    cout << matrix_23(0, 0) << endl;

    // 相乘
    // 相乘的两个矩阵类型应一致, 因此需要强制类型转化
    Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;

    // 随机初始化矩阵
    matrix_33 = Eigen::Matrix3d::Random();
    cout << "matrix_33: \n" << matrix_33 << endl << endl;

    matrix_33.transpose(); // 转置
    matrix_33.sum(); //求和
    matrix_33.inverse(); // 求逆
    matrix_33.trace(); // 矩阵的迹
    matrix_33 * 10; // 矩阵的数乘
    matrix_33.determinant(); // 矩阵的行列式的值

    // 特征值
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(matrix_33 * matrix_33.transpose());
    cout << "The Eigen values: \n" << eigen_solver.eigenvalues() << endl;
    cout << "The Eigen vectors: \n" << eigen_solver.eigenvectors() << endl;

    // 求解方程
    Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_nn = Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE>::Random();
    Eigen::Matrix<double, MATRIX_SIZE, 1> v_Nd = Eigen::Matrix<double, MATRIX_SIZE, 1>::Random();
    // 方式一, 直接求逆
    clock_t time_start = clock();
    Eigen::Matrix<double, MATRIX_SIZE, 1> x = matrix_nn.inverse() * v_Nd;
    cout << "The time used in normal inverse way is " << 1000*(clock() - time_start)/(double)CLOCKS_PER_SEC << "ms" << endl;
    // 方式二, 矩阵分解
    time_start = clock();
    x = matrix_nn.colPivHouseholderQr().solve(v_Nd);
    cout << "The time used in Qr composition way is " << 1000*(clock() - time_start)/(double)CLOCKS_PER_SEC << "ms" << endl;
    return 0;
}

使用 Eigen 进行坐标变换

//
// Created by fjczx on 2021/8/25.
//
#include <iostream>
#include <cmath>

#include "Eigen/Core"
#include "Eigen/Geometry"

using namespace std;

int main(){
    // 3D旋转矩阵的表示
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    cout << "Identity matrix: \n" << rotation_matrix << endl;

    // 旋转向量表示
    Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0, 0, 1)); //绕Z轴旋转45度
    cout .precision(3);
    cout << "Rotation matrix: \n" << rotation_vector.matrix() << endl; //转换成旋转矩阵表示
    rotation_matrix = rotation_vector.toRotationMatrix(); //也可以直接赋值

    //使用旋转向量进行坐标变换
    Eigen::Vector3d v(1, 0, 0);
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout << "Vector (1, 0, 0) after rotation: " << v_rotated.transpose() << endl;

    // 使用旋转矩阵进行坐标变换
    v_rotated = rotation_matrix * v;
    cout << "Vector (1, 0, 0) after rotation: " << v_rotated.transpose() << endl;

    // 将旋转矩阵转换为欧拉角表示法
    Eigen::Vector3d euler_angle = rotation_matrix.eulerAngles(2, 1, 0); // 按照 yaw pitch roll 的顺序
    cout << "yaw pitch roll: " << euler_angle.transpose() << endl;

    // 变换矩阵T
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    cout << "Identity Transform matrix: \n" << T.matrix() << endl;
    T.rotate(rotation_vector);
    T.pretranslate(Eigen::Vector3d(1, 3, 4));
    cout << "Transform matrix: \n" << T.matrix() << endl;

    // 使用变换矩阵进行坐标变换
    Eigen::Vector3d v_transform = T*v;
    cout << "v transform: \n" << v_transform.transpose() << endl;

    // 四元数
    // 可以直接把旋转向量赋值给四元素, 也可以把四元数赋值给旋转向量
    Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
    cout << "Quaternion: \n" << q.coeffs().transpose() << endl;

    // 使用四元数进行坐标变换
    v_rotated = q*v;
    cout << "Vector (1, 0, 0) after rotation: " << v_rotated.transpose() << endl;
    return 0;
}

旋转向量

对于任意旋转都可以用一个旋转轴和一个旋转角来表示. 因此, 我们可以使用一个向量, 其方向与旋转轴一致, 而长度等于旋转角. 我们把这个向量称为旋转向量.

假设有一个旋转轴为 $n$ , 角度为 $\theta$ 的旋转, 则其对应的旋转向量为 $\theta n$.

• 从旋转向量转为为旋转矩阵, 可以使用 罗德里格旋转公式 进行转换

  $$ R= \cos \theta I+(1-\cos \theta)nn^T+\sin \theta n^{\wedge} $$

  运算符 $^{\wedge}$

  $$ \theta = \arccos \left( \cfrac{tr(R)-1}{2}\right) $$

  转轴 $n$ 为旋转矩阵 $R$ 特征值为 $1$ 所对应的特征向量.

欧拉角

1. 绕物体的 $Z$ 轴旋转 — 偏航角 yaw
2. 绕旋转之后的 $Y$ 轴旋转 — 俯仰角 pitch
3. 绕旋转之后的 $X$ 轴旋转 — 滚转角 roll

Unity WebGL Player | Objects

• 欧拉角的旋转顺序一般是yaw-pitch-roll
• 欧拉角存在万向锁问题, 即当 pitch = $\pm 90^\circ$ 时, 第一次旋转将和第三次旋转使用同一个轴

四元数

使用四元数能够紧凑的, 无奇异性的表示一个三维旋转

四元数的表示方式

1. $q = q_0+q_1i+q_2j+q_3k$
2. $q = [s, v], s=q_0 \in \mathbb{R},v=[q_1,q_2,q_3]^T \in \mathbb{R}^3$

• 两个互为相反数的四元数代表一个相同的旋转.
• 虚四元数($v=[0,0,0]^T$) 可以表示三维空间中的一个点
• 单位四元数表示三维空间中的任意一个旋转

旋转向量与四元数的转化

对于旋转向量 $\theta n$, 其对应的四元数为 $q =[s, v]$

$$ q = [\cos \cfrac{\theta}{2}, n\sin \theta]^T

$$

$$ \begin{cases}\theta = 2 \arccos q_0 \\ n = v / \sin \theta \end{cases} $$

使用四元数来表示旋转

对于三维空间中的点 $p = [0, v]$, 使用四元数表示这个旋转 $q = [\cos \cfrac{\theta}{2}, n\sin \theta]$, 那么旋转之后的点 $p' = qpq^{-1}$

相似, 仿射, 射影变换矩阵形式

1. 相似变换

   相似变换比普通的欧式变换多了一个自由度, 允许物体进行均匀缩放

   $$ T= \left[ \begin{array}{c} sR & t \\ 0^T &1\end{array}\right] $$

2. 仿射变换只要求 $A$ 是一个可逆矩阵, 而不一定是一个正交矩阵.

   $$ T= \left[ \begin{array}{c} A & t \\ 0^T &1\end{array}\right] $$

3. 射影变换

   射影变换是对一般的变换

   $$ T= \left[ \begin{array}{c} A & t \\ \alpha ^T &v\end{array}\right] $$