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/sensor/landmark_data.h"
00018
00019 #include <cmath>
00020
00021 #include "cartographer/sensor/internal/test_helpers.h"
00022 #include "cartographer/transform/rigid_transform_test_helpers.h"
00023 #include "cartographer/transform/transform.h"
00024 #include "gtest/gtest.h"
00025
00026 namespace cartographer {
00027 namespace sensor {
00028 namespace {
00029
00030 using ::testing::DoubleNear;
00031 using ::testing::Field;
00032
00033 ::testing::Matcher<const LandmarkObservation&> EqualsLandmark(
00034 const LandmarkObservation& expected) {
00035 return ::testing::AllOf(
00036 Field(&LandmarkObservation::id, expected.id),
00037 Field(&LandmarkObservation::landmark_to_tracking_transform,
00038 transform::IsNearly(expected.landmark_to_tracking_transform, 1e-2)),
00039 Field(&LandmarkObservation::translation_weight,
00040 DoubleNear(expected.translation_weight, 0.01)),
00041 Field(&LandmarkObservation::rotation_weight,
00042 DoubleNear(expected.rotation_weight, 0.01)));
00043 }
00044
00045 class LandmarkDataTest : public ::testing::Test {
00046 protected:
00047 LandmarkDataTest()
00048 : observations_(
00049 {{
00050 "ID1",
00051 transform::Rigid3d(Eigen::Vector3d(1., 1., 1.),
00052 Eigen::Quaterniond(1., 1., -1., -1.)),
00053 1.f,
00054 3.f,
00055 },
00056 {
00057 "ID2",
00058 transform::Rigid3d(Eigen::Vector3d(2., 2., 2.),
00059 Eigen::Quaterniond(2., 2., -2., -2.)),
00060 2.f,
00061 4.f,
00062 }}) {}
00063 std::vector<LandmarkObservation> observations_;
00064 };
00065
00066 TEST_F(LandmarkDataTest, LandmarkDataToAndFromProto) {
00067 const auto expected = LandmarkData{common::FromUniversal(50), observations_};
00068 const auto actual = FromProto(ToProto(expected));
00069 EXPECT_EQ(expected.time, actual.time);
00070 EXPECT_THAT(actual.landmark_observations,
00071 ElementsAre(EqualsLandmark(expected.landmark_observations[0]),
00072 EqualsLandmark(expected.landmark_observations[1])));
00073 }
00074
00075 }
00076 }
00077 }