cartographer: motion_filter_test.cc Source File

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/mapping/internal/motion_filter.h"
00018 
00019 #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
00020 #include "gmock/gmock.h"
00021 
00022 namespace cartographer {
00023 namespace mapping {
00024 namespace {
00025 
00026 class MotionFilterTest : public ::testing::Test {
00027  protected:
00028   MotionFilterTest() {
00029     auto parameter_dictionary = common::MakeDictionary(
00030         "return {"
00031         "max_time_seconds = 0.5, "
00032         "max_distance_meters = 0.2, "
00033         "max_angle_radians = 2., "
00034         "}");
00035     options_ = CreateMotionFilterOptions(parameter_dictionary.get());
00036   }
00037 
00038   common::Time SecondsSinceEpoch(int seconds) {
00039     return common::FromUniversal(seconds * 10000000);
00040   }
00041 
00042   proto::MotionFilterOptions options_;
00043 };
00044 
00045 TEST_F(MotionFilterTest, NotInitialized) {
00046   MotionFilter motion_filter(options_);
00047   EXPECT_FALSE(
00048       motion_filter.IsSimilar(common::Time(), transform::Rigid3d::Identity()));
00049 }
00050 
00051 TEST_F(MotionFilterTest, NoChange) {
00052   MotionFilter motion_filter(options_);
00053   EXPECT_FALSE(motion_filter.IsSimilar(SecondsSinceEpoch(42),
00054                                        transform::Rigid3d::Identity()));
00055   EXPECT_TRUE(motion_filter.IsSimilar(SecondsSinceEpoch(42),
00056                                       transform::Rigid3d::Identity()));
00057 }
00058 
00059 TEST_F(MotionFilterTest, TimeElapsed) {
00060   MotionFilter motion_filter(options_);
00061   EXPECT_FALSE(motion_filter.IsSimilar(SecondsSinceEpoch(42),
00062                                        transform::Rigid3d::Identity()));
00063   EXPECT_FALSE(motion_filter.IsSimilar(SecondsSinceEpoch(43),
00064                                        transform::Rigid3d::Identity()));
00065   EXPECT_TRUE(motion_filter.IsSimilar(SecondsSinceEpoch(43),
00066                                       transform::Rigid3d::Identity()));
00067 }
00068 
00069 TEST_F(MotionFilterTest, LinearMotion) {
00070   MotionFilter motion_filter(options_);
00071   EXPECT_FALSE(motion_filter.IsSimilar(SecondsSinceEpoch(42),
00072                                        transform::Rigid3d::Identity()));
00073   EXPECT_FALSE(motion_filter.IsSimilar(
00074       SecondsSinceEpoch(42),
00075       transform::Rigid3d::Translation(Eigen::Vector3d(0.3, 0., 0.))));
00076   EXPECT_TRUE(motion_filter.IsSimilar(
00077       SecondsSinceEpoch(42),
00078       transform::Rigid3d::Translation(Eigen::Vector3d(0.45, 0., 0.))));
00079   EXPECT_FALSE(motion_filter.IsSimilar(
00080       SecondsSinceEpoch(42),
00081       transform::Rigid3d::Translation(Eigen::Vector3d(0.6, 0., 0.))));
00082   EXPECT_TRUE(motion_filter.IsSimilar(
00083       SecondsSinceEpoch(42),
00084       transform::Rigid3d::Translation(Eigen::Vector3d(0.6, 0.15, 0.))));
00085 }
00086 
00087 TEST_F(MotionFilterTest, RotationalMotion) {
00088   MotionFilter motion_filter(options_);
00089   EXPECT_FALSE(motion_filter.IsSimilar(SecondsSinceEpoch(42),
00090                                        transform::Rigid3d::Identity()));
00091   EXPECT_TRUE(motion_filter.IsSimilar(
00092       SecondsSinceEpoch(42), transform::Rigid3d::Rotation(Eigen::AngleAxisd(
00093                                  1.9, Eigen::Vector3d::UnitY()))));
00094   EXPECT_FALSE(motion_filter.IsSimilar(
00095       SecondsSinceEpoch(42), transform::Rigid3d::Rotation(Eigen::AngleAxisd(
00096                                  2.1, Eigen::Vector3d::UnitY()))));
00097   EXPECT_TRUE(motion_filter.IsSimilar(
00098       SecondsSinceEpoch(42), transform::Rigid3d::Rotation(Eigen::AngleAxisd(
00099                                  4., Eigen::Vector3d::UnitY()))));
00100   EXPECT_FALSE(motion_filter.IsSimilar(
00101       SecondsSinceEpoch(42), transform::Rigid3d::Rotation(Eigen::AngleAxisd(
00102                                  5.9, Eigen::Vector3d::UnitY()))));
00103   EXPECT_TRUE(motion_filter.IsSimilar(SecondsSinceEpoch(42),
00104                                       transform::Rigid3d::Identity()));
00105 }
00106 
00107 }  // namespace
00108 }  // namespace mapping
00109 }  // namespace cartographer