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