/
/
/
1#include <gtest/gtest.h>
2
3#include "fast_slam/Particle.hpp"
4#include "fast_slam/Measurement.hpp"
5#include "fast_slam/MeasurementSet.hpp"
6#include <eigen3/Eigen/Core>
7#include <chrono>
8
9class ParticleTester : public ::testing::Test {
10protected:
11 Eigen::Vector3d initial_pose = {1.0 , 2.0 , 3.0};
12 std::unique_ptr<fastslam::Particle> particle = std::make_unique<fastslam::Particle>();
13
14 virtual void SetUp(){
15
16 }
17};
18
19
20TEST_F(ParticleTester, update_test){
21 Eigen::Vector2d m_m_1 = {1.0, 1.0};
22 Eigen::Vector2d m_m_2 = {-1.0, -1.0};
23 Eigen::Vector2d m_m_3 = {1.0, -1.0};
24 Eigen::Vector2d m_m_4 = {-1.0, 1.0};
25
26 std::shared_ptr<fastslam::Measurement> m_1 = std::make_shared<fastslam::LandmarkXYMeasurement>(1, m_m_1);
27 std::shared_ptr<fastslam::Measurement> m_2 = std::make_shared<fastslam::LandmarkXYMeasurement>(2, m_m_2);
28 std::shared_ptr<fastslam::Measurement> m_3 = std::make_shared<fastslam::LandmarkXYMeasurement>(3, m_m_3);
29 std::shared_ptr<fastslam::Measurement> m_4 = std::make_shared<fastslam::LandmarkXYMeasurement>(4, m_m_4);
30
31 std::shared_ptr<fastslam::MeasurementSet> m_set = std::make_shared<fastslam::MeasurementSet>();
32
33 m_set->addMeasurement(m_1);
34 m_set->addMeasurement(m_2);
35 m_set->addMeasurement(m_3);
36 m_set->addMeasurement(m_4);
37
38 Eigen::Vector3d u = {0.1, 0.1, 0.1};
39 std::chrono::nanoseconds delta_time = std::chrono::nanoseconds(100);
40
41 particle->updateParticle(m_set, &u, 1, delta_time, false);
42
43 auto weight = particle->getWeight();
44 auto pose = particle->getPose();
45 auto map = particle->getMap();
46
47 for(auto lm : map){
48 std::cout << lm->landmark_pose.transpose() << "\n";
49 }
50}
51
52TEST_F(ParticleTester, copy_test){
53 std::unique_ptr<fastslam::Particle> particle_copy = std::make_unique<fastslam::Particle>(*particle);
54}