| #include <gtest/gtest.h> |
| |
| #include <private/dvr/eigen.h> |
| #include <private/dvr/pose.h> |
| #include <private/dvr/test/test_macros.h> |
| |
| using PoseTypes = ::testing::Types<float, double>; |
| |
| template <class T> |
| class PoseTest : public ::testing::TestWithParam<T> { |
| public: |
| using FT = T; |
| using Pose_t = android::dvr::Pose<FT>; |
| using quat_t = Eigen::Quaternion<FT>; |
| using vec3_t = Eigen::Vector3<FT>; |
| using mat4_t = Eigen::AffineMatrix<FT, 4>; |
| }; |
| |
| TYPED_TEST_CASE(PoseTest, PoseTypes); |
| |
| // Check that the two matrix methods are inverses of each other |
| TYPED_TEST(PoseTest, SelfInverse) { |
| using quat_t = typename TestFixture::quat_t; |
| using vec3_t = typename TestFixture::vec3_t; |
| using Pose_t = typename TestFixture::Pose_t; |
| using mat4_t = typename TestFixture::mat4_t; |
| using FT = typename TestFixture::FT; |
| |
| const auto tolerance = FT(0.0001); |
| |
| const quat_t initial_rotation(Eigen::AngleAxis<FT>( |
| FT(M_PI / 3.0), vec3_t(FT(3.0), FT(4.0), FT(5.0)).normalized())); |
| const vec3_t initial_position = vec3_t(FT(2.0), FT(10.0), FT(-4.0)); |
| const Pose_t initial_pose(initial_rotation, initial_position); |
| |
| auto result_pose = initial_pose.GetReferenceFromObjectMatrix() * |
| initial_pose.GetObjectFromReferenceMatrix(); |
| |
| EXPECT_MAT4_NEAR(result_pose, mat4_t::Identity(), tolerance); |
| } |
| |
| TYPED_TEST(PoseTest, TransformPoint) { |
| using quat_t = typename TestFixture::quat_t; |
| using vec3_t = typename TestFixture::vec3_t; |
| using Pose_t = typename TestFixture::Pose_t; |
| using FT = typename TestFixture::FT; |
| |
| const auto tolerance = FT(0.0001); |
| |
| const quat_t pose_rotation( |
| Eigen::AngleAxis<FT>(FT(M_PI / 2.0), vec3_t(FT(0.0), FT(0.0), FT(1.0)))); |
| const auto pose_position = vec3_t(FT(1.0), FT(1.0), FT(2.0)); |
| |
| const Pose_t test_pose(pose_rotation, pose_position); |
| |
| for (int axis = 0; axis < 3; ++axis) { |
| vec3_t start_position = vec3_t::Zero(); |
| start_position[axis] = FT(1.0); |
| const vec3_t expected_transformed = |
| (pose_rotation * start_position) + pose_position; |
| const vec3_t actual_transformed = test_pose.TransformPoint(start_position); |
| EXPECT_VEC3_NEAR(expected_transformed, actual_transformed, tolerance); |
| } |
| } |
| |
| TYPED_TEST(PoseTest, TransformVector) { |
| using quat_t = typename TestFixture::quat_t; |
| using vec3_t = typename TestFixture::vec3_t; |
| using Pose_t = typename TestFixture::Pose_t; |
| using FT = typename TestFixture::FT; |
| |
| const auto tolerance = FT(0.0001); |
| |
| const quat_t pose_rotation(Eigen::AngleAxis<FT>( |
| FT(M_PI / 6.0), vec3_t(FT(3.0), FT(4.0), FT(5.0)).normalized())); |
| |
| const auto pose_position = vec3_t(FT(500.0), FT(-500.0), FT(300.0)); |
| |
| const Pose_t test_pose(pose_rotation, pose_position); |
| |
| for (int axis = 0; axis < 3; ++axis) { |
| vec3_t start_position = vec3_t::Zero(); |
| start_position[axis] = FT(1.0); |
| const vec3_t expected_rotated = pose_rotation * start_position; |
| const vec3_t actual_rotated = test_pose.Transform(start_position); |
| EXPECT_VEC3_NEAR(expected_rotated, actual_rotated, tolerance); |
| } |
| } |
| |
| TYPED_TEST(PoseTest, Composition) { |
| using quat_t = typename TestFixture::quat_t; |
| using Pose_t = typename TestFixture::Pose_t; |
| using vec3_t = typename TestFixture::vec3_t; |
| using FT = typename TestFixture::FT; |
| |
| const auto tolerance = FT(0.0001); |
| |
| const quat_t first_rotation( |
| Eigen::AngleAxis<FT>(FT(M_PI / 2.0), vec3_t(FT(0.0), FT(0.0), FT(1.0)))); |
| const auto first_offset = vec3_t(FT(-3.0), FT(2.0), FT(-1.0)); |
| const quat_t second_rotation(Eigen::AngleAxis<FT>( |
| FT(M_PI / 3.0), vec3_t(FT(1.0), FT(-1.0), FT(0.0)).normalized())); |
| const auto second_offset = vec3_t(FT(6.0), FT(-7.0), FT(-8.0)); |
| |
| const Pose_t first_pose(first_rotation, first_offset); |
| const Pose_t second_pose(second_rotation, second_offset); |
| |
| const auto combined_pose(second_pose.Compose(first_pose)); |
| |
| for (int axis = 0; axis < 3; ++axis) { |
| vec3_t start_position = vec3_t::Zero(); |
| start_position[axis] = FT(1.0); |
| const vec3_t expected_transformed = |
| second_pose.TransformPoint(first_pose.TransformPoint(start_position)); |
| const vec3_t actual_transformed = |
| combined_pose.TransformPoint(start_position); |
| EXPECT_VEC3_NEAR(expected_transformed, actual_transformed, tolerance); |
| } |
| } |
| |
| TYPED_TEST(PoseTest, Inverse) { |
| using quat_t = typename TestFixture::quat_t; |
| using vec3_t = typename TestFixture::vec3_t; |
| using Pose_t = typename TestFixture::Pose_t; |
| using FT = typename TestFixture::FT; |
| |
| const auto tolerance = FT(0.0001); |
| |
| const quat_t pose_rotation(Eigen::AngleAxis<FT>( |
| FT(M_PI / 2.0), vec3_t(FT(4.0), FT(-2.0), FT(-1.0)).normalized())); |
| const auto pose_position = vec3_t(FT(-1.0), FT(2.0), FT(-4.0)); |
| |
| Pose_t pose(pose_rotation, pose_position); |
| const Pose_t pose_inverse = pose.Inverse(); |
| |
| for (int axis = 0; axis < 3; ++axis) { |
| vec3_t start_position = vec3_t::Zero(); |
| start_position[axis] = FT(1.0); |
| const vec3_t transformed = pose.Transform(start_position); |
| const vec3_t inverted = pose_inverse.Transform(transformed); |
| EXPECT_VEC3_NEAR(start_position, inverted, tolerance); |
| } |
| |
| Pose_t nullified_pose[2] = { |
| pose.Compose(pose_inverse), pose_inverse.Compose(pose), |
| }; |
| |
| for (int i = 0; i < 2; ++i) { |
| EXPECT_QUAT_NEAR(quat_t::Identity(), nullified_pose[i].GetRotation(), |
| tolerance); |
| EXPECT_VEC3_NEAR(vec3_t::Zero(), nullified_pose[i].GetPosition(), |
| tolerance); |
| } |
| } |