fast_slam

1#include <gtest/gtest.h>
2
3#include "fast_slam/Measurement.hpp"
4#include "fast_slam/MeasurementSet.hpp"
5#include <eigen3/Eigen/Core>
6#include <chrono>
7
8class MeasurementTester : public ::testing::Test {
9protected:
10
11    Eigen::Vector3d initial_pose = {1.0 , 2.0 , 3.0};
12    std::unique_ptr<fastslam::MeasurementSet> m_set = std::make_unique<fastslam::MeasurementSet>();
13
14    virtual void SetUp(){
15        Eigen::Vector3d measurement_3d;
16        Eigen::Vector2d measurement_2d;
17        measurement_3d << 1.0, 2.0, 0.0;
18        measurement_2d << 1.0, 2.0;
19
20        std::shared_ptr<fastslam::Measurement> got_measurement = std::make_shared<fastslam::GOTMeasurement>(1, measurement_3d);
21        std::shared_ptr<fastslam::Measurement> xy_measurement = std::make_shared<fastslam::LandmarkXYMeasurement>(2, measurement_2d);
22        std::shared_ptr<fastslam::Measurement> xy_yaw_measurement = std::make_shared<fastslam::LandmarkXYYawMeasurement>(3, measurement_3d);
23
24        m_set->addMeasurement(got_measurement);
25        m_set->addMeasurement(xy_measurement);
26        m_set->addMeasurement(xy_yaw_measurement);
27    }
28};
29
30TEST_F(MeasurementTester, getMeasurement_test){
31    auto m_test = m_set->getMeasurement(1)->getMeasurement();
32
33    EXPECT_DOUBLE_EQ(m_test(0), 1.0);
34    EXPECT_DOUBLE_EQ(m_test(1), 2.0);
35    EXPECT_DOUBLE_EQ(m_test(2), 0.0);
36}
37
38TEST_F(MeasurementTester, nMeasurements_test){
39    auto n_measurements = m_set->getNumberOfMeasurements();
40
41    EXPECT_EQ(n_measurements, 3);
42}
43
44TEST_F(MeasurementTester, GOTMeasurement_test){
45    auto got_m = m_set->getMeasurement(1);
46
47    Eigen::Vector3d pose;
48    pose << 1.0, 2.0, 3.0;
49}
50
51TEST_F(MeasurementTester, XYMeasurement_test){
52    auto xy_m = m_set->getMeasurement(2);
53    auto measurement = xy_m->getMeasurement();
54
55    Eigen::Vector2d landmark;
56    Eigen::Vector3d pose;
57    pose << 1.0, 2.0, M_PI / 2;
58    landmark << -1.0, 3.0;
59
60    auto inverse_m = xy_m->inverseMeasurementModel(pose);
61    auto measurement_m = xy_m->MeasurementModel(pose, landmark);
62
63
64    EXPECT_DOUBLE_EQ(landmark(0), inverse_m(0));
65    EXPECT_DOUBLE_EQ(landmark(1), inverse_m(1));
66
67    EXPECT_DOUBLE_EQ(measurement(0), measurement_m(0));
68    EXPECT_DOUBLE_EQ(measurement(1), measurement_m(1));
69
70    auto Hl = xy_m->calculateHl(pose, landmark);
71}
72
73TEST_F(MeasurementTester, XYYawMeasurement_test){
74    auto xy_yaw_m = m_set->getMeasurement(3);
75    auto measurement = xy_yaw_m->getMeasurement();
76
77    Eigen::Vector3d landmark;
78    Eigen::Vector3d pose;
79    pose << 1.0, 2.0, M_PI / 2;
80    landmark << -1.0, 3.0, M_PI / 2;
81
82    auto inverse_m = xy_yaw_m->inverseMeasurementModel(pose);
83    auto measurement_m = xy_yaw_m->MeasurementModel(pose, landmark);
84
85
86    EXPECT_DOUBLE_EQ(landmark(0), inverse_m(0));
87    EXPECT_DOUBLE_EQ(landmark(1), inverse_m(1));
88    EXPECT_DOUBLE_EQ(landmark(2), inverse_m(2));
89
90    EXPECT_DOUBLE_EQ(measurement(0), measurement_m(0));
91    EXPECT_DOUBLE_EQ(measurement(1), measurement_m(1));
92    EXPECT_DOUBLE_EQ(measurement(2), measurement_m(2));
93}