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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:30