cartographer: transform_interpolation_buffer

Go to the documentation of this file.
00001 /*
00002  * Copyright 2016 The Cartographer Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *      http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include "cartographer/transform/transform_interpolation_buffer.h"
00018 
00019 #include "Eigen/Core"
00020 #include "Eigen/Geometry"
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(TransformInterpolationBufferTest, testHas) {
00030   TransformInterpolationBuffer buffer;
00031   EXPECT_FALSE(buffer.Has(common::FromUniversal(50)));
00032   buffer.Push(common::FromUniversal(50), transform::Rigid3d::Identity());
00033   EXPECT_FALSE(buffer.Has(common::FromUniversal(25)));
00034   EXPECT_TRUE(buffer.Has(common::FromUniversal(50)));
00035   EXPECT_FALSE(buffer.Has(common::FromUniversal(75)));
00036   buffer.Push(common::FromUniversal(100), transform::Rigid3d::Identity());
00037   EXPECT_FALSE(buffer.Has(common::FromUniversal(25)));
00038   EXPECT_TRUE(buffer.Has(common::FromUniversal(50)));
00039   EXPECT_TRUE(buffer.Has(common::FromUniversal(75)));
00040   EXPECT_TRUE(buffer.Has(common::FromUniversal(100)));
00041   EXPECT_FALSE(buffer.Has(common::FromUniversal(125)));
00042   EXPECT_EQ(common::FromUniversal(50), buffer.earliest_time());
00043   EXPECT_EQ(common::FromUniversal(100), buffer.latest_time());
00044 }
00045 
00046 TEST(TransformInterpolationBufferTest, testLookup) {
00047   TransformInterpolationBuffer buffer;
00048   buffer.Push(common::FromUniversal(50), transform::Rigid3d::Identity());
00049   // The rotation needs to be relatively small in order for the interpolation to
00050   // remain a z-axis rotation.
00051   buffer.Push(common::FromUniversal(100),
00052               transform::Rigid3d::Translation(Eigen::Vector3d(10., 10., 10.)) *
00053                   transform::Rigid3d::Rotation(
00054                       Eigen::AngleAxisd(2., Eigen::Vector3d::UnitZ())));
00055   const common::Time time = common::FromUniversal(75);
00056   const transform::Rigid3d interpolated = buffer.Lookup(time);
00057   EXPECT_THAT(
00058       interpolated,
00059       IsNearly(transform::Rigid3d::Translation(Eigen::Vector3d(5., 5., 5.)) *
00060                    transform::Rigid3d::Rotation(
00061                        Eigen::AngleAxisd(1., Eigen::Vector3d::UnitZ())),
00062                1e-6));
00063 }
00064 
00065 TEST(TransformInterpolationBufferTest, testLookupSingleTransform) {
00066   TransformInterpolationBuffer buffer;
00067   const common::Time time = common::FromUniversal(75);
00068   buffer.Push(time, transform::Rigid3d::Identity());
00069   const transform::Rigid3d interpolated = buffer.Lookup(time);
00070   EXPECT_THAT(interpolated, IsNearly(transform::Rigid3d::Identity(), 1e-6));
00071 }
00072 
00073 }  // namespace
00074 }  // namespace transform
00075 }  // namespace cartographer