transform_test.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
18 
19 #include <random>
20 
23 #include "gtest/gtest.h"
24 
25 namespace cartographer {
26 namespace transform {
27 namespace {
28 
29 TEST(TransformTest, GetAngle) {
30  std::mt19937 rng(42);
31  std::uniform_real_distribution<float> angle_distribution(0.f, M_PI);
32  std::uniform_real_distribution<float> position_distribution(-1.f, 1.f);
33 
34  for (int i = 0; i != 100; ++i) {
35  const float angle = angle_distribution(rng);
36  const float x = position_distribution(rng);
37  const float y = position_distribution(rng);
38  const float z = position_distribution(rng);
39  const Eigen::Vector3f axis = Eigen::Vector3f(x, y, z).normalized();
40  EXPECT_NEAR(angle,
42  Eigen::Vector3f(angle * axis)))),
43  1e-6f);
44  }
45 }
46 
47 TEST(TransformTest, GetYaw) {
48  const auto rotation =
49  Rigid3d::Rotation(Eigen::AngleAxisd(1.2345, Eigen::Vector3d::UnitZ()));
50  EXPECT_NEAR(1.2345, GetYaw(rotation), 1e-9);
51  EXPECT_NEAR(-1.2345, GetYaw(rotation.inverse()), 1e-9);
52 }
53 
54 TEST(TransformTest, GetYawAxisOrdering) {
55  const auto rotation =
56  Rigid3d::Rotation(Eigen::AngleAxisd(1.2345, Eigen::Vector3d::UnitZ()) *
57  Eigen::AngleAxisd(0.4321, Eigen::Vector3d::UnitY()) *
58  Eigen::AngleAxisd(0.6789, Eigen::Vector3d::UnitX()));
59  EXPECT_NEAR(1.2345, GetYaw(rotation), 1e-9);
60 }
61 
62 TEST(TransformTest, Embed3D) {
63  const Rigid2d rigid2d({1., 2.}, 0.);
64  const Rigid3d rigid3d(
65  Rigid3d::Translation(Eigen::Vector3d(1., 2., 3.)) *
66  Rigid3d::Rotation(Eigen::Quaterniond(1., 2., 3., 4.).normalized()));
67  const Rigid3d expected =
68  rigid3d * Rigid3d::Translation(Eigen::Vector3d(1., 2., 0.));
69  EXPECT_THAT(expected, IsNearly(rigid3d * Embed3D(rigid2d), 1e-9));
70 }
71 
72 } // namespace
73 } // namespace transform
74 } // namespace cartographer
Eigen::Quaternion< T > AngleAxisVectorToRotationQuaternion(const Eigen::Matrix< T, 3, 1 > &angle_axis)
Definition: transform.h:86
static Rigid3 Rotation(const AngleAxis &angle_axis)
Rigid3< double > Rigid3d
Rigid3< T > Embed3D(const Rigid2< T > &transform)
Definition: transform.h:110
T GetYaw(const Eigen::Quaternion< T > &rotation)
Definition: transform.h:43
Rigid2< double > Rigid2d
FloatType GetAngle(const Rigid3< FloatType > &transform)
Definition: transform.h:34
static Rigid3 Translation(const Vector &vector)
TEST(TrajectoryConnectivityStateTest, UnknownTrajectory)


cartographer
Author(s): The Cartographer Authors
autogenerated on Mon Feb 28 2022 22:00:59