/
/
/
1#include "fast_slam/ParticleSet.hpp"
2
3namespace fastslam{
4
5ParticleSet::ParticleSet(uint32_t n_particles, StateVector initial_state, StateMatrix state_covariance):
6 n_particles_(n_particles),
7 state_covariance_(state_covariance),
8 iterations_since_start_(0),
9 loop_is_closed_(false){
10 // makes new path to keep track of the estimated mean of the Particle filter!
11 state_mean_ = std::make_unique<Path>(initial_state, iterations_since_start_);
12
13 particles_.reserve(n_particles_);
14
15 for(size_t i = 0; i<n_particles_; i++){
16 particles_.push_back(std::make_unique<Particle>(initial_state, state_covariance, iterations_since_start_));
17 }
18}
19
20ParticleSet::~ParticleSet(){}
21
22std::vector<StateVector> ParticleSet::getAllParticlePoseEstimates() const{
23 std::vector<StateVector> out;
24 for(const auto &particle : particles_)
25 out.push_back(particle->getPose());
26
27 return out;
28}
29
30std::pair<StateVector, std::vector<std::shared_ptr<Landmark>>> ParticleSet::getBestParticle(){
31 std::pair<StateVector, std::vector<std::shared_ptr<Landmark>>> out;
32 auto max_element = std::max_element(particles_.begin(), particles_.end(), [](std::unique_ptr<Particle> &a, std::unique_ptr<Particle> &b){
33 return a->getWeight() < b->getWeight();
34 });
35 out.first = (*max_element)->getPose();
36
37 out.second = (*max_element)->getMap();
38
39 return out;
40}
41
42void ParticleSet::updateParticleSet(std::shared_ptr<MeasurementSet> measurement_set, StateVectorDerivative state_dot, std::chrono::nanoseconds delta_time){
43
44 iterations_since_start_++;
45
46
47 double weighted_sum = 0;
48 double sum_squared_weights = 0;
49
50 // update each particle
51 for(auto &particle : particles_){
52 particle->updateParticle(measurement_set, &state_dot, iterations_since_start_, delta_time, loop_is_closed_);
53 weighted_sum += particle->getWeight();
54 sum_squared_weights += pow(particle->getWeight(), 2);
55 }
56 double kishs_effective_sample_size = pow(weighted_sum,2)/sum_squared_weights;
57
58
59 estimateDistribution(delta_time);
60
61
62 std::cout << "Kishs effective sample size: " << kishs_effective_sample_size << std::endl;
63
64 // resample only if Kishs effective sample size drops below threshold
65 if(kishs_effective_sample_size < 0.3 * n_particles_)
66 resample();
67
68}
69
70void ParticleSet::resample(){
71 // Resampling wheel
72 cout << "resampling..." << endl;
73
74 vector<std::unique_ptr<Particle>> particles_tmp;
75 particles_tmp.reserve(n_particles_);
76
77 auto max_element = std::max_element(particles_.begin(), particles_.end(), [](std::unique_ptr<Particle> &a, std::unique_ptr<Particle> &b){
78 return a->getWeight() < b->getWeight();
79 });
80
81 if ((*max_element)->getWeight() == 0){
82 cout << "something went wrong, all particles have 0 weight" << endl;
83 return;
84 }
85
86 // generate random index between 1 and number of particles
87 std::default_random_engine generator;
88 std::uniform_int_distribution<uint32_t> distribution_int(0, n_particles_);
89 std::uniform_real_distribution<double> distribution_double(0, 1);
90
91 uint32_t random_index = distribution_int(generator); // random index
92
93 double beta = 0;
94
95
96 for(size_t i = 0; i < n_particles_; i++){
97 // generate random addition to beta
98 double rand = distribution_double(generator);
99 beta += rand * 2 * (*max_element)->getWeight();
100
101 double weight = particles_.at(random_index)->getWeight();
102 while (beta > weight){
103 beta -= weight;
104
105 random_index += 1;
106 if (random_index >= n_particles_){
107 random_index = 1;
108 }
109 weight = particles_.at(random_index)->getWeight();
110 }
111 particles_tmp.push_back(std::make_unique<Particle>(*(particles_.at(random_index))));
112 }
113
114 particles_.clear();
115 for(auto &particle : particles_tmp){
116 particles_.push_back(std::move(particle));
117 }
118
119 cout << "Done resampling!" << endl;
120}
121
122void ParticleSet::estimateDistribution(std::chrono::nanoseconds delta_time){
123 double sum_of_weights = 0;
124
125 // get sum of all weights
126 for(auto &particle : particles_){
127 if(particle->getWeight() != particle->getWeight()){
128 cout << "Err NaN in Particle: " << endl;
129 }
130 sum_of_weights += particle->getWeight();
131 }
132
133
134 // get weighted mean of all state vector
135 StateVector state_mean_estimate = StateVector::Zero();
136 double sum_of_weights_squared = 0;
137
138 for(auto &particle : particles_){
139 auto normalized_weight = particle->getWeight() / sum_of_weights;
140
141 // weighted mean!
142 state_mean_estimate += normalized_weight * particle->getPose();
143 }
144
145
146 // get weighted mean of state covariance
147 StateMatrix state_covariance_estimate = StateMatrix::Zero();
148
149 for(auto &particle : particles_){
150 StateMatrix state_covariance_estimate_particle = StateMatrix::Zero();
151 double normalized_weight = particle->getWeight() / sum_of_weights;
152 auto particle_distance_from_mean = particle->getPose() - state_mean_estimate;
153
154 state_covariance_estimate_particle = particle_distance_from_mean * particle_distance_from_mean.transpose() * normalized_weight;
155
156 state_covariance_estimate += state_covariance_estimate_particle;
157 sum_of_weights_squared += normalized_weight * normalized_weight;
158 }
159
160 // state_covariance_estimate *= 1/(1-sum_of_weights_squared);
161 state_covariance_ = state_covariance_estimate;
162 state_mean_->addPose(state_mean_estimate, iterations_since_start_, delta_time);
163}
164
165} //namespace fastslam
166