BayesFilter.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
29 #include "rtabmap/core/Memory.h"
30 #include "rtabmap/core/Signature.h"
32 #include <iostream>
33 #include <set>
34 #if __cplusplus >= 201103L
35 #include <unordered_map>
36 #include <unordered_set>
37 #endif
38 
40 
41 namespace rtabmap {
42 
44  _virtualPlacePrior(Parameters::defaultBayesVirtualPlacePriorThr()),
45  _fullPredictionUpdate(Parameters::defaultBayesFullPredictionUpdate()),
46  _totalPredictionLCValues(0.0f),
47  _predictionEpsilon(0.0f)
48 {
49  this->setPredictionLC(Parameters::defaultBayesPredictionLC());
50  this->parseParameters(parameters);
51 }
52 
54 }
55 
57 {
58  ParametersMap::const_iterator iter;
59  if((iter=parameters.find(Parameters::kBayesPredictionLC())) != parameters.end())
60  {
61  this->setPredictionLC((*iter).second);
62  }
63  Parameters::parse(parameters, Parameters::kBayesVirtualPlacePriorThr(), _virtualPlacePrior);
64  Parameters::parse(parameters, Parameters::kBayesFullPredictionUpdate(), _fullPredictionUpdate);
65 
67 }
68 
69 // format = {Virtual place, Loop closure, level1, level2, l3, l4...}
70 void BayesFilter::setPredictionLC(const std::string & prediction)
71 {
72  std::list<std::string> strValues = uSplit(prediction, ' ');
73  if(strValues.size() < 2)
74  {
75  UERROR("The number of values < 2 (prediction=\"%s\")", prediction.c_str());
76  }
77  else
78  {
79  std::vector<double> tmpValues(strValues.size());
80  int i=0;
81  bool valid = true;
82  for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
83  {
84  tmpValues[i] = uStr2Float((*iter).c_str());
85  //UINFO("%d=%e", i, tmpValues[i]);
86  if(tmpValues[i] < 0.0 || tmpValues[i]>1.0)
87  {
88  valid = false;
89  break;
90  }
91  ++i;
92  }
93 
94  if(!valid)
95  {
96  UERROR("The prediction is not valid (values must be between >0 && <=1) prediction=\"%s\"", prediction.c_str());
97  }
98  else
99  {
100  _predictionLC = tmpValues;
101  }
102  }
104  for(unsigned int j=0; j<_predictionLC.size(); ++j)
105  {
107  if(j==0 || _predictionLC[j] < _predictionEpsilon)
108  {
110  }
111  }
112  if(!_predictionLC.empty())
113  {
114  UDEBUG("predictionEpsilon = %f", _predictionEpsilon);
115  }
116 }
117 
118 const std::vector<double> & BayesFilter::getPredictionLC() const
119 {
120  // {Vp, Lc, l1, l2, l3, l4...}
121  return _predictionLC;
122 }
123 
125 {
126  std::string values;
127  for(unsigned int i=0; i<_predictionLC.size(); ++i)
128  {
129  values.append(uNumber2Str(_predictionLC[i]));
130  if(i+1 < _predictionLC.size())
131  {
132  values.append(" ");
133  }
134  }
135  return values;
136 }
137 
139 {
140  _posterior.clear();
141  _prediction = cv::Mat();
142  _neighborsIndex.clear();
143 }
144 
145 const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory, const std::map<int, float> & likelihood)
146 {
147  ULOGGER_DEBUG("");
148 
149  if(!memory)
150  {
151  ULOGGER_ERROR("Memory is Null!");
152  return _posterior;
153  }
154 
155  if(!likelihood.size())
156  {
157  ULOGGER_ERROR("likelihood is empty!");
158  return _posterior;
159  }
160 
161  if(_predictionLC.size() < 2)
162  {
163  ULOGGER_ERROR("Prediction is not valid!");
164  return _posterior;
165  }
166 
167  UTimer timer;
168  timer.start();
169 
170  cv::Mat prior;
171  cv::Mat posterior;
172 
173  float sum = 0;
174  int j=0;
175  // Recursive Bayes estimation...
176  // STEP 1 - Prediction : Prior*lastPosterior
177  _prediction = this->generatePrediction(memory, uKeys(likelihood));
178 
179  UDEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), _prediction.rows, _prediction.cols);
180  //std::cout << "Prediction=" << _prediction << std::endl;
181 
182  // Adjust the last posterior if some images were
183  // reactivated or removed from the working memory
184  posterior = cv::Mat(likelihood.size(), 1, CV_32FC1);
185  this->updatePosterior(memory, uKeys(likelihood));
186  j=0;
187  for(std::map<int, float>::const_iterator i=_posterior.begin(); i!= _posterior.end(); ++i)
188  {
189  ((float*)posterior.data)[j++] = (*i).second;
190  }
191  ULOGGER_DEBUG("STEP1-update posterior=%fs, posterior=%d, _posterior size=%d", posterior.rows, _posterior.size());
192  //std::cout << "LastPosterior=" << posterior << std::endl;
193 
194  // Multiply prediction matrix with the last posterior
195  // (m,m) X (m,1) = (m,1)
196  prior = _prediction * posterior;
197  ULOGGER_DEBUG("STEP1-matrix mult time=%fs", timer.ticks());
198  //std::cout << "ResultingPrior=" << prior << std::endl;
199 
200  ULOGGER_DEBUG("STEP1-matrix mult time=%fs", timer.ticks());
201  std::vector<float> likelihoodValues = uValues(likelihood);
202  //std::cout << "Likelihood=" << cv::Mat(likelihoodValues) << std::endl;
203 
204  // STEP 2 - Update : Multiply with observations (likelihood)
205  j=0;
206  for(std::map<int, float>::const_iterator i=likelihood.begin(); i!= likelihood.end(); ++i)
207  {
208  std::map<int, float>::iterator p =_posterior.find((*i).first);
209  if(p!= _posterior.end())
210  {
211  (*p).second = (*i).second * ((float*)prior.data)[j++];
212  sum+=(*p).second;
213  }
214  else
215  {
216  ULOGGER_ERROR("Problem1! can't find id=%d", (*i).first);
217  }
218  }
219  ULOGGER_DEBUG("STEP2-likelihood time=%fs", timer.ticks());
220  //std::cout << "Posterior (before normalization)=" << _posterior << std::endl;
221 
222  // Normalize
223  ULOGGER_DEBUG("sum=%f", sum);
224  if(sum != 0)
225  {
226  for(std::map<int, float>::iterator i=_posterior.begin(); i!= _posterior.end(); ++i)
227  {
228  (*i).second /= sum;
229  }
230  }
231  ULOGGER_DEBUG("normalize time=%fs", timer.ticks());
232  //std::cout << "Posterior=" << _posterior << std::endl;
233 
234  return _posterior;
235 }
236 
237 float addNeighborProb(cv::Mat & prediction,
238  unsigned int col,
239  const std::map<int, int> & neighbors,
240  const std::vector<double> & predictionLC,
241 #if __cplusplus >= 201103L
242  const std::unordered_map<int, int> & idToIndex
243 #else
244  const std::map<int, int> & idToIndex
245 #endif
246  )
247 {
248  UASSERT(col < (unsigned int)prediction.cols &&
249  col < (unsigned int)prediction.rows);
250 
251  float sum=0.0f;
252  float * dataPtr = (float*)prediction.data;
253  for(std::map<int, int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
254  {
255  if(iter->first>=0)
256  {
257 #if __cplusplus >= 201103L
258  std::unordered_map<int, int>::const_iterator jter = idToIndex.find(iter->first);
259 #else
260  std::map<int, int>::const_iterator jter = idToIndex.find(iter->first);
261 #endif
262  if(jter != idToIndex.end())
263  {
264  UASSERT((iter->second+1) < (int)predictionLC.size());
265  sum += dataPtr[col + jter->second*prediction.cols] = predictionLC[iter->second+1];
266  }
267  }
268  }
269  return sum;
270 }
271 
272 
273 cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector<int> & ids)
274 {
275  std::vector<int> oldIds = uKeys(_posterior);
276  if(oldIds.size() == ids.size() &&
277  memcmp(oldIds.data(), ids.data(), oldIds.size()*sizeof(int)) == 0)
278  {
279  return _prediction;
280  }
281 
282  if(!_fullPredictionUpdate && !_prediction.empty())
283  {
284  return updatePrediction(_prediction, memory, oldIds, ids);
285  }
286  UDEBUG("");
287 
288  UASSERT(memory &&
289  _predictionLC.size() >= 2 &&
290  ids.size());
291 
292  UTimer timer;
293  timer.start();
294  UTimer timerGlobal;
295  timerGlobal.start();
296 
297 #if __cplusplus >= 201103L
298  std::unordered_map<int,int> idToIndexMap;
299  idToIndexMap.reserve(ids.size());
300 #else
301  std::map<int,int> idToIndexMap;
302 #endif
303  for(unsigned int i=0; i<ids.size(); ++i)
304  {
305  if(ids[i]>0)
306  {
307  idToIndexMap[ids[i]] = i;
308  }
309  }
310 
311 
312  //int rows = prediction.rows;
313  cv::Mat prediction = cv::Mat::zeros(ids.size(), ids.size(), CV_32FC1);
314  int cols = prediction.cols;
315 
316  // Each prior is a column vector
317  UDEBUG("_predictionLC.size()=%d",_predictionLC.size());
318  std::set<int> idsDone;
319 
320  for(unsigned int i=0; i<ids.size(); ++i)
321  {
322  if(idsDone.find(ids[i]) == idsDone.end())
323  {
324  if(ids[i] > 0)
325  {
326  // Set high values (gaussians curves) to loop closure neighbors
327 
328  // ADD prob for each neighbors
329  std::map<int, int> neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0, false, false, true, true);
330 
332  {
333  uInsert(_neighborsIndex, std::make_pair(ids[i], neighbors));
334  }
335 
336  std::list<int> idsLoopMargin;
337  //filter neighbors in STM
338  for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
339  {
340  if(memory->isInSTM(iter->first))
341  {
342  neighbors.erase(iter++);
343  }
344  else
345  {
346  if(iter->second == 0 && idToIndexMap.find(iter->first)!=idToIndexMap.end())
347  {
348  idsLoopMargin.push_back(iter->first);
349  }
350  ++iter;
351  }
352  }
353 
354  // should at least have 1 id in idsMarginLoop
355  if(idsLoopMargin.size() == 0)
356  {
357  UFATAL("No 0 margin neighbor for signature %d !?!?", ids[i]);
358  }
359 
360  // same neighbor tree for loop signatures (margin = 0)
361  for(std::list<int>::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter)
362  {
364  {
365  uInsert(_neighborsIndex, std::make_pair(*iter, neighbors));
366  }
367 
368  float sum = 0.0f; // sum values added
369  int index = idToIndexMap.at(*iter);
370  sum += addNeighborProb(prediction, index, neighbors, _predictionLC, idToIndexMap);
371  idsDone.insert(*iter);
372  this->normalize(prediction, index, sum, ids[0]<0);
373  }
374  }
375  else
376  {
377  // Set the virtual place prior
378  if(_virtualPlacePrior > 0)
379  {
380  if(cols>1) // The first must be the virtual place
381  {
382  ((float*)prediction.data)[i] = _virtualPlacePrior;
383  float val = (1.0-_virtualPlacePrior)/(cols-1);
384  for(int j=1; j<cols; j++)
385  {
386  ((float*)prediction.data)[i + j*cols] = val;
387  }
388  }
389  else if(cols>0)
390  {
391  ((float*)prediction.data)[i] = 1;
392  }
393  }
394  else
395  {
396  // Only for some tests...
397  // when _virtualPlacePrior=0, set all priors to the same value
398  if(cols>1)
399  {
400  float val = 1.0/cols;
401  for(int j=0; j<cols; j++)
402  {
403  ((float*)prediction.data)[i + j*cols] = val;
404  }
405  }
406  else if(cols>0)
407  {
408  ((float*)prediction.data)[i] = 1;
409  }
410  }
411  }
412  }
413  }
414 
415  ULOGGER_DEBUG("time = %fs", timerGlobal.ticks());
416 
417  return prediction;
418 }
419 
420 unsigned long BayesFilter::getMemoryUsed() const
421 {
422  long memoryUsage = sizeof(BayesFilter);
423  memoryUsage += _posterior.size() * (sizeof(float)+sizeof(int)+sizeof(std::map<int, float>::iterator)) + sizeof(std::map<int, float>);
424  memoryUsage += _prediction.total() * _prediction.elemSize();
425  memoryUsage += _predictionLC.size() * sizeof(double);
426  memoryUsage += _neighborsIndex.size() * (sizeof(int)+sizeof(std::map<int, int>)+sizeof(std::map<int, std::map<int, int> >::iterator)) + sizeof(std::map<int, std::map<int, int> >);
427  for(std::map<int, std::map<int, int> >::const_iterator iter=_neighborsIndex.begin(); iter!=_neighborsIndex.end(); ++iter)
428  {
429  memoryUsage += iter->second.size() * (sizeof(int)*2+sizeof(std::map<int, int>::iterator)) + sizeof(std::map<int, int>);
430  }
431  return memoryUsage;
432 }
433 
434 void BayesFilter::normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const
435 {
436  UASSERT(index < (unsigned int)prediction.rows && index < (unsigned int)prediction.cols);
437 
438  int cols = prediction.cols;
439  // ADD values of not found neighbors to loop closure
440  if(addedProbabilitiesSum < _totalPredictionLCValues-_predictionLC[0])
441  {
442  float delta = _totalPredictionLCValues-_predictionLC[0]-addedProbabilitiesSum;
443  ((float*)prediction.data)[index + index*cols] += delta;
444  addedProbabilitiesSum+=delta;
445  }
446 
447  float allOtherPlacesValue = 0;
449  {
450  allOtherPlacesValue = 1.0f - _totalPredictionLCValues;
451  }
452 
453  // Set all loop events to small values according to the model
454  if(allOtherPlacesValue > 0 && cols>1)
455  {
456  float value = allOtherPlacesValue / float(cols - 1);
457  for(int j=virtualPlaceUsed?1:0; j<cols; ++j)
458  {
459  if(((float*)prediction.data)[index + j*cols] == 0)
460  {
461  ((float*)prediction.data)[index + j*cols] = value;
462  addedProbabilitiesSum += ((float*)prediction.data)[index + j*cols];
463  }
464  }
465  }
466 
467  //normalize this row
468  float maxNorm = 1 - (virtualPlaceUsed?_predictionLC[0]:0); // 1 - virtual place probability
469  if(addedProbabilitiesSum<maxNorm-0.0001 || addedProbabilitiesSum>maxNorm+0.0001)
470  {
471  for(int j=virtualPlaceUsed?1:0; j<cols; ++j)
472  {
473  ((float*)prediction.data)[index + j*cols] *= maxNorm / addedProbabilitiesSum;
474  if(((float*)prediction.data)[index + j*cols] < _predictionEpsilon)
475  {
476  ((float*)prediction.data)[index + j*cols] = 0.0f;
477  }
478  }
479  addedProbabilitiesSum = maxNorm;
480  }
481 
482  // ADD virtual place prob
483  if(virtualPlaceUsed)
484  {
485  ((float*)prediction.data)[index] = _predictionLC[0];
486  addedProbabilitiesSum += ((float*)prediction.data)[index];
487  }
488 
489  //debug
490  //for(int j=0; j<cols; ++j)
491  //{
492  // ULOGGER_DEBUG("test col=%d = %f", i, prediction.data.fl[i + j*cols]);
493  //}
494 
495  if(addedProbabilitiesSum<0.99 || addedProbabilitiesSum > 1.01)
496  {
497  UWARN("Prediction is not normalized sum=%f", addedProbabilitiesSum);
498  }
499 }
500 
501 cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
502  const Memory * memory,
503  const std::vector<int> & oldIds,
504  const std::vector<int> & newIds)
505 {
506  UTimer timer;
507  UDEBUG("");
508 
509  UASSERT(memory &&
510  oldIds.size() &&
511  newIds.size() &&
512  oldIds.size() == (unsigned int)oldPrediction.cols &&
513  oldIds.size() == (unsigned int)oldPrediction.rows);
514 
515  cv::Mat prediction = cv::Mat::zeros(newIds.size(), newIds.size(), CV_32FC1);
516  UDEBUG("time creating prediction = %fs", timer.restart());
517 
518  // Create id to index maps
519 #if __cplusplus >= 201103L
520  std::unordered_set<int> oldIdsSet(oldIds.begin(), oldIds.end());
521 #else
522  std::set<int> oldIdsSet(oldIds.begin(), oldIds.end());
523 #endif
524  UDEBUG("time creating old ids set = %fs", timer.restart());
525 
526 #if __cplusplus >= 201103L
527  std::unordered_map<int,int> newIdToIndexMap;
528  newIdToIndexMap.reserve(newIds.size());
529 #else
530  std::map<int,int> newIdToIndexMap;
531 #endif
532  for(unsigned int i=0; i<newIds.size(); ++i)
533  {
534  if(newIds[i]>0)
535  {
536  newIdToIndexMap[newIds[i]] = i;
537  }
538  }
539 
540  UDEBUG("time creating id-index vector (size=%d oldIds.back()=%d newIds.back()=%d) = %fs", (int)newIdToIndexMap.size(), oldIds.back(), newIds.back(), timer.restart());
541 
542  //Get removed ids
543  std::set<int> removedIds;
544  for(unsigned int i=0; i<oldIds.size(); ++i)
545  {
546  if(oldIds[i] > 0 && newIdToIndexMap.find(oldIds[i]) == newIdToIndexMap.end())
547  {
548  removedIds.insert(removedIds.end(), oldIds[i]);
549  _neighborsIndex.erase(oldIds[i]);
550  UDEBUG("removed id=%d at oldIndex=%d", oldIds[i], i);
551  }
552  }
553  UDEBUG("time getting removed ids = %fs", timer.restart());
554 
555  bool oldAllCopied = false;
556  if(removedIds.empty() &&
557  newIds.size() > oldIds.size() &&
558  memcmp(oldIds.data(), newIds.data(), oldIds.size()*sizeof(int)) == 0)
559  {
560  oldPrediction.copyTo(cv::Mat(prediction, cv::Range(0, oldPrediction.rows), cv::Range(0, oldPrediction.cols)));
561  oldAllCopied = true;
562  UDEBUG("Copied all old prediction: = %fs", timer.ticks());
563  }
564 
565  int added = 0;
566  // get ids to update
567  std::set<int> idsToUpdate;
568  for(unsigned int i=0; i<oldIds.size() || i<newIds.size(); ++i)
569  {
570  if(i<oldIds.size())
571  {
572  if(removedIds.find(oldIds[i]) != removedIds.end())
573  {
574  unsigned int cols = oldPrediction.cols;
575  int count = 0;
576  for(unsigned int j=0; j<cols; ++j)
577  {
578  if(j!=i && removedIds.find(oldIds[j]) == removedIds.end())
579  {
580  //UDEBUG("to update id=%d from id=%d removed (value=%f)", oldIds[j], oldIds[i], ((const float *)oldPrediction.data)[i + j*cols]);
581  idsToUpdate.insert(oldIds[j]);
582  ++count;
583  }
584  }
585  UDEBUG("From removed id %d, %d neighbors to update.", oldIds[i], count);
586  }
587  }
588  if(i<newIds.size() && oldIdsSet.find(newIds[i]) == oldIdsSet.end())
589  {
590  if(_neighborsIndex.find(newIds[i]) == _neighborsIndex.end())
591  {
592  std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0, false, false, true, true);
593 
594  for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
595  {
596  std::map<int, std::map<int, int> >::iterator jter = _neighborsIndex.find(iter->first);
597  if(jter != _neighborsIndex.end())
598  {
599  uInsert(jter->second, std::make_pair(newIds[i], iter->second));
600  }
601  }
602  _neighborsIndex.insert(std::make_pair(newIds[i], neighbors));
603  }
604  const std::map<int, int> & neighbors = _neighborsIndex.at(newIds[i]);
605  //std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0, false, false, true, true);
606 
607  float sum = addNeighborProb(prediction, i, neighbors, _predictionLC, newIdToIndexMap);
608  this->normalize(prediction, i, sum, newIds[0]<0);
609 
610  ++added;
611  int count = 0;
612  for(std::map<int,int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
613  {
614  if(oldIdsSet.find(iter->first)!=oldIdsSet.end() &&
615  removedIds.find(iter->first) == removedIds.end())
616  {
617  idsToUpdate.insert(iter->first);
618  ++count;
619  }
620  }
621  UDEBUG("From added id %d, %d neighbors to update.", newIds[i], count);
622  }
623  }
624  UDEBUG("time getting %d ids to update = %fs", idsToUpdate.size(), timer.restart());
625 
626  UTimer t1;
627  double e0=0,e1=0, e2=0, e3=0, e4=0;
628  // update modified/added ids
629  int modified = 0;
630  for(std::set<int>::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter)
631  {
632  int id = *iter;
633  if(id > 0)
634  {
635  int index = newIdToIndexMap.at(id);
636 
637  e0 = t1.ticks();
638  std::map<int, std::map<int, int> >::iterator kter = _neighborsIndex.find(id);
639  UASSERT_MSG(kter != _neighborsIndex.end(), uFormat("Did not find %d (current index size=%d)", id, (int)_neighborsIndex.size()).c_str());
640  const std::map<int, int> & neighbors = kter->second;
641  //std::map<int, int> neighbors = memory->getNeighborsId(id, _predictionLC.size()-1, 0, false, false, true, true);
642  e1+=t1.ticks();
643 
644  float sum = addNeighborProb(prediction, index, neighbors, _predictionLC, newIdToIndexMap);
645  e3+=t1.ticks();
646 
647  this->normalize(prediction, index, sum, newIds[0]<0);
648  ++modified;
649  e4+=t1.ticks();
650  }
651  }
652  UDEBUG("time updating modified/added %d ids = %fs (e0=%f e1=%f e2=%f e3=%f e4=%f)", idsToUpdate.size(), timer.restart(), e0, e1, e2, e3, e4);
653 
654  int copied = 0;
655  if(!oldAllCopied)
656  {
657  //UDEBUG("oldIds.size()=%d, oldPrediction.cols=%d, oldPrediction.rows=%d", oldIds.size(), oldPrediction.cols, oldPrediction.rows);
658  //UDEBUG("newIdToIndexMap.size()=%d, prediction.cols=%d, prediction.rows=%d", newIdToIndexMap.size(), prediction.cols, prediction.rows);
659  // copy not changed probabilities
660  for(unsigned int i=0; i<oldIds.size(); ++i)
661  {
662  if(oldIds[i]>0 && removedIds.find(oldIds[i]) == removedIds.end() && idsToUpdate.find(oldIds[i]) == idsToUpdate.end())
663  {
664  for(int j=0; j<oldPrediction.cols; ++j)
665  {
666  if(oldIds[j]>0 && removedIds.find(oldIds[j]) == removedIds.end())
667  {
668  //UDEBUG("i=%d, j=%d", i, j);
669  //UDEBUG("oldIds[i]=%d, oldIds[j]=%d", oldIds[i], oldIds[j]);
670  //UDEBUG("newIdToIndexMap.at(oldIds[i])=%d", newIdToIndexMap.at(oldIds[i]));
671  //UDEBUG("newIdToIndexMap.at(oldIds[j])=%d", newIdToIndexMap.at(oldIds[j]));
672  float v = ((const float *)oldPrediction.data)[i + j*oldPrediction.cols];
673  int ii = newIdToIndexMap.at(oldIds[i]);
674  int jj = newIdToIndexMap.at(oldIds[j]);
675  ((float *)prediction.data)[ii + jj*prediction.cols] = v;
676  //if(ii != jj)
677  //{
678  // ((float *)prediction.data)[jj + ii*prediction.cols] = v;
679  //}
680  }
681  }
682  ++copied;
683  }
684  }
685  UDEBUG("time copying = %fs", timer.restart());
686  }
687 
688  //update virtual place
689  if(newIds[0] < 0)
690  {
691  if(prediction.cols>1) // The first must be the virtual place
692  {
693  ((float*)prediction.data)[0] = _virtualPlacePrior;
694  float val = (1.0-_virtualPlacePrior)/(prediction.cols-1);
695  for(int j=1; j<prediction.cols; j++)
696  {
697  ((float*)prediction.data)[j*prediction.cols] = val;
698  ((float*)prediction.data)[j] = _predictionLC[0];
699  }
700  }
701  else if(prediction.cols>0)
702  {
703  ((float*)prediction.data)[0] = 1;
704  }
705  }
706  UDEBUG("time updating virtual place = %fs", timer.restart());
707 
708  UDEBUG("Modified=%d, Added=%d, Copied=%d", modified, added, copied);
709  return prediction;
710 }
711 
712 void BayesFilter::updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds)
713 {
714  ULOGGER_DEBUG("");
715  std::map<int, float> newPosterior;
716  for(std::vector<int>::const_iterator i=likelihoodIds.begin(); i != likelihoodIds.end(); ++i)
717  {
718  std::map<int, float>::iterator post = _posterior.find(*i);
719  if(post == _posterior.end())
720  {
721  if(_posterior.size() == 0)
722  {
723  newPosterior.insert(std::pair<int, float>(*i, 1));
724  }
725  else
726  {
727  newPosterior.insert(std::pair<int, float>(*i, 0));
728  }
729  }
730  else
731  {
732  newPosterior.insert(std::pair<int, float>((*post).first, (*post).second));
733  }
734  }
735  _posterior = newPosterior;
736 }
737 
738 } // namespace rtabmap
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:497
std::map< int, int > getNeighborsId(int signatureId, int maxGraphDepth, int maxCheckedInDatabase=-1, bool incrementMarginOnLoop=false, bool ignoreLoopIds=false, bool ignoreIntermediateNodes=false, bool ignoreLocalSpaceLoopIds=false, const std::set< int > &nodesSet=std::set< int >(), double *dbAccessTime=0) const
Definition: Memory.cpp:1404
std::string getPredictionLCStr() const
double restart()
Definition: UTimer.h:94
float _totalPredictionLCValues
Definition: BayesFilter.h:80
Definition: UTimer.h:46
std::vector< K > uKeys(const std::multimap< K, V > &mm)
Definition: UStl.h:67
f
virtual void parseParameters(const ParametersMap &parameters)
Definition: BayesFilter.cpp:56
float UTILITE_EXP uStr2Float(const std::string &str)
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
#define UFATAL(...)
const std::vector< double > & getPredictionLC() const
prior
float addNeighborProb(cv::Mat &prediction, unsigned int col, const std::map< int, int > &neighbors, const std::vector< double > &predictionLC, const std::map< int, int > &idToIndex)
cv::Mat generatePrediction(const Memory *memory, const std::vector< int > &ids)
void normalize(cv::Mat &prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const
#define UASSERT(condition)
std::list< std::string > uSplit(const std::string &str, char separator=' ')
Definition: UStl.h:566
#define ULOGGER_DEBUG(...)
Definition: ULogger.h:53
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
cv::Mat updatePrediction(const cv::Mat &oldPrediction, const Memory *memory, const std::vector< int > &oldIds, const std::vector< int > &newIds)
std::map< int, std::map< int, int > > _neighborsIndex
Definition: BayesFilter.h:82
void start()
Definition: UTimer.cpp:87
BayesFilter(const ParametersMap &parameters=ParametersMap())
Definition: BayesFilter.cpp:43
std::vector< V > uValues(const std::multimap< K, V > &mm)
Definition: UStl.h:100
std::map< int, float > _posterior
Definition: BayesFilter.h:75
#define UDEBUG(...)
void updatePosterior(const Memory *memory, const std::vector< int > &likelihoodIds)
#define UERROR(...)
std::vector< double > _predictionLC
Definition: BayesFilter.h:78
#define UWARN(...)
double ticks()
Definition: UTimer.cpp:117
unsigned long getMemoryUsed() const
#define ULOGGER_ERROR(...)
Definition: ULogger.h:56
std::string UTILITE_EXP uFormat(const char *fmt,...)
std::string UTILITE_EXP uNumber2Str(unsigned int number)
Definition: UConversion.cpp:91
bool isInSTM(int signatureId) const
Definition: Memory.h:215
const std::map< int, float > & computePosterior(const Memory *memory, const std::map< int, float > &likelihood)
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:443
void setPredictionLC(const std::string &prediction)
Definition: BayesFilter.cpp:70


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:27