Add two reference paper, add testing of angle between quarternions.

CMakeLists.txt

@@ -128,6 +128,7 @@ ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so
 -lcrypto
 )
 
+include(test/CMakeLists.txt)
 
 # Build examples
 

test/CMakeLists.txt

@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 2.8)
+project(TEST)
+
+find_package(Eigen3 3.1.0 REQUIRED)
+
+add_executable(test_quat test/test_quat_main.cc)
+
+target_link_libraries(test_quat ${EIGEN3_LIBS})
+
+add_executable(test_quat_2_ test/test_quat_main_2.cc)
+
+target_link_libraries(test_quat_2_ ${EIGEN3_LIBS})

test/test_quat_main.cc

@@ -0,0 +1,70 @@
+//
+// Created by sph on 2020/9/15.
+//
+
+#include <iostream>
+
+using namespace std;
+
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/Geometry>
+
+int main() {
+
+  // 四元数初始化
+  double Q1_angle_ = M_PI / 4;
+  Eigen::Quaterniond Q1(cos(Q1_angle_ / 2), 0 * sin(Q1_angle_ / 2), 0 * sin(Q1_angle_ / 2), 1 * sin(Q1_angle_ / 2));
+
+  double Q2_angle_ = M_PI / 4 + M_PI / 4;
+  Eigen::Quaterniond Q2(cos(Q2_angle_ / 2), 0 * sin(Q2_angle_ / 2), 0 * sin(Q2_angle_ / 2), 1 * sin(Q2_angle_ / 2));
+
+  double Q3_angle_ = M_PI / 4 - M_PI / 4;
+  Eigen::Quaterniond Q3(cos(Q3_angle_ / 2), 0 * sin(Q3_angle_ / 2), 0 * sin(Q3_angle_ / 2), 1 * sin(Q3_angle_ / 2));
+
+  // quat to rotation_matrix
+  Eigen::Matrix3d rotation_matrix_Q1_ = Q1.matrix();
+  cout << "rotation_matrix_Q1_ = \n" << rotation_matrix_Q1_ << endl;
+
+  Eigen::Matrix3d rotation_matrix_Q2_ = Q2.matrix();
+  cout << "rotation_matrix_Q2_ = \n" << rotation_matrix_Q2_ << endl;
+  Eigen::Matrix3d rotation_matrix_Q2_ed_ = rotation_matrix_Q2_ * rotation_matrix_Q1_.inverse();
+
+  cout <<  "旋转角度： " << -asin(rotation_matrix_Q2_ed_(0, 1)) * 180 / M_PI << "度" << endl;
+  cout << endl;
+
+  Eigen::Matrix3d rotation_matrix_Q3_ = Q3.matrix();
+  cout << "rotation_matrix_Q3_ = \n" << rotation_matrix_Q3_ << endl;
+
+//  //平移向量
+//  Eigen::Vector3d t1 = Eigen::Vector3d(0.3, 0.1, 0.1);
+//  Eigen::Vector3d t2 = Eigen::Vector3d(-0.1, 0.5, 0.3);
+//
+//  //目标向量
+//  Eigen::Vector3d p1 = Eigen::Vector3d(0.5, 0, 0.2);
+//  Eigen::Vector3d p2;
+//
+//  //打印输出
+//  // cout << q1.coeffs() << "\n"
+//  //      << q2.coeffs() << "\n"
+//  //      << t1.transpose() << "\n"
+//  //      << t2.transpose() << endl;
+//
+//  //四元数求解
+//  p2 = q2 * q1.inverse() * (p1 - t1) + t2;
+//  cout << p2.transpose() << endl;
+//
+//  //欧拉矩阵
+//  Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();
+//  Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
+//  T1.rotate(q1.toRotationMatrix());
+//  T1.pretranslate(t1);
+//  T2.rotate(q2.toRotationMatrix());
+//  T2.pretranslate(t2);
+//
+//  // cout << T1.matrix() << endl;
+//  // cout << T2.matrix() << endl;
+//
+//  //欧拉矩阵求解
+//  p2 = T2 * T1.inverse() * p1;
+//  cout << p2.transpose() << endl;
+}

test/test_quat_main_2.cc

