/
/
/
1#ifndef FASTSLAM_PARTICLESET_HPP
2#define FASTSLAM_PARTICLESET_HPP
3
4#include <random>
5#include <fstream>
6#include <cstdlib>
7#include <string>
8#include <vector>
9#include <chrono>
10
11#include "fast_slam/Particle.hpp"
12#include "fast_slam/MeasurementSet.hpp"
13#include "fast_slam/Helper.hpp"
14
15namespace fastslam{
16
17class ParticleSet{
18 public:
19 ParticleSet(uint32_t n_particles = 100, StateVector initial_state = StateVector::Constant(0), StateMatrix state_covariance = 0.1*StateMatrix::Identity());
20 ~ParticleSet();
21
22 void updateParticleSet(std::shared_ptr<MeasurementSet> measurement_set, StateVectorDerivative state_dot, std::chrono::nanoseconds delta_time);
23
24 inline StateVector getLatestPoseEstimate() const {return state_mean_->getPose();}
25 inline StateMatrix getLatestCovarianceEstimate() const {return state_covariance_;}
26
27 std::vector<StateVector> getAllParticlePoseEstimates() const;
28 std::pair<StateVector, std::vector<std::shared_ptr<Landmark>>> getBestParticle();
29
30 inline uint32_t getNParticles() const {return n_particles_;}
31 inline uint32_t getCurrentIteration() const {return iterations_since_start_;}
32
33
34 private:
35 void resample();
36 void estimateDistribution(std::chrono::nanoseconds delta_time);
37
38 std::vector<std::unique_ptr<Particle>> particles_;
39
40 uint32_t n_particles_;
41 StateMatrix state_covariance_;
42
43 // number of interations since time zero
44 uint32_t iterations_since_start_;
45
46 bool loop_is_closed_;
47
48 // instance of path Class to keep track of the estimated mean of the Particle filter!
49 std::unique_ptr<Path> state_mean_;
50};
51
52} //namespace fastslam
53
54#endif
55