/
/
/
1
2#include "fast_slam/Particle.hpp"
3
4namespace fastslam{
5
6
7Particle::Particle(StateVector initial_state, StateMatrix initial_covariance, uint32_t iteration):
8 // M(3,0), K(1), kt(0), w_DA(0.), dAindices(), lc(false) // default Constructor definition
9 current_iteration_(iteration),
10 weight_(1),
11 weight_distance_(0.0),
12 loop_is_closed_(false),
13 particle_covariance_(initial_covariance){
14
15 // make path and map
16 path_ = std::make_unique<Path>(initial_state, iteration);
17 map_ = std::make_unique<MapTree>();
18}
19
20Particle::Particle(const Particle &particle_to_copy){
21
22 path_ = std::make_unique<Path>(*(particle_to_copy.path_));
23 map_ = std::make_unique<MapTree>(*(particle_to_copy.map_));
24 weight_ = particle_to_copy.weight_;
25 weight_distance_ = particle_to_copy.weight_distance_;
26 loop_is_closed_ = particle_to_copy.loop_is_closed_;
27
28 slow_init_ = particle_to_copy.slow_init_;
29 use_motion_model_jacobian_ = particle_to_copy.use_motion_model_jacobian_;
30 always_reset_particle_covariance_ = particle_to_copy.always_reset_particle_covariance_;
31 use_numerical_stabilized_kalman_filters_ = particle_to_copy.use_numerical_stabilized_kalman_filters_;
32 force_covariance_symmetry_ = particle_to_copy.force_covariance_symmetry_;
33 add_landmarks_after_resampling_ = particle_to_copy.add_landmarks_after_resampling_;
34
35 particle_covariance_ = particle_to_copy.particle_covariance_;
36}
37
38Particle::~Particle(){}
39
40std::vector<std::shared_ptr<Landmark>> Particle::getMap(){
41 std::vector<std::shared_ptr<Landmark>> out;
42
43 for(uint32_t i = 1; i<=map_->getNLandmarks(); i++)
44 out.push_back(map_->extractLandmarkNodePointer(i));
45
46 return out;
47}
48
49void Particle::updateParticle(std::shared_ptr<MeasurementSet> measurement_set, StateVectorDerivative* state_dot, uint32_t iteration, std::chrono::nanoseconds delta_time, bool loop_closure){
50
51 loop_is_closed_ = loop_closure;
52
53 StateVector state_proposal;
54 StateVector state_old = path_->getPose();
55 current_iteration_ = iteration;
56
57 MeasurementSet measurement_set_new;
58 MeasurementSet measurement_set_existing;
59
60 std::vector<int> id_new;
61 std::vector<int> id_existing;
62
63 StateVector state_propagated = motionModel(state_old, *state_dot, delta_time);
64
65 associateData(state_propagated, measurement_set, measurement_set_existing, measurement_set_new, id_existing, id_new);
66
67
68 StateMatrix Fs, Fw;
69 if(use_motion_model_jacobian_){
70 Fs = calculateFs(state_old, *state_dot, delta_time);
71 Fw = calculateFw(state_old, *state_dot, delta_time);
72 }
73 else{
74 Fs = StateMatrix::Zero();
75 Fw = StateMatrix::Identity();
76 }
77
78
79 state_proposal = drawSampleFromProposaleDistribution(state_propagated, Fs, Fw, measurement_set_existing, id_existing);
80
81 // we are done estimating our pose and add it to the path!
82 path_->addPose(state_proposal, current_iteration_, delta_time);
83
84 calculateImportanceWeight(measurement_set_existing, state_proposal, Fw, id_existing);
85
86 handleExMeas(measurement_set_existing, state_proposal, id_existing);
87
88 handleNewMeas(measurement_set_new, state_proposal, id_new);
89}
90
91void Particle::associateData(StateVector &state_propagated, std::shared_ptr<MeasurementSet> &measurement_set, MeasurementSet &measurement_set_existing, MeasurementSet &measurement_set_new, std::vector<int> &id_existing, std::vector<int> &id_new){
92 // if no landmarks exist in map add all as new
93
94 if(map_->getNLandmarks() == 0){
95 std::unique_ptr<MeasurementSetNode> *curr;
96 if(measurement_set != nullptr)
97 curr = measurement_set->getHeadPointer();
98
99 for(size_t i=1; i<=measurement_set->getNumberOfMeasurements(); i++){
100 id_new.push_back(i);
101 measurement_set_new.addMeasurement((*curr)->measurement);
102 curr = &(*curr)->next_node;
103 }
104 return;
105 }
106
107
108 Eigen::MatrixXd map_landmark_matrix(2,0);
109 Eigen::MatrixXd landmark_calculated(2,0);
110 Eigen::MatrixXd measurement_calculated(2,0);
111
112 //Extract Landmarks from map
113 //TODO: not necessary, integrate kd tree directly
114 for(size_t i=1; i<= map_->getNLandmarks(); i++){
115 auto landmark_ptr = map_->extractLandmarkNodePointer(i);
116 if (landmark_ptr != NULL){
117 map_landmark_matrix.conservativeResize(map_landmark_matrix.rows(), map_landmark_matrix.cols()+1);
118 map_landmark_matrix.col(map_landmark_matrix.cols()-1) = landmark_ptr->landmark_pose;
119 }
120 }
121
122 //Extract measurements from the set.
123 std::unique_ptr<MeasurementSetNode> *curr;
124 if(measurement_set != nullptr)
125 curr = measurement_set->getHeadPointer();
126
127 // if we have a non-emtpy measurement set update the particles, otherwise just do resampling
128 while(*curr != nullptr){
129 landmark_calculated.conservativeResize(landmark_calculated.rows(),landmark_calculated.cols()+1);
130 landmark_calculated.col(landmark_calculated.cols()-1) = (*curr)->measurement->inverseMeasurementModel(state_propagated);
131
132 curr = &(*curr)->next_node;
133 }
134
135
136 Eigen::MatrixXi nn_indices(1, landmark_calculated.cols());
137 Eigen::MatrixXd nn_distances(1, landmark_calculated.cols());
138
139 // run nearest neighbor search
140 Nabo::NNSearchD* nns = Nabo::NNSearchD::createKDTreeLinearHeap(map_landmark_matrix);
141
142 int n_nearest_neighbors = 1;
143 nns->knn(landmark_calculated, nn_indices, nn_distances, n_nearest_neighbors, 0, Nabo::NNSearchD::ALLOW_SELF_MATCH, 1.0);
144
145 delete nns;
146
147 curr = measurement_set->getHeadPointer();
148
149 //add measurements to the sets of existing and new measurements
150 int j = 1;
151 for(int i=0; i<nn_distances.cols(); i++){
152 if(nn_distances(i) < 0.8 || loop_is_closed_){
153 id_existing.push_back(nn_indices(i)+1);
154 measurement_set_existing.addMeasurement((*curr)->measurement);
155 }
156 else{
157 id_new.push_back(map_landmark_matrix.cols()+j);
158 j++;
159 measurement_set_new.addMeasurement((*curr)->measurement);
160 }
161 curr = &(*curr)->next_node;
162 }
163}
164
165void Particle::handleExMeas(MeasurementSet &measurent_set_existing, StateVector &state_proposal, std::vector<int> &id_existing){
166 if (measurent_set_existing.getNumberOfMeasurements() == 0 )
167 return;
168
169
170 for(size_t i = 1; i <= measurent_set_existing.getNumberOfMeasurements(); i++) {
171
172 auto current_measurement = measurent_set_existing.getMeasurement(i);
173
174 auto landmark_old = map_->extractLandmarkNodePointer(id_existing.at(i-1));
175
176 Eigen::VectorXd measurement_calculated = current_measurement->MeasurementModel(state_proposal, landmark_old->landmark_pose);
177 Eigen::VectorXd measurement_difference = current_measurement->getMeasurement() - measurement_calculated;
178
179 Eigen::MatrixXd landmark_jacobian = current_measurement->calculateHl(state_proposal, landmark_old->landmark_pose);
180
181
182 auto landmark_update = std::make_shared<Landmark>();
183 if(use_numerical_stabilized_kalman_filters_){
184 Eigen::VectorXd x = landmark_old->landmark_pose;
185 Eigen::MatrixXd P = landmark_old->landmark_covariance;
186 Eigen::MatrixXd H = landmark_jacobian;
187 Eigen::MatrixXd R = current_measurement->getzCov();
188 Eigen::VectorXd v = measurement_difference;
189
190 KFCholeskyUpdate(x, P, v, R, H);
191
192 landmark_update->landmark_identifier = id_existing.at(i-1);
193 landmark_update->landmark_pose = x;
194 landmark_update->landmark_covariance = P;
195 }
196 else{
197 // innovation covariance
198 Eigen::MatrixXd Zk = current_measurement->getzCov() + landmark_jacobian * landmark_old->landmark_covariance * landmark_jacobian.transpose();
199 // kalman gain
200 Eigen::MatrixXd Kk = landmark_old->landmark_covariance * landmark_jacobian.transpose() * Zk.inverse();
201
202 landmark_update->landmark_identifier = id_existing.at(i-1);
203 landmark_update->landmark_pose += Kk*(current_measurement->getMeasurement() - measurement_calculated);
204
205 Eigen::MatrixXd tmp_matrix = Kk * landmark_jacobian;
206 landmark_update->landmark_covariance = (Eigen::MatrixXd::Identity(tmp_matrix.rows(),tmp_matrix.cols()) - tmp_matrix) * landmark_update->landmark_covariance;
207 }
208
209 //make symmetric
210 if(force_covariance_symmetry_)
211 landmark_update->landmark_covariance = (landmark_update->landmark_covariance + landmark_update->landmark_covariance.transpose()) * 0.5;
212
213
214 if (landmark_update->landmark_pose != landmark_update->landmark_pose) {
215 cout << "landmark update NaN err, ID: " << current_measurement->getC() << endl;
216 }
217
218 map_->correctLandmark(landmark_update);
219 }
220}
221
222
223void Particle::handleNewMeas(MeasurementSet &measurement_set_new, StateVector &state_proposal, std::vector<int> &id_new){
224
225 if (measurement_set_new.getNumberOfMeasurements() == 0 )
226 return;
227
228 for(uint32_t i = 1; i <= measurement_set_new.getNumberOfMeasurements(); i++) {
229
230 auto current_measurement = measurement_set_new.getMeasurement(i);
231
232 auto landmark = std::make_shared<Landmark>();
233 landmark->landmark_identifier = id_new.at(i-1);
234 landmark->landmark_pose = current_measurement->inverseMeasurementModel(state_proposal);
235
236 Eigen::MatrixXd landmark_jacobian = current_measurement->calculateHl(state_proposal, landmark->landmark_pose);
237
238 landmark->landmark_covariance = (landmark_jacobian.transpose() * (current_measurement->getzCov()).inverse() * landmark_jacobian).inverse();
239
240 //make symmetric
241 if(force_covariance_symmetry_)
242 landmark->landmark_covariance = (landmark->landmark_covariance + (landmark->landmark_covariance).transpose()) * 0.5;
243
244 map_->insertLandmark(landmark);
245
246 }
247}
248
249StateVector Particle::drawSampleFromProposaleDistribution(StateVector &state_propagated, StateMatrix &Fs, StateMatrix &Fw, MeasurementSet &measurement_set_existing, std::vector<int> &id_existing){
250
251 StateVector state_proposal = state_propagated; // eq (3.29)
252
253 if(always_reset_particle_covariance_)
254 particle_covariance_ = StateMatrix::Zero();
255
256
257 // sCovPrev should be reset if resampling has occured
258 StateMatrix state_proposal_covariance = Fs.transpose() * particle_covariance_ * Fs +
259 Fw.transpose() * motion_model_covariance_ * Fw;
260
261 if (measurement_set_existing.getNumberOfMeasurements() != 0 && loop_is_closed_){
262
263 for(size_t i = 1; i <= measurement_set_existing.getNumberOfMeasurements(); i++) {
264
265 auto current_measurement = measurement_set_existing.getMeasurement(i);
266 auto landmark_old = map_->extractLandmarkNodePointer(id_existing.at(i-1));
267
268 //resizes automatically due to the "=" operator
269 Eigen::MatrixXd landmark_jacobian = current_measurement->calculateHl(state_propagated, landmark_old->landmark_pose);
270 Eigen::MatrixXd state_jacobian = current_measurement->calculateHs(state_propagated, landmark_old->landmark_pose);
271
272
273 Eigen::MatrixXd Zki = current_measurement->getzCov() + landmark_jacobian * (landmark_old->landmark_covariance) * landmark_jacobian.transpose();
274
275 Eigen::VectorXd measurement_calculated = current_measurement->MeasurementModel(state_propagated, landmark_old->landmark_pose);
276
277 if(use_numerical_stabilized_kalman_filters_){
278 Eigen::VectorXd x = state_proposal;
279 Eigen::MatrixXd P = state_proposal_covariance;
280 Eigen::VectorXd v = (current_measurement->getMeasurement() - measurement_calculated);
281 Eigen::MatrixXd R = Zki;
282 Eigen::MatrixXd H = state_jacobian;
283
284 KFCholeskyUpdate(x, P, v, R, H);
285
286 state_proposal = x;
287 state_proposal_covariance = P;
288 }
289 else{
290 Eigen::MatrixXd Kk;
291 Kk = state_proposal_covariance * state_jacobian.transpose() *
292 (state_jacobian * state_proposal_covariance * state_jacobian.transpose() + Zki).inverse();
293
294 state_proposal_covariance = (state_jacobian.transpose() * Zki.inverse() * state_jacobian + state_proposal_covariance.inverse()).inverse(); // eq (3.30)
295 state_proposal = state_proposal + state_proposal_covariance * state_jacobian.transpose() * Zki.inverse() *
296 (current_measurement->getMeasurement() - measurement_calculated); // eq (3.31)
297 }
298
299 if(force_covariance_symmetry_){
300 //make symmetric
301 state_proposal_covariance = (state_proposal_covariance + state_proposal_covariance.transpose()) * 0.5;
302 }
303 }
304 }
305
306 particle_covariance_ = state_proposal_covariance;
307
308 return drawSampleRandomPose(state_proposal, state_proposal_covariance);
309
310}
311
312
313void Particle::KFCholeskyUpdate(Eigen::VectorXd &x, Eigen::MatrixXd &P, Eigen::VectorXd &v, Eigen::MatrixXd &R, Eigen::MatrixXd &H){
314 Eigen::MatrixXd PHt = P*H.transpose();
315 Eigen::MatrixXd S = H*PHt + R;
316
317 // FIXME: why use conjugate()?
318 //make symmetric
319 if(force_covariance_symmetry_)
320 S = (S+S.transpose()) * 0.5;
321
322 Eigen::MatrixXd SChol = S.llt().matrixU();
323
324 //tri matrix
325 Eigen::MatrixXd SCholInv = SChol.inverse();
326 Eigen::MatrixXd W1 = PHt * SCholInv;
327 Eigen::MatrixXd W = W1 * SCholInv.transpose();
328
329 x = x + W*v;
330 P = P - W1*W1.transpose();
331}
332
333
334//http://moby.ihme.washington.edu/bradbell/mat2cpp/randn.cpp.xml
335Eigen::MatrixXd randn(int m, int n){
336 // use formula 30.3 of Statistical Distributions (3rd ed)
337 // Merran Evans, Nicholas Hastings, and Brian Peacock
338 int urows = m * n + 1;
339 Eigen::VectorXd u(urows);
340
341 //u is a random matrix
342 for (int r=0; r<urows; r++) {
343 // FIXME: better way?
344 u(r) = std::rand() * 1.0/RAND_MAX;
345 }
346
347
348
349 Eigen::MatrixXd x(m,n);
350
351 int i, j, k;
352 double square, amp, angle;
353
354 k = 0;
355 for(i = 0; i < m; i++) {
356 for(j = 0; j < n; j++) {
357 if( k % 2 == 0 ) {
358 square = - 2. * std::log( u(k) );
359 if( square < 0. )
360 square = 0.;
361 amp = sqrt(square);
362 angle = 2. * M_PI * u(k+1);
363 x(i, j) = amp * std::sin( angle );
364 }
365 else
366 x(i, j) = amp * std::cos( angle );
367
368 k++;
369 }
370 }
371
372 return x;
373}
374
375Eigen::MatrixXd rand(int m, int n)
376{
377 Eigen::MatrixXd x(m,n);
378 int i, j;
379 float rand_max = float(RAND_MAX);
380
381 for(i = 0; i < m; i++) {
382 for(j = 0; j < n; j++)
383 x(i, j) = float(std::rand()) / rand_max;
384 }
385 return x;
386}
387
388StateVector Particle::drawSampleRandomPose(StateVector &state_proposal, StateMatrix &state_proposal_covariance){
389 Eigen::MatrixXd Cov = state_proposal_covariance;
390
391 //make symmetric
392 if(force_covariance_symmetry_)
393 Cov = (Cov+Cov.transpose()) * 0.5;
394
395 //choleksy decomposition
396 Eigen::MatrixXd S = Cov.llt().matrixL();
397 Eigen::MatrixXd X = randn(3,1);
398
399 auto sample = S * X + state_proposal;
400 return sample;
401}
402
403// Ts == sample time
404StateVector Particle::motionModel(StateVector &state_old, StateVectorDerivative &state_dot, std::chrono::nanoseconds delta_time) {
405 // s(k) = f(s(k-1),u(k))
406 StateVector state_propagated(state_old);
407
408 double theta = state_old(2);
409 double dt_in_s = std::chrono::duration<double>(delta_time).count();
410
411 // std::cout << dt_in_s << "\n";
412
413 if (dt_in_s > 3) {
414 cout << "Motion model: Sample rate error" << endl;
415 return state_propagated; // error with the sampling time, just return old pose estimate
416 }
417
418 // Kinematic motion model where u=[x_dot, y_dot, yaw_difference] with x_dot and y_dot being in heading frame
419
420 state_propagated(0) += dt_in_s * cos(theta) * state_dot(0);
421 state_propagated(1) += dt_in_s * sin(theta) * state_dot(1);
422 state_propagated(2) += dt_in_s * state_dot(2); // add yaw difference
423
424
425 return state_propagated;
426}
427
428// Motion model Jacobian relative to pose
429StateMatrix Particle::calculateFs(StateVector &state_old, StateVectorDerivative &state_dot, std::chrono::nanoseconds &delta_time){
430 StateMatrix Fs = StateMatrix::Identity();
431 double dt_in_s = std::chrono::duration<double>(delta_time).count();
432
433 Fs(0,2) = dt_in_s * -sin((state_old)(2)) * state_dot(0);
434 Fs(1,2) = dt_in_s * cos((state_old)(2)) * state_dot(1);
435 return Fs;
436}
437
438// Motion model Jacobian relative to motion
439StateMatrix Particle::calculateFw(StateVector &state_old, StateVectorDerivative &state_dot, std::chrono::nanoseconds &delta_time){
440 StateMatrix Fw = StateMatrix::Zero();
441 double dt_in_s = std::chrono::duration<double>(delta_time).count();
442
443 Fw(0,0) = dt_in_s * cos(state_old(2));
444 Fw(1,1) = dt_in_s * sin(state_old(2));
445 Fw(2,2) = dt_in_s;
446 return Fw;
447}
448
449void Particle::calculateImportanceWeight(MeasurementSet &measurment_set_existing, StateVector &state_proposal, StateMatrix &Fw, std::vector<int> &id_existing){
450 if (measurment_set_existing.getNumberOfMeasurements() == 0)
451 return;
452
453 Eigen::MatrixXd weight_covariance;
454 double wi = 1;
455
456 for(uint32_t i = 1; i <= measurment_set_existing.getNumberOfMeasurements(); i++ ) {
457
458 auto current_measurement = measurment_set_existing.getMeasurement(i);
459 auto landmark_old = map_->extractLandmarkNodePointer(id_existing.at(i-1));
460
461 //resizes automatically due to the "=" operator
462 Eigen::MatrixXd landmark_jacobian = current_measurement->calculateHl(state_proposal, landmark_old->landmark_pose);
463 Eigen::MatrixXd state_jacobian = current_measurement->calculateHs(state_proposal, landmark_old->landmark_pose);
464
465 Eigen::VectorXd measurement_calculated = current_measurement->MeasurementModel(state_proposal, landmark_old->landmark_pose);
466
467 Eigen::VectorXd measurement_difference = current_measurement->getMeasurement() - measurement_calculated;
468
469
470 weight_covariance = state_jacobian * Fw * motion_model_covariance_ * Fw.transpose() * state_jacobian.transpose() +
471 landmark_jacobian * landmark_old->landmark_covariance * landmark_jacobian.transpose() + current_measurement->getzCov();
472
473 double exponent = measurement_difference.transpose() * weight_covariance.inverse() * measurement_difference;
474 double determinant = (2 * M_PI * weight_covariance).determinant();
475
476 wi *= exp(-0.5 * exponent) / (sqrt(determinant));
477
478 }
479
480 weight_ = wi;
481}
482
483StateMatrix Particle::motion_model_covariance_ = 0.001*StateMatrix::Identity(); // static variable - has to be declared outside class!
484
485boost::mt19937 Particle::rng; // Creating a new random number generator every time could be optimized
486//rng.seed(static_cast<unsigned int>(time(0)));
487
488
489} //namespace fastslam
490