@@ -0,0 +1,123 @@
+//
+// Created by sph on 2020/9/15.
+//
+
+#include <iostream>
+#include <Eigen/Dense>
+
+#include <math.h>
+
+using namespace Eigen;
+using namespace std;
+
+int main()
+{
+  /* 三维笛卡尔坐标系
+   * 1）一个物体在我们的正前方；朝向我们的方向为X正，绕着X方向的旋转为翻滚角;YZ坐标系顺时针为正
+   * 2）物体右手侧的方向为Y；绕着Y方向的旋转为俯仰角；ZX坐标系顺时针为正；
+   * 3）物体上方的方向为Z；绕着Z方向的旋转为偏航角；XY坐标系顺时针为正；
+  */
+
+  /* 验证翻滚角 */
+  double rollPrevious = M_PI/6;
+  double pitchPrevious = 0;
+  double yawPrevious = 0;
+
+  Vector3d eulerAngulePrevious(rollPrevious, pitchPrevious, yawPrevious);
+
+  Matrix3f matrixPrevious;
+  matrixPrevious = AngleAxisf(eulerAngulePrevious(0), Vector3f::UnitX()) * AngleAxisf(eulerAngulePrevious(1), Vector3f::UnitY()) * AngleAxisf(eulerAngulePrevious(2), Vector3f::UnitZ());
+
+  double rollCurrent = M_PI/3;
+  double pitchCurrent = 0;
+  double yawCurrent = 0;
+
+  Vector3d eulerAnguleCurrent(rollCurrent, pitchCurrent, yawCurrent);
+
+  Matrix3f matrixCurrent;
+  matrixCurrent = AngleAxisf(eulerAnguleCurrent(0), Vector3f::UnitX()) * AngleAxisf(eulerAnguleCurrent(1), Vector3f::UnitY()) * AngleAxisf(eulerAnguleCurrent(2), Vector3f::UnitZ());
+
+  Matrix3f rotationMatrix = matrixCurrent * matrixPrevious.inverse();
+  cout << "绕X轴旋转PI/6的旋转矩阵:\n" << rotationMatrix << endl;
+
+  /*绕X轴的旋转矩阵
+   * 1        0               0
+   * 0      cos(roll)   -sin(roll)
+   * 0      sin(roll)    cos(roll)
+  */
+  cout <<  "旋转角度： " << -asin(rotationMatrix(1, 2)) * 180 / M_PI << "度" << endl;
+  cout << endl;
+
+  /* 验证俯仰角 */
+  rollPrevious = 0;
+  pitchPrevious = M_PI/6;
+  yawPrevious = 0;
+
+  eulerAngulePrevious(0) = rollPrevious;
+  eulerAngulePrevious(1) = pitchPrevious;
+  eulerAngulePrevious(2) = yawPrevious;
+
+  matrixPrevious = AngleAxisf(eulerAngulePrevious(0), Vector3f::UnitX()) * AngleAxisf(eulerAngulePrevious(1), Vector3f::UnitY()) * AngleAxisf(eulerAngulePrevious(2), Vector3f::UnitZ());
+
+  rollCurrent = 0;
+  pitchCurrent = M_PI/3;
+  yawCurrent = 0;
+
+  eulerAnguleCurrent(0) = rollCurrent;
+  eulerAnguleCurrent(1) = pitchCurrent;
+  eulerAnguleCurrent(2) = yawCurrent;
+
+  matrixCurrent = AngleAxisf(eulerAnguleCurrent(0), Vector3f::UnitX()) * AngleAxisf(eulerAnguleCurrent(1), Vector3f::UnitY()) * AngleAxisf(eulerAnguleCurrent(2), Vector3f::UnitZ());
+
+  rotationMatrix = matrixCurrent * matrixPrevious.inverse();
+  cout << "绕Y轴旋转PI/6的旋转矩阵:\n" << rotationMatrix << endl;
+
+  /*绕Y轴的旋转矩阵
+   * cos(pitch)        0          sin(pitch)
+   * 0                 1               0
+   * -sin(pitch)       0         -cos(pitch)
+  */
+  cout <<  "旋转角度： " << asin(rotationMatrix(0, 2)) * 180 / M_PI << "度" << endl;
+  cout << endl;
+
+
+  /* 验证偏航角 */
+  rollPrevious = 0;
+  pitchPrevious = 0;
+  yawPrevious = M_PI/6;
+
+  eulerAngulePrevious(0) = rollPrevious;
+  eulerAngulePrevious(1) = pitchPrevious;
+  eulerAngulePrevious(2) = yawPrevious;
+
+  matrixPrevious = AngleAxisf(eulerAngulePrevious(0), Vector3f::UnitX()) * AngleAxisf(eulerAngulePrevious(1), Vector3f::UnitY()) * AngleAxisf(eulerAngulePrevious(2), Vector3f::UnitZ());
+
+  rollCurrent = 0;
+  pitchCurrent = 0;
+  yawCurrent = M_PI/3;
+
+  eulerAnguleCurrent(0) = rollCurrent;
+  eulerAnguleCurrent(1) = pitchCurrent;
+  eulerAnguleCurrent(2) = yawCurrent;
+
+  matrixCurrent = AngleAxisf(eulerAnguleCurrent(0), Vector3f::UnitX()) * AngleAxisf(eulerAnguleCurrent(1), Vector3f::UnitY()) * AngleAxisf(eulerAnguleCurrent(2), Vector3f::UnitZ());
+
+  rotationMatrix = matrixCurrent * matrixPrevious.inverse();
+  cout << "绕Z轴旋转PI/6的旋转矩阵:\n" << rotationMatrix << endl;
+
+  /*绕Z轴的旋转矩阵
+   * cos(yaw)        -sin(yaw)          0
+   * sin(yaw)        cos(yaw)           0
+   * 0                  0               1
+  */
+  cout <<  "旋转角度： " << -asin(rotationMatrix(0, 1)) * 180 / M_PI << "度" << endl;
+  cout << endl;
+
+  /* 按照元素相乘 */
+  Eigen::Vector3d temp1(-1, 0, 1);
+  Eigen::Vector3d temp2(-2, 0, 2);
+
+  cout << temp1.cwiseProduct(temp2) << endl;
+  cout << endl;
+
+}