RtabmapThread.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/Rtabmap.h"
32 #include "rtabmap/core/Camera.h"
36 #include "rtabmap/core/Memory.h"
37 
40 #include <rtabmap/utilite/UStl.h>
41 #include <rtabmap/utilite/UTimer.h>
43 
44 namespace rtabmap {
45 
47  _dataBufferMaxSize(Parameters::defaultRtabmapImageBufferSize()),
48  _rate(Parameters::defaultRtabmapDetectionRate()),
49  _createIntermediateNodes(Parameters::defaultRtabmapCreateIntermediateNodes()),
50  _frameRateTimer(new UTimer()),
51  _previousStamp(0.0),
52  _rtabmap(rtabmap),
53  _paused(false),
54  lastPose_(Transform::getIdentity())
55 
56 {
57  UASSERT(rtabmap != 0);
58 }
59 
61 {
63 
64  close(true);
65 
66  delete _frameRateTimer;
67 }
68 
69 void RtabmapThread::pushNewState(State newState, const RtabmapEventCmd & cmdEvent)
70 {
71  ULOGGER_DEBUG("to %d", newState);
72 
73  _stateMutex.lock();
74  {
75  _state.push(newState);
76  _stateParam.push(cmdEvent);
77  }
79 
81 }
82 
84 {
85  _dataMutex.lock();
86  {
87  _dataBuffer.clear();
88  _newMapEvents.clear();
90  covariance_ = cv::Mat();
91  _previousStamp = 0;
92  }
94 
96  {
97  _userData = cv::Mat();
98  }
100 }
101 
103 {
104  UASSERT(rate >= 0.0f);
105  _rate = rate;
106 }
107 
108 void RtabmapThread::setDataBufferSize(unsigned int size)
109 {
111 }
112 
114 {
115  _createIntermediateNodes = enabled;
116 }
117 
118 void RtabmapThread::close(bool databaseSaved, const std::string & ouputDatabasePath)
119 {
120  this->join(true);
121  if(_rtabmap)
122  {
123  _rtabmap->close(databaseSaved, ouputDatabasePath);
124  delete _rtabmap;
125  _rtabmap = 0;
126  }
127 }
128 
129 void RtabmapThread::publishMap(bool optimized, bool full, bool graphOnly) const
130 {
131  UDEBUG("optimized=%s, full=%s, graphOnly=%s", optimized?"true":"false", full?"true":"false", graphOnly?"true":"false");
132  if(_rtabmap)
133  {
134  std::map<int, Signature> signatures;
135  std::map<int, Transform> poses;
136  std::multimap<int, Link> constraints;
137  std::map<int, int> mapIds;
138  std::map<int, double> stamps;
139  std::map<int, std::string> labels;
140  std::map<int, std::vector<unsigned char> > userDatas;
141 
142  _rtabmap->getGraph(poses,
143  constraints,
144  optimized,
145  full,
146  &signatures,
147  !graphOnly,
148  !graphOnly,
149  !graphOnly,
150  !graphOnly);
151 
152  this->post(new RtabmapEvent3DMap(
153  signatures,
154  poses,
155  constraints));
156  }
157  else
158  {
159  UERROR("Rtabmap is null!");
160  }
161 }
162 
164 {
166  if(_rtabmap == 0)
167  {
168  UERROR("Cannot start rtabmap thread if no rtabmap object is set! Stopping the thread...");
169  this->kill();
170  }
171 }
172 
174 {
175  this->clearBufferedData();
176  // this will post the newData semaphore
178 }
179 
181 {
184 
185  _stateMutex.lock();
186  {
187  if(!_state.empty() && !_stateParam.empty())
188  {
189  state = _state.front();
190  _state.pop();
191  cmdEvent = _stateParam.front();
192  _stateParam.pop();
193  }
194  }
196 
197  int id = 0;
198  cv::Mat userData;
199  UTimer timer;
200  std::string str;
201  RtabmapEventCmd::Cmd cmd = cmdEvent.getCmd();
202  switch(state)
203  {
204  case kStateDetecting:
205  this->process();
206  break;
208  if(cmd == RtabmapEventCmd::kCmdInit)
209  {
210  ULOGGER_DEBUG("CMD_INIT");
211  Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
212  Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapDetectionRate(), _rate);
213  Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
214  UASSERT(_rate >= 0.0f);
215  _rtabmap->init(cmdEvent.getParameters(), cmdEvent.value1().toStr());
216  }
217  else if(cmd == RtabmapEventCmd::kCmdClose)
218  {
219  ULOGGER_DEBUG("CMD_CLOSE");
220  if(_dataBuffer.size())
221  {
222  UWARN("Closing... %d data still buffered! They will be cleared.", (int)_dataBuffer.size());
223  this->clearBufferedData();
224  }
225  _rtabmap->close(cmdEvent.value1().toBool(), cmdEvent.value2().toStr());
226  }
228  {
229  ULOGGER_DEBUG("CMD_UPDATE_PARAMS");
230  Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
231  Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapDetectionRate(), _rate);
232  Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
233  UASSERT(_rate >= 0.0f);
235  break;
236  }
238  {
239  ULOGGER_DEBUG("CMD_RESET_MEMORY");
241  this->clearBufferedData();
242  }
243  else if(cmd == RtabmapEventCmd::kCmdDumpMemory)
244  {
245  ULOGGER_DEBUG("CMD_DUMP_MEMORY");
246  _rtabmap->dumpData();
247  }
249  {
250  ULOGGER_DEBUG("CMD_DUMP_PREDICTION");
252  }
254  {
255  ULOGGER_DEBUG("CMD_GENERATE_DOT_GRAPH");
257  cmdEvent.value2().toStr(),
258  cmdEvent.value1().toBool()?0:cmdEvent.value3().toInt(),
259  cmdEvent.value1().toBool()?0:cmdEvent.value4().toInt());
260  }
262  {
263  ULOGGER_DEBUG("CMD_EXPORT_POSES");
265  cmdEvent.value3().toStr(),
266  cmdEvent.value2().toBool(),
267  cmdEvent.value1().toBool(),
268  cmdEvent.value4().toInt());
269 
270  }
272  {
273  ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
274  this->clearBufferedData();
275  }
276  else if(cmd == RtabmapEventCmd::kCmdPublish3DMap)
277  {
278  ULOGGER_DEBUG("CMD_PUBLISH_MAP");
279  this->publishMap(
280  cmdEvent.value2().toBool(),
281  cmdEvent.value1().toBool(),
282  cmdEvent.value3().toBool());
283  }
285  {
286  ULOGGER_DEBUG("CMD_TRIGGER_NEW_MAP");
288  }
289  else if(cmd == RtabmapEventCmd::kCmdPause)
290  {
291  ULOGGER_DEBUG("CMD_PAUSE");
292  _paused = !_paused;
293  }
294  else if(cmd == RtabmapEventCmd::kCmdGoal)
295  {
296  ULOGGER_DEBUG("CMD_GOAL");
297  if(cmdEvent.value1().isStr() && !cmdEvent.value1().toStr().empty() && _rtabmap->getMemory())
298  {
299  id = _rtabmap->getMemory()->getSignatureIdByLabel(cmdEvent.value1().toStr());
300  if(id <= 0)
301  {
302  UERROR("Failed to find a node with label \"%s\".", cmdEvent.value1().toStr().c_str());
303  }
304  }
305  else if(cmdEvent.value1().isInt() || cmdEvent.value1().isUInt())
306  {
307  id = cmdEvent.value1().toInt();
308  }
309 
310  if(id < 0)
311  {
312  UERROR("Failed to set a goal. ID (%d) should be positive > 0", id);
313  }
314  timer.start();
315  if(id > 0 && !_rtabmap->computePath(id, true))
316  {
317  UERROR("Failed to compute a path to goal %d.", id);
318  }
319  this->post(new RtabmapGlobalPathEvent(
320  id,
321  cmdEvent.value1().isStr()?cmdEvent.value1().toStr():"",
322  _rtabmap->getPath(),
323  timer.elapsed()));
324  break;
325  }
327  {
328  ULOGGER_DEBUG("CMD_CANCEL_GOAL");
329  _rtabmap->clearPath(0);
330  }
331  else if(cmd == RtabmapEventCmd::kCmdLabel)
332  {
333  ULOGGER_DEBUG("CMD_LABEL");
334  if(!_rtabmap->labelLocation(cmdEvent.value2().toInt(), cmdEvent.value1().toStr()))
335  {
336  this->post(new RtabmapLabelErrorEvent(cmdEvent.value2().toInt(), cmdEvent.value1().toStr()));
337  }
338  }
340  {
341  ULOGGER_DEBUG("CMD_REMOVE_LABEL");
342  id = _rtabmap->getMemory()->getSignatureIdByLabel(cmdEvent.value1().toStr(), true);
343  if(id <= 0 || !_rtabmap->labelLocation(id, ""))
344  {
345  this->post(new RtabmapLabelErrorEvent(id, cmdEvent.value1().toStr()));
346  }
347  }
349  {
350  ULOGGER_DEBUG("CMD_REPUBLISH_DATA");
352  }
353  else
354  {
355  UWARN("Cmd %d unknown!", cmd);
356  }
357  break;
358  default:
359  UFATAL("Invalid state !?!?");
360  break;
361  }
362 }
363 
364 
366 {
367  if(this->isRunning())
368  {
369  if(event->getClassName().compare("IMUEvent") == 0)
370  {
371  // IMU events are published at high frequency, early exit
372  return false;
373  }
374  else if(event->getClassName().compare("SensorEvent") == 0)
375  {
376  UDEBUG("SensorEvent");
377  SensorEvent * e = (SensorEvent*)event;
378  if(e->getCode() == SensorEvent::kCodeData)
379  {
380  if (_rtabmap->isRGBDMode())
381  {
382  if (!e->info().odomPose.isNull() || (_rtabmap->getMemory() && !_rtabmap->getMemory()->isIncremental()))
383  {
384  OdometryInfo infoCov;
385  infoCov.reg.covariance = e->info().odomCovariance;
386  if(e->info().odomVelocity.size() == 6)
387  {
388  infoCov.transform = Transform(
389  e->info().odomVelocity[0],
390  e->info().odomVelocity[1],
391  e->info().odomVelocity[2],
392  e->info().odomVelocity[3],
393  e->info().odomVelocity[4],
394  e->info().odomVelocity[5]);
395  infoCov.interval = 1.0;
396  }
397  this->addData(OdometryEvent(e->data(), e->info().odomPose, infoCov));
398  }
399  else
400  {
401  lastPose_.setNull();
402  }
403  }
404  else
405  {
406  OdometryInfo infoCov;
407  infoCov.reg.covariance = e->info().odomCovariance;
408  this->addData(OdometryEvent(e->data(), e->info().odomPose, infoCov));
409  }
410 
411  }
412  }
413  else if(event->getClassName().compare("OdometryEvent") == 0)
414  {
415  UDEBUG("OdometryEvent");
416  OdometryEvent * e = (OdometryEvent*)event;
417  if(!e->pose().isNull() || (_rtabmap->getMemory() && !_rtabmap->getMemory()->isIncremental()))
418  {
419  this->addData(*e);
420  }
421  else
422  {
423  lastPose_.setNull();
424  }
425  }
426  else if(event->getClassName().compare("UserDataEvent") == 0)
427  {
428  if(!_paused)
429  {
430  UDEBUG("UserDataEvent");
431  bool updated = false;
432  UserDataEvent * e = (UserDataEvent*)event;
434  if(!e->data().empty())
435  {
436  updated = !_userData.empty();
437  _userData = e->data();
438  }
440  if(updated)
441  {
442  UWARN("New user data received before the last one was processed... replacing "
443  "user data with this new one. Note that UserDataEvent should be used only "
444  "if the rate of UserDataEvent is lower than RTAB-Map's detection rate (%f Hz).", _rate);
445  }
446  }
447  }
448  else if(event->getClassName().compare("RtabmapEventCmd") == 0)
449  {
450  RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;
451  pushNewState(kStateProcessCommand, *rtabmapEvent);
452  }
453  else if(event->getClassName().compare("ParamEvent") == 0)
454  {
455  ULOGGER_DEBUG("changing parameters");
457  }
458  }
459  return false;
460 }
461 
462 //============================================================
463 // MAIN LOOP
464 //============================================================
466 {
468  if(_state.empty() && getData(data))
469  {
470  if(_rtabmap->getMemory())
471  {
472  bool wasPlanning = _rtabmap->getPath().size()>0;
473  if(_rtabmap->process(data.data(), data.pose(), data.covariance(), data.velocity()))
474  {
476  stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size());
477  ULOGGER_DEBUG("posting statistics_ event...");
478  this->post(new RtabmapEvent(stats));
479 
480  if(wasPlanning && _rtabmap->getPath().size() == 0)
481  {
482  // Goal reached or failed
484  }
485  }
486  }
487  else
488  {
489  UERROR("RTAB-Map is not initialized! Ignoring received data...");
490  }
491  }
492 }
493 
494 void RtabmapThread::addData(const OdometryEvent & odomEvent)
495 {
496  if(!_paused)
497  {
498  UScopeMutex scopeMutex(_dataMutex);
499 
500  bool ignoreFrame = false;
501  if(_rate>0.0f)
502  {
503  if((_previousStamp>=0.0 && odomEvent.data().stamp()>_previousStamp && odomEvent.data().stamp() - _previousStamp < 1.0f/_rate) ||
504  ((_previousStamp<=0.0 || odomEvent.data().stamp()<=_previousStamp) && _frameRateTimer->getElapsedTime() < 1.0f/_rate))
505  {
506  ignoreFrame = true;
507  }
508  }
509  if(!lastPose_.isIdentity() &&
510  (odomEvent.pose().isIdentity() ||
511  odomEvent.info().reg.covariance.at<double>(0,0)>=9999))
512  {
513  if(odomEvent.pose().isIdentity())
514  {
515  UWARN("Odometry is reset (identity pose detected). Increment map id! (stamp=%fs)", odomEvent.data().stamp());
516  }
517  else
518  {
519  UWARN("Odometry is reset (high variance (%f >=9999 detected, stamp=%fs). Increment map id!", odomEvent.info().reg.covariance.at<double>(0,0), odomEvent.data().stamp());
520  }
521  _newMapEvents.push_back(odomEvent.data().stamp());
522  covariance_ = cv::Mat();
523  }
524 
525  if(uIsFinite(odomEvent.info().reg.covariance.at<double>(0,0)) &&
526  odomEvent.info().reg.covariance.at<double>(0,0) != 1.0 &&
527  odomEvent.info().reg.covariance.at<double>(0,0)>0.0)
528  {
529  // Use largest covariance error (to be independent of the odometry frame rate)
530  if(covariance_.empty() || odomEvent.info().reg.covariance.at<double>(0,0) > covariance_.at<double>(0,0))
531  {
532  covariance_ = odomEvent.info().reg.covariance;
533  }
534  }
535 
536  if(ignoreFrame && !_createIntermediateNodes)
537  {
538  return;
539  }
540  else if(!ignoreFrame)
541  {
543  _previousStamp = odomEvent.data().stamp();
544  }
545 
546  lastPose_ = odomEvent.pose();
547 
548  bool notify = true;
549 
550  if(covariance_.empty())
551  {
552  covariance_ = cv::Mat::eye(6,6,CV_64FC1);
553  }
554  OdometryInfo odomInfo = odomEvent.info().copyWithoutData();
555  odomInfo.reg.covariance = covariance_;
556  if(ignoreFrame)
557  {
558  // set negative id so rtabmap will detect it as an intermediate node
559  SensorData tmp = odomEvent.data();
560  tmp.setId(-1);
561  tmp.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
562  _dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), odomInfo));
563  }
564  else
565  {
566  _dataBuffer.push_back(OdometryEvent(odomEvent.data(), odomEvent.pose(), odomInfo));
567  }
568  UINFO("Added data %d", odomEvent.data().id());
569 
570  covariance_ = cv::Mat();
571  while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
572  {
573  if(_rate > 0.0f)
574  {
575  ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
576  }
577  _dataBuffer.pop_front();
578  notify = false;
579  }
580 
581  if(notify)
582  {
584  }
585  }
586 }
587 
589 {
590  ULOGGER_DEBUG("");
591 
592  ULOGGER_INFO("waiting for data");
594  ULOGGER_INFO("wake-up");
595 
596  bool dataFilled = false;
597  bool triggerNewMap = false;
598  _dataMutex.lock();
599  {
600  if(_state.empty() && !_dataBuffer.empty())
601  {
602  data = _dataBuffer.front();
603  _dataBuffer.pop_front();
604 
606  {
607  if(!_userData.empty())
608  {
609  data.data().setUserData(_userData);
610  _userData = cv::Mat();
611  }
612  }
614 
615  while(_newMapEvents.size() && _newMapEvents.front() <= data.data().stamp())
616  {
617  UWARN("Triggering new map %f<=%f...", _newMapEvents.front() , data.data().stamp());
618  triggerNewMap = true;
619  _newMapEvents.pop_front();
620  }
621 
622  dataFilled = true;
623  }
624  }
625  _dataMutex.unlock();
626 
627  if(triggerNewMap)
628  {
630  }
631 
632  return dataFilled;
633 }
634 
635 } /* namespace rtabmap */
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::OdometryEvent
Definition: OdometryEvent.h:39
UMutex::lock
int lock() const
Definition: UMutex.h:87
USemaphore::acquire
bool acquire(int n=1, int ms=0)
Definition: USemaphore.h:101
rtabmap::Memory::isIncremental
bool isIncremental() const
Definition: Memory.h:213
UINFO
#define UINFO(...)
UEventsManager::removeHandler
static void removeHandler(UEventsHandler *handler)
Definition: UEventsManager.cpp:41
rtabmap::Statistics
Definition: Statistics.h:53
rtabmap::RtabmapThread::handleEvent
virtual bool handleEvent(UEvent *anEvent)
Definition: RtabmapThread.cpp:365
rtabmap::RtabmapEventCmd
Definition: RtabmapEvent.h:57
ParamEvent.h
UVariant::isInt
bool isInt() const
Definition: UVariant.h:84
rtabmap::OdometryEvent::pose
const Transform & pose() const
Definition: OdometryEvent.h:71
rtabmap::RtabmapEventCmd::getCmd
Cmd getCmd() const
Definition: RtabmapEvent.h:115
rtabmap::RtabmapThread::setDataBufferSize
void setDataBufferSize(unsigned int bufferSize)
Definition: RtabmapThread.cpp:108
timer
rtabmap::RtabmapThread::~RtabmapThread
virtual ~RtabmapThread()
Definition: RtabmapThread.cpp:60
rtabmap::ParamEvent
Definition: ParamEvent.h:41
UVariant::toIntArray
std::vector< int > toIntArray(bool *ok=0) const
Definition: UVariant.cpp:783
rtabmap::Rtabmap::getMemory
const Memory * getMemory() const
Definition: Rtabmap.h:147
rtabmap::RtabmapEventCmd::kCmdPublish3DMap
@ kCmdPublish3DMap
Definition: RtabmapEvent.h:72
UMutex::unlock
int unlock() const
Definition: UMutex.h:113
rtabmap::RtabmapEventCmd::kCmdGenerateDOTGraph
@ kCmdGenerateDOTGraph
Definition: RtabmapEvent.h:69
rtabmap::RtabmapThread::_paused
bool _paused
Definition: RtabmapThread.h:113
size
Index size
rtabmap::RtabmapEventCmd::value3
const UVariant & value3() const
Definition: RtabmapEvent.h:119
rtabmap::RtabmapEventCmd::kCmdExportPoses
@ kCmdExportPoses
Definition: RtabmapEvent.h:70
UThread::join
void join(bool killFirst=false)
Definition: UThread.cpp:85
updated
updated
rtabmap::RtabmapEventCmd::kCmdTriggerNewMap
@ kCmdTriggerNewMap
Definition: RtabmapEvent.h:74
rtabmap::RtabmapLabelErrorEvent
Definition: RtabmapEvent.h:237
rtabmap::RtabmapThread::_rtabmap
Rtabmap * _rtabmap
Definition: RtabmapThread.h:112
state
RecoveryProgressState state
Definition: tools/Recovery/main.cpp:56
rtabmap::Memory::getSignatureIdByLabel
int getSignatureIdByLabel(const std::string &label, bool lookInDatabase=true) const
Definition: Memory.cpp:2593
rtabmap::RtabmapThread::_dataAdded
USemaphore _dataAdded
Definition: RtabmapThread.h:105
rtabmap::RtabmapThread::close
void close(bool databaseSaved, const std::string &databasePath="")
Definition: RtabmapThread.cpp:118
rtabmap::Rtabmap::getGraph
void getGraph(std::map< int, Transform > &poses, std::multimap< int, Link > &constraints, bool optimized, bool global, std::map< int, Signature > *signatures=0, bool withImages=false, bool withScan=false, bool withUserData=false, bool withGrid=false, bool withWords=true, bool withGlobalDescriptors=true) const
Definition: Rtabmap.cpp:5479
rtabmap::RtabmapEvent
Definition: RtabmapEvent.h:42
rtabmap::SensorData::stamp
double stamp() const
Definition: SensorData.h:176
rtabmap::RtabmapEventCmd::kCmdDumpMemory
@ kCmdDumpMemory
Definition: RtabmapEvent.h:67
rtabmap::RtabmapEventCmd::kCmdUndef
@ kCmdUndef
Definition: RtabmapEvent.h:62
UTimer::start
void start()
Definition: UTimer.cpp:87
rtabmap::RtabmapThread::mainLoop
virtual void mainLoop()
Definition: RtabmapThread.cpp:180
rtabmap::Rtabmap::dumpData
void dumpData() const
Definition: Rtabmap.cpp:4905
rtabmap::RtabmapThread::setDetectorRate
void setDetectorRate(float rate)
Definition: RtabmapThread.cpp:102
rtabmap::RtabmapEventCmd::kCmdUpdateParams
@ kCmdUpdateParams
Definition: RtabmapEvent.h:66
rtabmap::Transform::setNull
void setNull()
Definition: Transform.cpp:152
UVariant::isUInt
bool isUInt() const
Definition: UVariant.h:85
rtabmap::Rtabmap::addNodesToRepublish
void addNodesToRepublish(const std::vector< int > &ids)
Definition: Rtabmap.cpp:6460
UTimer.h
rtabmap::RtabmapEventCmd::kCmdRepublishData
@ kCmdRepublishData
Definition: RtabmapEvent.h:73
ULOGGER_DEBUG
#define ULOGGER_DEBUG(...)
Definition: ULogger.h:53
RtabmapThread.h
rtabmap::RtabmapThread::pushNewState
void pushNewState(State newState, const RtabmapEventCmd &cmdEvent=RtabmapEventCmd(RtabmapEventCmd::kCmdUndef))
Definition: RtabmapThread.cpp:69
rtabmap::RtabmapEventCmd::kCmdResetMemory
@ kCmdResetMemory
Definition: RtabmapEvent.h:64
rtabmap::OdometryInfo::transform
Transform transform
Definition: OdometryInfo.h:115
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:500
rtabmap::RtabmapEventCmd::value2
const UVariant & value2() const
Definition: RtabmapEvent.h:118
rtabmap::RtabmapEventCmd::kCmdInit
@ kCmdInit
Definition: RtabmapEvent.h:63
rtabmap::RtabmapThread::createIntermediateNodes
void createIntermediateNodes(bool enabled)
Definition: RtabmapThread.cpp:113
rtabmap::RtabmapEventCmd::value1
const UVariant & value1() const
Definition: RtabmapEvent.h:117
UEvent
Definition: UEvent.h:57
labels
std::vector< std::string > labels
rtabmap::Rtabmap::getStatistics
const Statistics & getStatistics() const
Definition: Rtabmap.cpp:849
rtabmap::RtabmapThread::_createIntermediateNodes
bool _createIntermediateNodes
Definition: RtabmapThread.h:108
rtabmap::RtabmapThread::_userData
cv::Mat _userData
Definition: RtabmapThread.h:117
rtabmap::RtabmapThread::process
void process()
Definition: RtabmapThread.cpp:465
UFATAL
#define UFATAL(...)
RtabmapEvent.h
rtabmap::RtabmapEventCmd::value4
const UVariant & value4() const
Definition: RtabmapEvent.h:120
data
int data[]
UScopeMutex
Definition: UMutex.h:157
rtabmap::RtabmapEventCmd::Cmd
Cmd
Definition: RtabmapEvent.h:61
UEvent::getClassName
virtual std::string getClassName() const =0
rtabmap::RtabmapEvent3DMap
Definition: RtabmapEvent.h:172
rtabmap::Rtabmap::computePath
bool computePath(int targetNode, bool global)
Definition: Rtabmap.cpp:6498
rtabmap::Rtabmap::exportPoses
void exportPoses(const std::string &path, bool optimized, bool global, int format)
Definition: Rtabmap.cpp:1024
stats
bool stats
UConversion.h
Some conversion functions.
rtabmap::Rtabmap::triggerNewMap
int triggerNewMap()
Definition: Rtabmap.cpp:887
rtabmap::SensorData::id
int id() const
Definition: SensorData.h:174
rtabmap::Rtabmap::clearPath
void clearPath(int status)
Definition: Rtabmap.cpp:6480
Rtabmap.h
rtabmap::Rtabmap::isRGBDMode
bool isRGBDMode() const
Definition: Rtabmap.h:127
rtabmap::RtabmapThread::_previousStamp
double _previousStamp
Definition: RtabmapThread.h:110
rtabmap::SensorEvent::kCodeData
@ kCodeData
Definition: SensorEvent.h:42
UThread::kill
void kill()
Definition: UThread.cpp:48
SensorEvent.h
rtabmap::RegistrationInfo::covariance
cv::Mat covariance
Definition: RegistrationInfo.h:75
UVariant::isStr
bool isStr() const
Definition: UVariant.h:88
rtabmap::SensorData::setId
void setId(int id)
Definition: SensorData.h:175
UASSERT
#define UASSERT(condition)
rtabmap::RtabmapEventCmd::kCmdGoal
@ kCmdGoal
Definition: RtabmapEvent.h:77
rtabmap::RtabmapThread::_frameRateTimer
UTimer * _frameRateTimer
Definition: RtabmapThread.h:109
rtabmap::OdometryEvent::info
const OdometryInfo & info() const
Definition: OdometryEvent.h:89
rtabmap::RtabmapThread::_dataBuffer
std::list< OdometryEvent > _dataBuffer
Definition: RtabmapThread.h:102
rtabmap::OdometryInfo::copyWithoutData
OdometryInfo copyWithoutData() const
Definition: OdometryInfo.h:65
rtabmap::Parameters
Definition: Parameters.h:170
rtabmap::Transform::setIdentity
void setIdentity()
Definition: Transform.cpp:157
OdometryEvent.h
rtabmap::RtabmapThread::clearBufferedData
void clearBufferedData()
Definition: RtabmapThread.cpp:83
UserDataEvent.h
rtabmap::Rtabmap::generateDOTGraph
void generateDOTGraph(const std::string &path, int id=0, int margin=5)
Definition: Rtabmap.cpp:992
rtabmap::RtabmapEventCmd::kCmdLabel
@ kCmdLabel
Definition: RtabmapEvent.h:79
rtabmap::SensorData::setFeatures
void setFeatures(const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &keypoints3D, const cv::Mat &descriptors)
Definition: SensorData.cpp:801
str
UWARN
#define UWARN(...)
rtabmap::RtabmapThread::lastPose_
Transform lastPose_
Definition: RtabmapThread.h:114
rtabmap::RtabmapThread::_dataBufferMaxSize
unsigned int _dataBufferMaxSize
Definition: RtabmapThread.h:106
rtabmap::Rtabmap::init
void init(const ParametersMap &parameters, const std::string &databasePath="", bool loadDatabaseParameters=false)
Definition: Rtabmap.cpp:320
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
rtabmap::RtabmapThread::kStateProcessCommand
@ kStateProcessCommand
Definition: RtabmapThread.h:58
timer::elapsed
double elapsed() const
rtabmap::RtabmapThread::_stateMutex
UMutex _stateMutex
Definition: RtabmapThread.h:98
rtabmap::Rtabmap::getPathStatus
int getPathStatus() const
Definition: Rtabmap.h:231
ULogger::registerCurrentThread
static void registerCurrentThread(const std::string &name)
Definition: ULogger.cpp:218
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
UVariant::toStr
std::string toStr(bool *ok=0) const
Definition: UVariant.cpp:634
ULogger.h
ULogger class and convenient macros.
State
rtabmap::Rtabmap::getPath
const std::vector< std::pair< int, Transform > > & getPath() const
Definition: Rtabmap.h:235
rtabmap::Transform
Definition: Transform.h:41
rtabmap::RtabmapGoalStatusEvent
Definition: RtabmapEvent.h:253
USemaphore::release
void release(int n=1)
Definition: USemaphore.h:168
rtabmap::RtabmapThread::publishMap
void publishMap(bool optimized, bool full, bool graphOnly) const
Definition: RtabmapThread.cpp:129
Memory.h
full
@ full
Definition: lz4.c:365
rtabmap::UserDataEvent
Definition: UserDataEvent.h:40
rtabmap::RtabmapEventCmd::kCmdCancelGoal
@ kCmdCancelGoal
Definition: RtabmapEvent.h:78
rtabmap::RtabmapEventCmd::getParameters
const ParametersMap & getParameters() const
Definition: RtabmapEvent.h:122
rtabmap::RtabmapEventCmd::kCmdCleanDataBuffer
@ kCmdCleanDataBuffer
Definition: RtabmapEvent.h:71
rtabmap::RtabmapThread::_dataMutex
UMutex _dataMutex
Definition: RtabmapThread.h:104
rtabmap::RtabmapThread::covariance_
cv::Mat covariance_
Definition: RtabmapThread.h:115
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
rtabmap::Rtabmap::parseParameters
void parseParameters(const ParametersMap &parameters)
Definition: Rtabmap.cpp:545
rtabmap::RtabmapEventCmd::kCmdClose
@ kCmdClose
Definition: RtabmapEvent.h:65
rtabmap::RtabmapThread::_state
std::queue< State > _state
Definition: RtabmapThread.h:99
rtabmap::Rtabmap::process
bool process(const SensorData &data, Transform odomPose, const cv::Mat &odomCovariance=cv::Mat::eye(6, 6, CV_64FC1), const std::vector< float > &odomVelocity=std::vector< float >(), const std::map< std::string, float > &externalStats=std::map< std::string, float >())
Main loop of rtabmap.
Definition: Rtabmap.cpp:1177
UStl.h
Wrappers of STL for convenient functions.
rtabmap::Rtabmap::close
void close(bool databaseSaved=true, const std::string &ouputDatabasePath="")
Definition: Rtabmap.cpp:463
rtabmap::RtabmapThread::getData
bool getData(OdometryEvent &data)
Definition: RtabmapThread.cpp:588
rtabmap::OdometryInfo::interval
double interval
Definition: OdometryInfo.h:114
UDEBUG
#define UDEBUG(...)
Camera.h
rtabmap::OdometryInfo::reg
RegistrationInfo reg
Definition: OdometryInfo.h:99
UTimer
Definition: UTimer.h:46
rtabmap::OdometryInfo
Definition: OdometryInfo.h:40
UVariant::toBool
bool toBool() const
Definition: UVariant.cpp:145
rtabmap::RtabmapThread::_userDataMutex
UMutex _userDataMutex
Definition: RtabmapThread.h:118
rtabmap::RtabmapThread::addData
void addData(const OdometryEvent &odomEvent)
Definition: RtabmapThread.cpp:494
rtabmap::RtabmapThread::mainLoopKill
virtual void mainLoopKill()
Definition: RtabmapThread.cpp:173
rtabmap::Rtabmap
Definition: Rtabmap.h:54
UThread::isRunning
bool isRunning() const
Definition: UThread.cpp:245
rtabmap::Rtabmap::dumpPrediction
void dumpPrediction() const
Definition: Rtabmap.cpp:5331
rtabmap::RtabmapThread::_newMapEvents
std::list< double > _newMapEvents
Definition: RtabmapThread.h:103
rtabmap::OdometryEvent::data
SensorData & data()
Definition: OdometryEvent.h:69
rtabmap::RtabmapThread::mainLoopBegin
virtual void mainLoopBegin()
Definition: RtabmapThread.cpp:163
false
#define false
Definition: ConvertUTF.c:56
cmd
string cmd
UTimer::getElapsedTime
double getElapsedTime()
Definition: UTimer.cpp:97
rtabmap::RtabmapThread::_rate
float _rate
Definition: RtabmapThread.h:107
rtabmap::RtabmapEventCmd::kCmdRemoveLabel
@ kCmdRemoveLabel
Definition: RtabmapEvent.h:80
rtabmap::RtabmapEventCmd::kCmdPause
@ kCmdPause
Definition: RtabmapEvent.h:75
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::RtabmapGlobalPathEvent
Definition: RtabmapEvent.h:201
UEventsManager.h
rtabmap::RtabmapThread::RtabmapThread
RtabmapThread(Rtabmap *rtabmap)
Definition: RtabmapThread.cpp:46
UERROR
#define UERROR(...)
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:53
UVariant::toInt
int toInt(bool *ok=0) const
Definition: UVariant.cpp:454
rtabmap::Rtabmap::labelLocation
bool labelLocation(int id, const std::string &label)
Definition: Rtabmap.cpp:936
ULOGGER_WARN
#define ULOGGER_WARN(...)
Definition: ULogger.h:55
rtabmap::SensorEvent
Definition: SensorEvent.h:37
rtabmap::RtabmapEventCmd::kCmdDumpPrediction
@ kCmdDumpPrediction
Definition: RtabmapEvent.h:68
rtabmap::Rtabmap::resetMemory
void resetMemory()
Definition: Rtabmap.cpp:1063
rtabmap::RtabmapThread::_stateParam
std::queue< RtabmapEventCmd > _stateParam
Definition: RtabmapThread.h:100
rtabmap::RtabmapThread::kStateDetecting
@ kStateDetecting
Definition: RtabmapThread.h:57
ULOGGER_INFO
#define ULOGGER_INFO(...)
Definition: ULogger.h:54
UEventsSender::post
void post(UEvent *event, bool async=true) const
Definition: UEventsSender.cpp:28


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:15