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