transform_interpolation_buffer_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 "Eigen/Core"
20 #include "Eigen/Geometry"
23 #include "gtest/gtest.h"
24 
25 namespace cartographer {
26 namespace transform {
27 namespace {
28 
29 TEST(TransformInterpolationBufferTest, testHas) {
30  TransformInterpolationBuffer buffer;
31  EXPECT_FALSE(buffer.Has(common::FromUniversal(50)));
33  EXPECT_FALSE(buffer.Has(common::FromUniversal(25)));
34  EXPECT_TRUE(buffer.Has(common::FromUniversal(50)));
35  EXPECT_FALSE(buffer.Has(common::FromUniversal(75)));
37  EXPECT_FALSE(buffer.Has(common::FromUniversal(25)));
38  EXPECT_TRUE(buffer.Has(common::FromUniversal(50)));
39  EXPECT_TRUE(buffer.Has(common::FromUniversal(75)));
40  EXPECT_TRUE(buffer.Has(common::FromUniversal(100)));
41  EXPECT_FALSE(buffer.Has(common::FromUniversal(125)));
42  EXPECT_EQ(common::FromUniversal(50), buffer.earliest_time());
43  EXPECT_EQ(common::FromUniversal(100), buffer.latest_time());
44 }
45 
46 TEST(TransformInterpolationBufferTest, testLookup) {
47  TransformInterpolationBuffer buffer;
49  // The rotation needs to be relatively small in order for the interpolation to
50  // remain a z-axis rotation.
51  buffer.Push(common::FromUniversal(100),
52  transform::Rigid3d::Translation(Eigen::Vector3d(10., 10., 10.)) *
54  Eigen::AngleAxisd(2., Eigen::Vector3d::UnitZ())));
56  const transform::Rigid3d interpolated = buffer.Lookup(time);
57  EXPECT_THAT(
58  interpolated,
59  IsNearly(transform::Rigid3d::Translation(Eigen::Vector3d(5., 5., 5.)) *
61  Eigen::AngleAxisd(1., Eigen::Vector3d::UnitZ())),
62  1e-6));
63 }
64 
65 TEST(TransformInterpolationBufferTest, testLookupSingleTransform) {
66  TransformInterpolationBuffer buffer;
67  const common::Time time = common::FromUniversal(75);
68  buffer.Push(time, transform::Rigid3d::Identity());
69  const transform::Rigid3d interpolated = buffer.Lookup(time);
70  EXPECT_THAT(interpolated, IsNearly(transform::Rigid3d::Identity(), 1e-6));
71 }
72 
73 } // namespace
74 } // namespace transform
75 } // namespace cartographer
static Rigid3 Rotation(const AngleAxis &angle_axis)
Rigid3< double > Rigid3d
UniversalTimeScaleClock::time_point Time
Definition: time.h:44
Time FromUniversal(const int64 ticks)
Definition: time.cc:34
static time_point time
static Rigid3 Translation(const Vector &vector)
TEST(TrajectoryConnectivityStateTest, UnknownTrajectory)


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