Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/transform/transform.h"
00018
00019 #include <random>
00020
00021 #include "cartographer/transform/rigid_transform.h"
00022 #include "cartographer/transform/rigid_transform_test_helpers.h"
00023 #include "gtest/gtest.h"
00024
00025 namespace cartographer {
00026 namespace transform {
00027 namespace {
00028
00029 TEST(TransformTest, GetAngle) {
00030 std::mt19937 rng(42);
00031 std::uniform_real_distribution<float> angle_distribution(0.f, M_PI);
00032 std::uniform_real_distribution<float> position_distribution(-1.f, 1.f);
00033
00034 for (int i = 0; i != 100; ++i) {
00035 const float angle = angle_distribution(rng);
00036 const float x = position_distribution(rng);
00037 const float y = position_distribution(rng);
00038 const float z = position_distribution(rng);
00039 const Eigen::Vector3f axis = Eigen::Vector3f(x, y, z).normalized();
00040 EXPECT_NEAR(angle,
00041 GetAngle(Rigid3f::Rotation(AngleAxisVectorToRotationQuaternion(
00042 Eigen::Vector3f(angle * axis)))),
00043 1e-6f);
00044 }
00045 }
00046
00047 TEST(TransformTest, GetYaw) {
00048 const auto rotation =
00049 Rigid3d::Rotation(Eigen::AngleAxisd(1.2345, Eigen::Vector3d::UnitZ()));
00050 EXPECT_NEAR(1.2345, GetYaw(rotation), 1e-9);
00051 EXPECT_NEAR(-1.2345, GetYaw(rotation.inverse()), 1e-9);
00052 }
00053
00054 TEST(TransformTest, GetYawAxisOrdering) {
00055 const auto rotation =
00056 Rigid3d::Rotation(Eigen::AngleAxisd(1.2345, Eigen::Vector3d::UnitZ()) *
00057 Eigen::AngleAxisd(0.4321, Eigen::Vector3d::UnitY()) *
00058 Eigen::AngleAxisd(0.6789, Eigen::Vector3d::UnitX()));
00059 EXPECT_NEAR(1.2345, GetYaw(rotation), 1e-9);
00060 }
00061
00062 TEST(TransformTest, Embed3D) {
00063 const Rigid2d rigid2d({1., 2.}, 0.);
00064 const Rigid3d rigid3d(
00065 Rigid3d::Translation(Eigen::Vector3d(1., 2., 3.)) *
00066 Rigid3d::Rotation(Eigen::Quaterniond(1., 2., 3., 4.).normalized()));
00067 const Rigid3d expected =
00068 rigid3d * Rigid3d::Translation(Eigen::Vector3d(1., 2., 0.));
00069 EXPECT_THAT(expected, IsNearly(rigid3d * Embed3D(rigid2d), 1e-9));
00070 }
00071
00072 }
00073 }
00074 }