Rotation & Translation Exercise

  // 变量表
  // Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列
  Eigen::Matrix<float, 2, 3> matrix_f23;

  // Eigen 通过 typedef 提供了许多内置类型，其底层是Eigen::Matrix
  Eigen::Vector3d v_3d_1;
  Eigen::Matrix<float, 3, 1> v_3f_1;

  // Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
  Eigen::Matrix3d matrix_d33 = Eigen::Matrix3d::Zero(); //初始化为0
  // 如果不确定矩阵大小，可以使用动态大小的矩阵
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;
  // 或者更简单的方式
  Eigen::MatrixXd matrix_x;

  clock_t time_stt;

  // 下面是对Eigen阵的操作
  // 输入数据（初始化）
  matrix_f23 << 1, 2, 3, 4, 5, 6;
  // 输出
  std::cout << "matrix 2x3 from 1 to 6: \n" << matrix_f23 << "\n";

  // 用()访问矩阵中的元素
  std::cout << "print matrix 2x3: " << "\n";
  for (int i = 0; i < 2; i++) {
      for (int j = 0; j < 3; j++)
          std::cout << matrix_f23(i, j) << "\t";
      std::cout << "\n";
  }

  // 矩阵和向量相乘（实际上仍是矩阵和矩阵）
  v_3d_1 << 3, 2, 1;
  v_3f_1 << 4, 5, 6;

  // 但是在Eigen里你不能混合两种不同类型的矩阵，这样是错的
  // Eigen::Matrix<double, 2, 1> result_wrong_type = matrix_f23 * v_3d_1;
  // 应该显式转换
  Eigen::Matrix<double, 2, 1> result = matrix_f23.cast<double>() * v_3d_1;
  std::cout << "[1,2,3;4,5,6]*[3,2,1]=" << result << "\n";
  // transpose 代表转置
  std::cout << "[1,2,3;4,5,6]*[3,2,1]=" << result.transpose() << "\n";

  Eigen::Matrix<float, 2, 1> result2 = matrix_f23 * v_3f_1;
  std::cout << "[1,2,3;4,5,6]*[4,5,6]: " << result2 << "\n";
  std::cout << "[1,2,3;4,5,6]*[4,5,6]: " << result2.transpose() << "\n";

  // 同样你不能搞错矩阵的维度
  // 试着取消下面的注释，看看Eigen会报什么错
  // Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d_1;

  // 四则运算直接用+-*/即可。

  // 生成随机数矩阵
  matrix_d33 = Eigen::Matrix3d::Random();
  // 显示矩阵
  std::cout << "random matrix: \n" << matrix_d33 << "\n";
  // 转置
  std::cout << "transpose: \n" << matrix_d33.transpose() << "\n";
  // 各元素和
  std::cout << "sum: " << matrix_d33.sum() << "\n";
  // 迹
  std::cout << "trace: " << matrix_d33.trace() << "\n";
  // 数乘
  std::cout << "times 10: \n" << 10 * matrix_d33 << "\n";
  // 逆
  std::cout << "inverse: \n" << matrix_d33.inverse() << "\n";
  // 行列式
  std::cout << "det: " << matrix_d33.determinant() << "\n";

  // 实对称矩阵可以保证对角化成功
  // A^T*A 一定是实对称矩阵
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(matrix_d33.transpose() * matrix_d33);
  // 特征值
  std::cout << "Eigen values = \n" << eigen_solver.eigenvalues() << "\n";
  // 特征向量
  std::cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << "\n";

  // 直接求逆法
  int matrix_equation_direct(Eigen::Matrix<double, MATRIX_SIZE, 1> &x,const Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> &matrix_NN,const Eigen::Matrix<double,MATRIX_SIZE,1> &v_Nd) {
      time_stt = clock(); // 计时
      x = matrix_NN.inverse() * v_Nd;
      std::cout << "time of normal inverse is "
                << (double) (1000 * (clock() - time_stt)) / (double) CLOCKS_PER_SEC << "ms" << "\n";
      return 0;
  }

  // 矩阵分解法，通常用此方法来求解，例如QR分解，速度会快很多
  int matrix_equation_qr(Eigen::Matrix<double, MATRIX_SIZE, 1> &x,const Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> &matrix_NN,const Eigen::Matrix<double,MATRIX_SIZE,1> &v_Nd) {
      time_stt = clock();
      x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
      std::cout << "time of Qr decomposition is "
                << (double)(1000 * (clock() - time_stt)) / (double) CLOCKS_PER_SEC << "ms" << "\n";
      return 0;
  }

  // cholesky分解法
  // 对于正定矩阵，还可以用cholesky分解来解方程
  int matrix_equation_cholesky(Eigen::Matrix<double, MATRIX_SIZE, 1> &x,const Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> &matrix_NN,const Eigen::Matrix<double,MATRIX_SIZE,1> &v_Nd) {
      time_stt = clock();
      x = matrix_NN.ldlt().solve(v_Nd);
      std::cout << "time of ldlt decomposition is "
                << (double)(1000 * (clock() - time_stt)) / (double) CLOCKS_PER_SEC << "ms" << "\n";
      return 0;
  }

  // 解矩阵方程
  int matrix_equation() {
      // 我们求解 matrix_NN * x = v_Nd 这个方程
      // N的大小在前边的宏里定义，它由随机数生成
      // 直接求逆自然是最直接的，但是求逆运算量大

      //Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN
      //        = Eigen::MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
      Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN
              = Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE>::Random();
      matrix_NN = matrix_NN * matrix_NN.transpose();  // 保证半正定
      Eigen::Matrix<double, MATRIX_SIZE, 1> v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE, 1);

      Eigen::Matrix<double, MATRIX_SIZE, 1> x{Eigen::Matrix<double,MATRIX_SIZE,1>::Random()};
      matrix_equation_direct(x,matrix_NN,v_Nd);
      std::cout << "x = " << x.transpose() << "\n";
      matrix_equation_qr(x,matrix_NN,v_Nd);
      std::cout << "x = " << x.transpose() << "\n";
      matrix_equation_cholesky(x,matrix_NN,v_Nd);
      std::cout << "x = " << x.transpose() << "\n";
      return 0;
  }

  // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
  Eigen::Matrix3d rotation_matrix;
  // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
  Eigen::AngleAxisd rotation_vector;

  Eigen::Vector3d v(1, 0, 0);
  // 向量v的旋转后坐标
  Eigen::Vector3d v_rotated;
  // 欧拉角: 可以将旋转矩阵直接转换成欧拉角
  Eigen::Vector3d euler_angles;
  // 四元数
  Eigen::Quaterniond q;

  // Eigen/Geometry 模块提供了各种旋转和平移的表示
  // 旋转矩阵初始化
  rotation_matrix = Eigen::Matrix3d::Identity();
  // 旋转向量初始化，沿 Z 轴旋转 45 度
  rotation_vector = Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d(0, 0, 1));
  std::cout.precision(3);
  std::cout << "rotation matrix =\n" << rotation_vector.matrix() << "\n";   //用matrix()转换成矩阵
  // 也可以直接赋值
  // 由旋转向量转为旋转矩阵
  rotation_matrix = rotation_vector.toRotationMatrix();
  // 用 AngleAxis 可以进行坐标变换
  v_rotated = rotation_vector * v;
  std::cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << "\n";
  // 或者用旋转矩阵
  v_rotated = rotation_matrix * v;
  std::cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << "\n";

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

  // 可以将旋转向量直接转换成欧拉角
  euler_angles = rotation_vector.eulerAngles(0, 1, 2); // XYZ 顺序，即 pitch-roll-yaw 顺序
  std::cout << "pitch roll yaw = " << euler_angles.transpose() << "\n";

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

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

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

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

  Eigen::Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
  q1.normalize(); // 归一化
  q2.normalize();
  Eigen::Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
  Eigen::Vector3d p1(0.5, 0, 0.2);

  Eigen::Isometry3d T1w(q1), T2w(q2); // 表示仿射变换的类
  T1w.pretranslate(t1); // 进行平移变换
  T2w.pretranslate(t2);

  // 对欧式变换矩阵求逆
  Eigen::Vector3d p2 = T2w * T1w.inverse() * p1;
  std::cout << "\n" << p2.transpose() << "\n";