DBReader.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/SensorEvent.h>
29 #include "rtabmap/core/DBReader.h"
30 #include "rtabmap/core/DBDriver.h"
31 
32 #include <rtabmap/utilite/ULogger.h>
33 #include <rtabmap/utilite/UFile.h>
34 #include <rtabmap/utilite/UStl.h>
35 #include <rtabmap/utilite/UConversion.h>
36 #include <rtabmap/utilite/UEventsManager.h>
37 
38 #include "rtabmap/core/RtabmapEvent.h"
39 #include "rtabmap/core/OdometryEvent.h"
40 #include "rtabmap/core/util3d.h"
41 #include "rtabmap/core/Compression.h"
42 
43 namespace rtabmap {
44 
45 DBReader::DBReader(const std::string & databasePath,
46  float frameRate,
47  bool odometryIgnored,
48  bool ignoreGoalDelay,
49  bool goalsIgnored,
50  int startId,
51  int cameraIndex,
52  int stopId,
53  bool intermediateNodesIgnored,
54  bool landmarksIgnored,
55  bool featuresIgnored,
56  int startMapId,
57  int stopMapId,
58  bool priorsIgnored) :
59  Camera(frameRate),
60  _paths(uSplit(databasePath, ';')),
61  _odometryIgnored(odometryIgnored),
62  _ignoreGoalDelay(ignoreGoalDelay),
63  _goalsIgnored(goalsIgnored),
64  _startId(startId),
65  _stopId(stopId),
66  _cameraIndex(cameraIndex),
67  _intermediateNodesIgnored(intermediateNodesIgnored),
68  _landmarksIgnored(landmarksIgnored),
69  _featuresIgnored(featuresIgnored),
70  _priorsIgnored(priorsIgnored),
71  _startMapId(startMapId),
72  _stopMapId(stopMapId),
73  _dbDriver(0),
74  _currentId(_ids.end()),
75  _previousMapId(-1),
76  _previousStamp(0),
77  _previousMapID(0),
78  _calibrated(false)
79 {
80  if(_stopId>0 && _stopId<_startId)
81  {
82  _stopId = _startId;
83  }
84 
85  if(_stopMapId>-1 && _stopMapId<_startMapId)
86  {
87  _stopMapId = _startMapId;
88  }
89 }
90 
91 DBReader::DBReader(const std::list<std::string> & databasePaths,
92  float frameRate,
93  bool odometryIgnored,
94  bool ignoreGoalDelay,
95  bool goalsIgnored,
96  int startId,
97  int cameraIndex,
98  int stopId,
99  bool intermediateNodesIgnored,
100  bool landmarksIgnored,
101  bool featuresIgnored,
102  int startMapId,
103  int stopMapId,
104  bool priorsIgnored) :
105  Camera(frameRate),
106  _paths(databasePaths),
107  _odometryIgnored(odometryIgnored),
108  _ignoreGoalDelay(ignoreGoalDelay),
109  _goalsIgnored(goalsIgnored),
110  _startId(startId),
111  _stopId(stopId),
112  _cameraIndex(cameraIndex),
113  _intermediateNodesIgnored(intermediateNodesIgnored),
114  _landmarksIgnored(landmarksIgnored),
115  _featuresIgnored(featuresIgnored),
116  _priorsIgnored(priorsIgnored),
117  _startMapId(startMapId),
118  _stopMapId(stopMapId),
119  _dbDriver(0),
120  _currentId(_ids.end()),
121  _previousMapId(-1),
122  _previousStamp(0),
123  _previousMapID(0),
124  _calibrated(false)
125 {
126  if(_stopId>0 && _stopId<_startId)
127  {
128  _stopId = _startId;
129  }
130 
131  if(_stopMapId>-1 && _stopMapId<_startMapId)
132  {
133  _stopMapId = _startMapId;
134  }
135 }
136 
137 DBReader::~DBReader()
138 {
139  if(_dbDriver)
140  {
141  _dbDriver->closeConnection();
142  delete _dbDriver;
143  }
144 }
145 
146 bool DBReader::init(
147  const std::string & calibrationFolder,
148  const std::string & cameraName)
149 {
150  if(_dbDriver)
151  {
152  _dbDriver->closeConnection();
153  delete _dbDriver;
154  _dbDriver = 0;
155  }
156  _ids.clear();
157  _currentId=_ids.end();
158  _previousMapId = -1;
159  _previousInfMatrix = cv::Mat();
160  _previousStamp = 0;
161  _previousMapID = 0;
162  _calibrated = false;
163 
164  if(_paths.size() == 0)
165  {
166  UERROR("No database path set...");
167  return false;
168  }
169 
170  std::string path = _paths.front();
171  if(!UFile::exists(path))
172  {
173  UERROR("Database path does not exist (%s)", path.c_str());
174  return false;
175  }
176 
177  rtabmap::ParametersMap parameters;
178  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
179  _dbDriver = DBDriver::create(parameters);
180  if(!_dbDriver)
181  {
182  UERROR("Driver doesn't exist.");
183  return false;
184  }
185  if(!_dbDriver->openConnection(path))
186  {
187  UERROR("Can't open database %s", path.c_str());
188  delete _dbDriver;
189  _dbDriver = 0;
190  return false;
191  }
192 
193  _dbDriver->getAllNodeIds(_ids);
194  _currentId = _ids.begin();
195  if(_startId>0 && _ids.size())
196  {
197  std::set<int>::iterator iter = _ids.find(_startId);
198  if(iter == _ids.end())
199  {
200  UWARN("Start index is too high (%d), the last ID in database is %d. Starting from beginning...", _startId, *_ids.rbegin());
201  }
202  else
203  {
204  _currentId = iter;
205  }
206  }
207 
208  if(_ids.size())
209  {
210  std::vector<CameraModel> models;
211  std::vector<StereoCameraModel> stereoModels;
212  if(_dbDriver->getCalibration(*_ids.begin(), models, stereoModels))
213  {
214  if(models.size())
215  {
216  if(models.at(0).isValidForProjection())
217  {
218  _calibrated = true;
219  }
220  else if(models.at(0).fx() && models.at(0).fy() && models.at(0).imageWidth() == 0)
221  {
222  // backward compatibility for databases not saving cx,cy and imageSize
223  SensorData data;
224  _dbDriver->getNodeData(*_ids.begin(), data, true, false, false, false);
225  cv::Mat rgb;
226  data.uncompressData(&rgb, 0); // this will update camera models if old format
227  if(data.cameraModels().size() && data.cameraModels().at(0).isValidForProjection())
228  {
229  _calibrated = true;
230  }
231  }
232  }
233  else if(stereoModels.size() && stereoModels.at(0).isValidForProjection())
234  {
235  _calibrated = true;
236  }
237  else
238  {
239  Signature * s = _dbDriver->loadSignature(*_ids.begin());
240  _dbDriver->loadNodeData(s);
241  if( s->sensorData().imageCompressed().empty() &&
242  s->getWords().empty() &&
243  !s->sensorData().laserScanCompressed().empty())
244  {
245  _calibrated = true; // only scans
246  }
247  delete s;
248  }
249  }
250  }
251  else
252  {
253  _calibrated = true; // database is empty, make sure calibration warning is not shown.
254  }
255 
256  _timer.start();
257 
258  return true;
259 }
260 
261 bool DBReader::isCalibrated() const
262 {
263  return _calibrated;
264 }
265 
266 std::string DBReader::getSerial() const
267 {
268  return "DBReader";
269 }
270 
271 bool DBReader::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
272 {
273  UERROR("DBReader only provides pose when capturing data, it cannot provide asynchronous pose.");
274  return false;
275 }
276 
277 SensorData DBReader::captureImage(SensorCaptureInfo * info)
278 {
279  SensorData data = this->getNextData(info);
280  if(data.id()>0 && _stopId>0 && data.id() > _stopId)
281  {
282  UINFO("Last ID %d has been reached! Ignoring", _stopId);
283  return SensorData();
284  }
285  if(data.id() == 0)
286  {
287  UINFO("no more images...");
288  while(_paths.size() > 1 && data.id() == 0)
289  {
290  _paths.pop_front();
291  UWARN("Loading next database \"%s\"...", _paths.front().c_str());
292  if(!this->init())
293  {
294  UERROR("Failed to initialize the next database \"%s\"", _paths.front().c_str());
295  return data;
296  }
297  else
298  {
299  data = this->getNextData(info);
300  }
301  }
302  }
303 
304  if(data.id())
305  {
306  std::string goalId;
307  double previousStamp = data.stamp();
308  if(previousStamp == 0)
309  {
310  data.setStamp(UTimer::now());
311  }
312 
313  if(!_goalsIgnored &&
314  data.userDataRaw().type() == CV_8SC1 &&
315  data.userDataRaw().cols >= 7 && // including null str ending
316  data.userDataRaw().rows == 1 &&
317  memcmp(data.userDataRaw().data, "GOAL:", 5) == 0)
318  {
319  //GOAL format detected, remove it from the user data and send it as goal event
320  std::string goalStr = (const char *)data.userDataRaw().data;
321  if(!goalStr.empty())
322  {
323  std::list<std::string> strs = uSplit(goalStr, ':');
324  if(strs.size() == 2)
325  {
326  goalId = *strs.rbegin();
327  data.setUserData(cv::Mat());
328 
329  double delay = 0.0;
330  if(!_ignoreGoalDelay && _currentId != _ids.end())
331  {
332  // get stamp for the next signature to compute the delay
333  // that was used originally for planning
334  int weight;
335  std::string label;
336  double stamp;
337  int mapId;
338  Transform localTransform, pose, groundTruth;
339  std::vector<float> velocity;
340  GPS gps;
341  EnvSensors sensors;
342  _dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors);
343  if(previousStamp && stamp && stamp > previousStamp)
344  {
345  delay = stamp - previousStamp;
346  }
347  }
348 
349  if(delay > 0.0)
350  {
351  UWARN("Goal \"%s\" detected, posting it! Waiting %f seconds before sending next data...",
352  goalId.c_str(), delay);
353  }
354  else
355  {
356  UWARN("Goal \"%s\" detected, posting it!", goalId.c_str());
357  }
358 
359  if(uIsInteger(goalId))
360  {
361  UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, atoi(goalId.c_str())));
362  }
363  else
364  {
365  UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId));
366  }
367 
368  if(delay > 0.0)
369  {
370  uSleep(delay*1000);
371  }
372  }
373  }
374  }
375  }
376  return data;
377 }
378 
379 SensorData DBReader::getNextData(SensorCaptureInfo * info)
380 {
381  SensorData data;
382  if(_dbDriver)
383  {
384  while(_currentId != _ids.end())
385  {
386  std::list<int> signIds;
387  signIds.push_back(*_currentId);
388  std::list<Signature *> signatures;
389  _dbDriver->loadSignatures(signIds, signatures);
390  if(signatures.empty())
391  {
392  return data;
393  }
394  _dbDriver->loadNodeData(signatures);
395  Signature * s = signatures.front();
396 
397  if(_intermediateNodesIgnored && s->getWeight() == -1)
398  {
399  UDEBUG("Ignoring node %d (intermediate nodes ignored)", s->id());
400  ++_currentId;
401  delete s;
402  continue;
403  }
404 
405  if(s->mapId() < _startMapId || (_stopMapId>=0 && s->mapId() > _stopMapId))
406  {
407  UDEBUG("Ignoring node %d (map id=%d, min=%d max=%d)", s->id(), s->mapId(), _startMapId, _stopMapId);
408  ++_currentId;
409  delete s;
410  continue;
411  }
412 
413  data = s->sensorData();
414 
415  // info
416  Transform pose = s->getPose();
417  Transform globalPose;
418  cv::Mat globalPoseCov;
419 
420  std::multimap<int, Link> priorLinks;
421  if(!_priorsIgnored)
422  {
423  _dbDriver->loadLinks(*_currentId, priorLinks, Link::kPosePrior);
424  if( priorLinks.size() &&
425  !priorLinks.begin()->second.transform().isNull() &&
426  priorLinks.begin()->second.infMatrix().cols == 6 &&
427  priorLinks.begin()->second.infMatrix().rows == 6)
428  {
429  globalPose = priorLinks.begin()->second.transform();
430  globalPoseCov = priorLinks.begin()->second.infMatrix().inv();
431  if(data.gps().stamp() != 0.0 &&
432  globalPoseCov.at<double>(3,3)>=9999 &&
433  globalPoseCov.at<double>(4,4)>=9999 &&
434  globalPoseCov.at<double>(5,5)>=9999)
435  {
436  // clear global pose as GPS was used for prior
437  globalPose.setNull();
438  }
439  }
440  }
441 
442  Transform gravityTransform;
443  std::multimap<int, Link> gravityLinks;
444  _dbDriver->loadLinks(*_currentId, gravityLinks, Link::kGravity);
445  if( gravityLinks.size() &&
446  !gravityLinks.begin()->second.transform().isNull() &&
447  gravityLinks.begin()->second.infMatrix().cols == 6 &&
448  gravityLinks.begin()->second.infMatrix().rows == 6)
449  {
450  gravityTransform = gravityLinks.begin()->second.transform();
451  }
452 
453  Landmarks landmarks;
454  if(!_landmarksIgnored)
455  {
456  std::multimap<int, Link> landmarkLinks;
457  _dbDriver->loadLinks(*_currentId, landmarkLinks, Link::kLandmark);
458  for(std::multimap<int, Link>::iterator iter=landmarkLinks.begin(); iter!=landmarkLinks.end(); ++iter)
459  {
460  cv::Mat landmarkSize = iter->second.uncompressUserDataConst();
461  landmarks.insert(std::make_pair(-iter->first,
462  Landmark(-iter->first,
463  !landmarkSize.empty() && landmarkSize.type() == CV_32FC1 && landmarkSize.total()==1?landmarkSize.at<float>(0,0):0.0f,
464  iter->second.transform(),
465  iter->second.infMatrix().inv())));
466  }
467  }
468 
469  cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
470  if(!_odometryIgnored)
471  {
472  std::multimap<int, Link> links;
473  _dbDriver->loadLinks(*_currentId, links, Link::kNeighbor);
474  if(links.size() && links.begin()->first < *_currentId)
475  {
476  // assume the first is the backward neighbor, take its variance
477  infMatrix = links.begin()->second.infMatrix();
478  _previousInfMatrix = infMatrix;
479  }
480  else
481  {
482  if(_previousMapId != s->mapId())
483  {
484  // first node, set high variance to make rtabmap trigger a new map
485  infMatrix /= 9999.0;
486  UDEBUG("First node of map %d, variance set to 9999", s->mapId());
487  }
488  else
489  {
490  // if localization data saved in database, covariance will be set in a prior link
491  _dbDriver->loadLinks(*_currentId, links, Link::kPosePrior);
492  if(links.size())
493  {
494  // assume the first is the backward neighbor, take its variance
495  infMatrix = links.begin()->second.infMatrix();
496  _previousInfMatrix = infMatrix;
497  }
498  else
499  {
500  if(_previousInfMatrix.empty())
501  {
502  _previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
503  }
504  // we have a node not linked to map, use last variance
505  infMatrix = _previousInfMatrix;
506  }
507  }
508  }
509  _previousMapId = s->mapId();
510  }
511  else
512  {
513  pose.setNull();
514  }
515 
516  int seq = *_currentId;
517  ++_currentId;
518 
519  // Frame rate
520  if(this->getImageRate() < 0.0f)
521  {
522  if(s->getStamp() == 0)
523  {
524  UERROR("The option to use database stamps is set (framerate<0), but there are no stamps saved in the database! Aborting...");
525  delete s;
526  return data;
527  }
528  else if(_previousMapID == s->mapId() && _previousStamp > 0)
529  {
530  float ratio = -this->getImageRate();
531  int sleepTime = 1000.0*(s->getStamp()-_previousStamp)/ratio - 1000.0*_timer.getElapsedTime();
532  double stamp = s->getStamp();
533  if(sleepTime > 10000)
534  {
535  UWARN("Detected long delay (%d sec, stamps = %f vs %f). Waiting a maximum of 10 seconds.",
536  sleepTime/1000, _previousStamp, s->getStamp());
537  sleepTime = 10000;
538  stamp = _previousStamp+10;
539  }
540  if(sleepTime > 2)
541  {
542  uSleep(sleepTime-2);
543  }
544 
545  // Add precision at the cost of a small overhead
546  while(_timer.getElapsedTime() < (stamp-_previousStamp)/ratio-0.000001)
547  {
548  //
549  }
550 
551  double slept = _timer.getElapsedTime();
552  _timer.start();
553  UDEBUG("slept=%fs vs target=%fs (ratio=%f)", slept, (stamp-_previousStamp)/ratio, ratio);
554  }
555  _previousStamp = s->getStamp();
556  _previousMapID = s->mapId();
557  }
558 
559  data.uncompressData();
560  if(data.cameraModels().size() > 1 &&
561  _cameraIndex >= 0)
562  {
563  if(_cameraIndex < (int)data.cameraModels().size())
564  {
565  // select one camera
566  int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
567  cv::Mat image;
568  UASSERT(!data.imageRaw().empty() &&
569  data.imageRaw().cols % data.cameraModels().size() == 0 &&
570  _cameraIndex*subImageWidth < data.imageRaw().cols);
571  image= cv::Mat(data.imageRaw(),
572  cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.imageRaw().rows)).clone();
573 
574  cv::Mat depth;
575  if(!data.depthOrRightRaw().empty())
576  {
577  UASSERT(data.depthOrRightRaw().cols % data.cameraModels().size() == 0 &&
578  subImageWidth == data.depthOrRightRaw().cols/(int)data.cameraModels().size() &&
579  _cameraIndex*subImageWidth < data.depthOrRightRaw().cols);
580  depth = cv::Mat(data.depthOrRightRaw(),
581  cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows)).clone();
582  }
583  data.setRGBDImage(image, depth, data.cameraModels().at(_cameraIndex));
584  }
585  else
586  {
587  UWARN("DBReader: Camera index %d doesn't exist! Camera models = %d.", _cameraIndex, (int)data.cameraModels().size());
588  }
589  }
590  data.setId(seq);
591  data.setStamp(s->getStamp());
592  data.setGroundTruth(s->getGroundTruthPose());
593  if(!globalPose.isNull())
594  {
595  data.setGlobalPose(globalPose, globalPoseCov);
596  }
597  if(!gravityTransform.isNull())
598  {
599  Eigen::Quaterniond q = gravityTransform.getQuaterniond();
600  data.setIMU(IMU(
601  cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat::eye(3,3,CV_64FC1),
602  cv::Vec3d(), cv::Mat(),
603  cv::Vec3d(), cv::Mat(),
604  Transform::getIdentity())); // we assume that gravity links are already transformed in base_link
605  }
606  data.setLandmarks(landmarks);
607 
608  UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, Grid=%d, UserData=%d, GlobalPose=%d, GPS=%d, IMU=%d",
609  data.laserScanRaw().isEmpty()?0:1,
610  data.imageRaw().empty()?0:1,
611  data.depthOrRightRaw().empty()?0:1,
612  data.gridCellSize()==0.0f?0:1,
613  data.userDataRaw().empty()?0:1,
614  globalPose.isNull()?0:1,
615  data.gps().stamp()!=0.0?1:0,
616  gravityTransform.isNull()?0:1);
617 
618  cv::Mat descriptors = s->getWordsDescriptors().clone();
619  const std::vector<cv::KeyPoint> & keypoints = s->getWordsKpts();
620  const std::vector<cv::Point3f> & keypoints3D = s->getWords3();
621  if(!_featuresIgnored &&
622  !keypoints.empty() &&
623  (keypoints3D.empty() || keypoints.size() == keypoints3D.size()) &&
624  (descriptors.empty() || (int)keypoints.size() == descriptors.rows))
625  {
626  data.setFeatures(keypoints, keypoints3D, descriptors);
627  }
628  else if(!_featuresIgnored && !keypoints.empty() && (!keypoints3D.empty() || !descriptors.empty()))
629  {
630  UERROR("Missing feature data, features won't be published.");
631  }
632 
633  if(data.imageCompressed().empty() && s->getWeight()>=0 && keypoints.empty())
634  {
635  UWARN("No image loaded from the database for id=%d!", seq);
636  }
637 
638  if(!_odometryIgnored)
639  {
640  if(pose.isNull())
641  {
642  UWARN("Reading the database: odometry is null! "
643  "Please set \"Ignore odometry = true\" if there is "
644  "no odometry in the database.");
645  }
646  if(info)
647  {
648  info->odomPose = pose;
649  info->odomCovariance = infMatrix.inv();
650  info->odomVelocity = s->getVelocity();
651  UDEBUG("odom variance = %f/%f", info->odomCovariance.at<double>(0,0), info->odomCovariance.at<double>(5,5));
652  }
653  }
654  delete s;
655  break;
656  }
657  }
658  else
659  {
660  UERROR("Not initialized...");
661  }
662  return data;
663 }
664 
665 } /* namespace rtabmap */
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
Compression.h
rtabmap::DBDriver::getNodeData
void getNodeData(int signatureId, SensorData &data, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const
Definition: DBDriver.cpp:685
rtabmap::DBReader::_calibrated
bool _calibrated
Definition: DBReader.h:118
rtabmap::DBDriver::getCalibration
bool getCalibration(int signatureId, std::vector< CameraModel > &models, std::vector< StereoCameraModel > &stereoModels) const
Definition: DBDriver.cpp:736
rtabmap::DBReader::_landmarksIgnored
bool _landmarksIgnored
Definition: DBReader.h:104
rtabmap::DBReader::getNextData
SensorData getNextData(SensorCaptureInfo *info=0)
Definition: DBReader.cpp:379
UINFO
#define UINFO(...)
rtabmap::RtabmapEventCmd
Definition: RtabmapEvent.h:57
rtabmap::DBReader::_previousMapID
int _previousMapID
Definition: DBReader.h:117
UTimer::now
static double now()
Definition: UTimer.cpp:80
s
RealScalar s
rtabmap::DBReader::_goalsIgnored
bool _goalsIgnored
Definition: DBReader.h:99
rtabmap::DBDriver::openConnection
bool openConnection(const std::string &url, bool overwritten=false)
Definition: DBDriver.cpp:86
rtabmap::DBReader::captureImage
virtual SensorData captureImage(SensorCaptureInfo *info=0)
Definition: DBReader.cpp:277
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
rtabmap::DBDriver::loadSignature
Signature * loadSignature(int id, bool *loadedFromTrash=0)
Definition: DBDriver.cpp:547
rtabmap::DBReader::_previousInfMatrix
cv::Mat _previousInfMatrix
Definition: DBReader.h:115
rtabmap::DBReader::getPose
virtual bool getPose(double stamp, Transform &pose, cv::Mat &covariance, double maxWaitTime=0.06)
Definition: DBReader.cpp:271
end
end
rtabmap::GPS
Definition: GPS.h:35
rtabmap::DBReader::_startId
int _startId
Definition: DBReader.h:100
rtabmap::DBReader::_ids
std::set< int > _ids
Definition: DBReader.h:112
DBReader.h
uIsInteger
bool uIsInteger(const std::string &str, bool checkForSign=true)
Definition: UStl.h:630
UTimer::start
void start()
Definition: UTimer.cpp:87
rtabmap::DBReader::_stopMapId
int _stopMapId
Definition: DBReader.h:108
rtabmap::DBDriver::loadNodeData
void loadNodeData(Signature *signature, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const
Definition: DBDriver.cpp:658
rtabmap::Transform::setNull
void setNull()
Definition: Transform.cpp:152
util3d.h
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::Landmarks
std::map< int, Landmark > Landmarks
Definition: Landmark.h:79
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::DBReader::_previousStamp
double _previousStamp
Definition: DBReader.h:116
rtabmap::Landmark
Definition: Landmark.h:39
rtabmap::SensorCaptureInfo
Definition: SensorCaptureInfo.h:36
rtabmap::DBReader::_ignoreGoalDelay
bool _ignoreGoalDelay
Definition: DBReader.h:98
rtabmap::DBReader::getSerial
virtual std::string getSerial() const
Definition: DBReader.cpp:266
rtabmap::IMU
Definition: IMU.h:19
rtabmap::DBReader::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: DBReader.cpp:146
RtabmapEvent.h
seq
ArithmeticSequence< FirstTypeDerived, symbolic::AddExpr< symbolic::AddExpr< LastTypeDerived, symbolic::NegateExpr< FirstTypeDerived > >, symbolic::ValueExpr< internal::FixedInt< 1 > > > > seq(const symbolic::BaseExpr< FirstTypeDerived > &f, const symbolic::BaseExpr< LastTypeDerived > &l)
data
int data[]
q
EIGEN_DEVICE_FUNC const Scalar & q
UConversion.h
Some conversion functions.
rtabmap::DBReader::isCalibrated
virtual bool isCalibrated() const
Definition: DBReader.cpp:261
rtabmap::DBReader::_stopId
int _stopId
Definition: DBReader.h:101
velocity
Velocity3 velocity(const NavState &X, OptionalJacobian< 3, 9 > H)
SensorEvent.h
rtabmap::DBDriver::getNodeInfo
bool getNodeInfo(int signatureId, Transform &pose, int &mapId, int &weight, std::string &label, double &stamp, Transform &groundTruthPose, std::vector< float > &velocity, GPS &gps, EnvSensors &sensors) const
Definition: DBDriver.cpp:786
info
else if n * info
UASSERT
#define UASSERT(condition)
rtabmap::RtabmapEventCmd::kCmdGoal
@ kCmdGoal
Definition: RtabmapEvent.h:77
rtabmap::DBReader::_previousMapId
int _previousMapId
Definition: DBReader.h:114
DBDriver.h
rtabmap::DBReader::_dbDriver
DBDriver * _dbDriver
Definition: DBReader.h:110
OdometryEvent.h
rtabmap::Camera
Definition: Camera.h:43
rtabmap::Camera::getImageRate
float getImageRate() const
Definition: Camera.h:49
rtabmap::DBReader::_intermediateNodesIgnored
bool _intermediateNodesIgnored
Definition: DBReader.h:103
rtabmap::DBDriver::loadLinks
void loadLinks(int signatureId, std::multimap< int, Link > &links, Link::Type type=Link::kUndef) const
Definition: DBDriver.cpp:825
rtabmap::DBReader::~DBReader
virtual ~DBReader()
Definition: DBReader.cpp:137
path
path
rtabmap::DBReader::_timer
UTimer _timer
Definition: DBReader.h:111
UWARN
#define UWARN(...)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Eigen::Quaternion
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform
Definition: Transform.h:41
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
Definition: UStl.h:564
rtabmap::EnvSensors
std::map< EnvSensor::Type, EnvSensor > EnvSensors
Definition: EnvSensor.h:81
rtabmap::DBReader::_priorsIgnored
bool _priorsIgnored
Definition: DBReader.h:106
rtabmap::DBReader::_cameraIndex
int _cameraIndex
Definition: DBReader.h:102
rtabmap::DBReader::DBReader
DBReader(const std::string &databasePath, float frameRate=0.0f, bool odometryIgnored=false, bool ignoreGoalDelay=false, bool goalsIgnored=false, int startId=0, int cameraIndex=-1, int stopId=0, bool intermediateNodesIgnored=false, bool landmarksIgnored=false, bool featuresIgnored=false, int startMapId=0, int stopMapId=-1, bool priorsIgnored=false)
Definition: DBReader.cpp:45
iter
iterator iter(handle obj)
ratio
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size ratio
UStl.h
Wrappers of STL for convenient functions.
UDEBUG
#define UDEBUG(...)
rtabmap::DBReader::_startMapId
int _startMapId
Definition: DBReader.h:107
UEventsManager::post
static void post(UEvent *event, bool async=true, const UEventsSender *sender=0)
Definition: UEventsManager.cpp:54
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
rtabmap::DBDriver::getAllNodeIds
void getAllNodeIds(std::set< int > &ids, bool ignoreChildren=false, bool ignoreBadSignatures=false, bool ignoreIntermediateNodes=false) const
Definition: DBDriver.cpp:886
false
#define false
Definition: ConvertUTF.c:56
UTimer::getElapsedTime
double getElapsedTime()
Definition: UTimer.cpp:97
UFile.h
rtabmap
Definition: CameraARCore.cpp:35
UFile::exists
bool exists()
Definition: UFile.h:104
UEventsManager.h
rtabmap::DBReader::_featuresIgnored
bool _featuresIgnored
Definition: DBReader.h:105
rtabmap::Transform::getQuaterniond
Eigen::Quaterniond getQuaterniond() const
Definition: Transform.cpp:406
UERROR
#define UERROR(...)
rtabmap::DBDriver::loadSignatures
void loadSignatures(const std::list< int > &ids, std::list< Signature * > &signatures, std::set< int > *loadedFromTrash=0)
Definition: DBDriver.cpp:564
rtabmap::DBReader::_currentId
std::set< int >::iterator _currentId
Definition: DBReader.h:113
rtabmap::DBDriver::closeConnection
void closeConnection(bool save=true, const std::string &outputUrl="")
Definition: DBDriver.cpp:64
rtabmap::DBReader::_paths
std::list< std::string > _paths
Definition: DBReader.h:96
rtabmap::Signature
Definition: Signature.h:48
rtabmap::DBReader::_odometryIgnored
bool _odometryIgnored
Definition: DBReader.h:97
rtabmap::DBDriver::create
static DBDriver * create(const ParametersMap &parameters=ParametersMap())
Definition: DBDriver.cpp:41

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