diff --git a/CMakeLists.txt b/CMakeLists.txt index 18a98b3..01b0eed 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -128,6 +128,7 @@ ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so -lcrypto ) +include(test/CMakeLists.txt) # Build examples diff --git a/reference/ORB An Efficient Alternative to SIFT or SURF.pdf b/reference/ORB An Efficient Alternative to SIFT or SURF.pdf new file mode 100644 index 0000000..6d3a8fd Binary files /dev/null and b/reference/ORB An Efficient Alternative to SIFT or SURF.pdf differ diff --git a/reference/The TUM VI Benchmark for Evaluating Visual-Inertial Odometry.pdf b/reference/The TUM VI Benchmark for Evaluating Visual-Inertial Odometry.pdf new file mode 100755 index 0000000..b60ff03 Binary files /dev/null and b/reference/The TUM VI Benchmark for Evaluating Visual-Inertial Odometry.pdf differ diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..471c262 --- /dev/null +++ b/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}) \ No newline at end of file diff --git a/test/test_quat_main.cc b/test/test_quat_main.cc new file mode 100644 index 0000000..727424a --- /dev/null +++ b/test/test_quat_main.cc @@ -0,0 +1,70 @@ +// +// Created by sph on 2020/9/15. +// + +#include + +using namespace std; + +#include +#include + +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; +} \ No newline at end of file diff --git a/test/test_quat_main_2.cc b/test/test_quat_main_2.cc new file mode 100644 index 0000000..b2ac3ac --- /dev/null +++ b/test/test_quat_main_2.cc @@ -0,0 +1,123 @@ +// +// Created by sph on 2020/9/15. +// + +#include +#include + +#include + +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; + +}