/
/
/
1#ifndef FASTSLAM_PARTICLE_H
2#define FASTSLAM_PARTICLE_H
3
4#include <iostream>
5#include <fstream>
6#include <cmath>
7#include <boost/random/mersenne_twister.hpp>
8#include <boost/filesystem.hpp>
9
10#include "nabo/nabo.h"
11
12#include "fast_slam/MapTree.hpp"
13#include "fast_slam/PathList.hpp"
14#include "fast_slam/MeasurementSet.hpp"
15#include "fast_slam/Helper.hpp"
16
17namespace fastslam{
18
19
20class Particle{
21public:
22 // Initialize a standard particle with "zero-pose" or custom pose
23 Particle(StateVector initial_state = StateVector::Constant(0), StateMatrix initial_covariance = 0.01*StateMatrix::Identity(), uint32_t iteration = 0);
24
25 // Copy constructer used in case where we need to make a copy of a Particle
26 Particle(const Particle &particle_to_copy);
27 ~Particle();
28
29 inline double getWeight() const {return weight_;}
30 inline StateVector getPose() const {return path_->getPose();}
31 std::vector<std::shared_ptr<Landmark>> getMap();
32
33 void updateParticle(std::shared_ptr<MeasurementSet> measurement_set, StateVectorDerivative* state_dot, uint32_t iteration, std::chrono::nanoseconds delta_time, bool loop_closure);
34
35private:
36 void handleExMeas(MeasurementSet &measurent_set_existing, StateVector &state_proposal, std::vector<int> &id_exiting);
37 void handleNewMeas(MeasurementSet &measurement_set_new, StateVector &state_proposal, std::vector<int> &id_new); // only moved up here to allow new landmarks to be added by Particle Set function
38
39 void associateData(StateVector &state_proposal, std::shared_ptr<MeasurementSet> &measurement_set, MeasurementSet &measurement_set_existing, MeasurementSet &measurement_set_new, std::vector<int> &id_existing, std::vector<int> &id_new);
40
41 StateVector drawSampleFromProposaleDistribution(StateVector &state_propagated, StateMatrix &Fs, StateMatrix &Fw, MeasurementSet &measurement_set_existing, std::vector<int> &id_existing);
42 StateVector motionModel(StateVector &state_old, StateVectorDerivative &state_dot, std::chrono::nanoseconds delta_time);
43 StateVector drawSampleRandomPose(StateVector &state_proposal_mean, StateMatrix &state_proposal_covariance);
44 void calculateImportanceWeight(MeasurementSet &measurment_set_existing, StateVector &state_proposal, StateMatrix &Fw, std::vector<int> &id_existing);
45 StateMatrix calculateFs(StateVector &state_old, StateVectorDerivative &state_dot, std::chrono::nanoseconds &delta_time);
46 StateMatrix calculateFw(StateVector &state_old, StateVectorDerivative &state_dot, std::chrono::nanoseconds &delta_time);
47
48 void KFCholeskyUpdate(Eigen::VectorXd &x, Eigen::MatrixXd &P, Eigen::VectorXd &v, Eigen::MatrixXd &R, Eigen::MatrixXd &H);
49 void updateSpatialIndex() const;
50
51 std::unique_ptr<Path> path_;
52 std::unique_ptr<MapTree> map_;
53
54 // Persistent spatial index for efficient data association
55 mutable std::unique_ptr<Nabo::NNSearchD> spatial_index_;
56 mutable bool spatial_index_dirty_;
57
58 uint32_t current_iteration_;
59 double weight_;
60 double weight_distance_;
61
62 bool loop_is_closed_;
63
64 bool slow_init_ = false;
65 bool use_motion_model_jacobian_ = false;
66 bool always_reset_particle_covariance_ = true;
67 bool use_numerical_stabilized_kalman_filters_ = true;
68 bool force_covariance_symmetry_ = true;
69 bool add_landmarks_after_resampling_ = true;
70
71 // particle covariance
72 StateMatrix particle_covariance_;
73
74 // motion model covariance
75 static StateMatrix motion_model_covariance_;
76
77 static boost::mt19937 rng; // Creating a new random number generator every time could be optimized
78 //rng.seed(static_cast<unsigned int>(time(0)));
79};
80
81}
82
83#endif
84