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  const std::vector<unsigned int> & cameraIndices,
52  int stopId,
53  bool intermediateNodesIgnored,
54  bool landmarksIgnored,
55  bool featuresIgnored,
56  int startMapId,
57  int stopMapId,
58  bool priorsIgnored,
59  const std::vector<Transform> & cameraLocalTransformOverrides) :
60  Camera(frameRate),
61  _paths(uSplit(databasePath, ';')),
62  _odometryIgnored(odometryIgnored),
63  _ignoreGoalDelay(ignoreGoalDelay),
64  _goalsIgnored(goalsIgnored),
65  _startId(startId),
66  _stopId(stopId),
67  _cameraIndices(cameraIndices),
68  _intermediateNodesIgnored(intermediateNodesIgnored),
69  _landmarksIgnored(landmarksIgnored),
70  _featuresIgnored(featuresIgnored),
71  _priorsIgnored(priorsIgnored),
72  _startMapId(startMapId),
73  _stopMapId(stopMapId),
74  _cameraLocalTransformOverrides(cameraLocalTransformOverrides),
75  _dbDriver(0),
76  _currentId(_ids.end()),
77  _previousMapId(-1),
78  _previousStamp(0),
79  _previousMapID(0),
80  _calibrated(false)
81 {
82  checkArguments();
83 }
84 
85 DBReader::DBReader(const std::list<std::string> & databasePaths,
86  float frameRate,
87  bool odometryIgnored,
88  bool ignoreGoalDelay,
89  bool goalsIgnored,
90  int startId,
91  const std::vector<unsigned int> & cameraIndices,
92  int stopId,
93  bool intermediateNodesIgnored,
94  bool landmarksIgnored,
95  bool featuresIgnored,
96  int startMapId,
97  int stopMapId,
98  bool priorsIgnored,
99  const std::vector<Transform> & cameraLocalTransformOverrides) :
100  Camera(frameRate),
101  _paths(databasePaths),
102  _odometryIgnored(odometryIgnored),
103  _ignoreGoalDelay(ignoreGoalDelay),
104  _goalsIgnored(goalsIgnored),
105  _startId(startId),
106  _stopId(stopId),
107  _cameraIndices(cameraIndices),
108  _intermediateNodesIgnored(intermediateNodesIgnored),
109  _landmarksIgnored(landmarksIgnored),
110  _featuresIgnored(featuresIgnored),
111  _priorsIgnored(priorsIgnored),
112  _startMapId(startMapId),
113  _stopMapId(stopMapId),
114  _cameraLocalTransformOverrides(cameraLocalTransformOverrides),
115  _dbDriver(0),
116  _currentId(_ids.end()),
117  _previousMapId(-1),
118  _previousStamp(0),
119  _previousMapID(0),
120  _calibrated(false)
121 {
122  checkArguments();
123 }
124 
125 void DBReader::checkArguments()
126 {
127  if(_stopId>0 && _stopId<_startId)
128  {
129  _stopId = _startId;
130  }
131 
132  if(_stopMapId>-1 && _stopMapId<_startMapId)
133  {
134  _stopMapId = _startMapId;
135  }
136 
137  if(!_cameraLocalTransformOverrides.empty())
138  {
139  if(!_cameraIndices.empty() &&
140  _cameraIndices.size() != _cameraLocalTransformOverrides.size())
141  {
142  UERROR("Camera local transform overrides (%d) are not the same size than the camera indices (%d). The overrides are ignored.",
143  _cameraLocalTransformOverrides.size(),
144  _cameraIndices.size()
145  );
146  _cameraLocalTransformOverrides.clear();
147  }
148  for(size_t i=0; i<_cameraLocalTransformOverrides.size(); ++i)
149  {
150  if(_cameraLocalTransformOverrides[i].isNull())
151  {
152  UERROR("Camera local transform overrides vector cannot contains null transforms! Clearing overrides.");
153  _cameraLocalTransformOverrides.clear();
154  break;
155  }
156  }
157  }
158 }
159 
160 DBReader::~DBReader()
161 {
162  if(_dbDriver)
163  {
164  _dbDriver->closeConnection();
165  delete _dbDriver;
166  }
167 }
168 
169 bool DBReader::init(
170  const std::string &,
171  const std::string &)
172 {
173  if(_dbDriver)
174  {
175  _dbDriver->closeConnection();
176  delete _dbDriver;
177  _dbDriver = 0;
178  }
179  _ids.clear();
180  _currentId=_ids.end();
181  _previousMapId = -1;
182  _previousInfMatrix = cv::Mat();
183  _previousStamp = 0;
184  _previousMapID = 0;
185  _calibrated = false;
186 
187  if(_paths.size() == 0)
188  {
189  UERROR("No database path set...");
190  return false;
191  }
192 
193  std::string path = _paths.front();
194  if(!UFile::exists(path))
195  {
196  UERROR("Database path does not exist (%s)", path.c_str());
197  return false;
198  }
199 
200  rtabmap::ParametersMap parameters;
201  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
202  _dbDriver = DBDriver::create(parameters);
203  if(!_dbDriver)
204  {
205  UERROR("Driver doesn't exist.");
206  return false;
207  }
208  if(!_dbDriver->openConnection(path))
209  {
210  UERROR("Can't open database %s", path.c_str());
211  delete _dbDriver;
212  _dbDriver = 0;
213  return false;
214  }
215 
216  _dbDriver->getAllNodeIds(_ids);
217  _currentId = _ids.begin();
218  if(_startId>0 && _ids.size())
219  {
220  std::set<int>::iterator iter = _ids.find(_startId);
221  if(iter == _ids.end())
222  {
223  UWARN("Start index is too high (%d), the last ID in database is %d. Starting from beginning...", _startId, *_ids.rbegin());
224  }
225  else
226  {
227  _currentId = iter;
228  }
229  }
230 
231  if(_ids.size())
232  {
233  std::vector<CameraModel> models;
234  std::vector<StereoCameraModel> stereoModels;
235  if(_dbDriver->getCalibration(*_ids.begin(), models, stereoModels))
236  {
237  if(models.size())
238  {
239  if(models.at(0).isValidForProjection())
240  {
241  _calibrated = true;
242  }
243  else if(models.at(0).fx() && models.at(0).fy() && models.at(0).imageWidth() == 0)
244  {
245  // backward compatibility for databases not saving cx,cy and imageSize
246  SensorData data;
247  _dbDriver->getNodeData(*_ids.begin(), data, true, false, false, false);
248  cv::Mat rgb;
249  data.uncompressData(&rgb, 0); // this will update camera models if old format
250  if(data.cameraModels().size() && data.cameraModels().at(0).isValidForProjection())
251  {
252  _calibrated = true;
253  }
254  }
255  }
256  else if(stereoModels.size() && stereoModels.at(0).isValidForProjection())
257  {
258  _calibrated = true;
259  }
260  else
261  {
262  Signature * s = _dbDriver->loadSignature(*_ids.begin());
263  _dbDriver->loadNodeData(s);
264  if( s->sensorData().imageCompressed().empty() &&
265  s->getWords().empty() &&
266  !s->sensorData().laserScanCompressed().empty())
267  {
268  _calibrated = true; // only scans
269  }
270  delete s;
271  }
272  }
273  }
274  else
275  {
276  _calibrated = true; // database is empty, make sure calibration warning is not shown.
277  }
278 
279  _timer.start();
280 
281  return true;
282 }
283 
284 bool DBReader::isCalibrated() const
285 {
286  return _calibrated;
287 }
288 
289 std::string DBReader::getSerial() const
290 {
291  return "DBReader";
292 }
293 
294 bool DBReader::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
295 {
296  UERROR("DBReader only provides pose when capturing data, it cannot provide asynchronous pose.");
297  return false;
298 }
299 
300 SensorData DBReader::captureImage(SensorCaptureInfo * info)
301 {
302  SensorData data = this->getNextData(info);
303  if(data.id()>0 && _stopId>0 && data.id() > _stopId)
304  {
305  UINFO("Last ID %d has been reached! Ignoring", _stopId);
306  return SensorData();
307  }
308  if(data.id() == 0)
309  {
310  UINFO("no more images...");
311  while(_paths.size() > 1 && data.id() == 0)
312  {
313  _paths.pop_front();
314  UWARN("Loading next database \"%s\"...", _paths.front().c_str());
315  if(!this->init())
316  {
317  UERROR("Failed to initialize the next database \"%s\"", _paths.front().c_str());
318  return data;
319  }
320  else
321  {
322  data = this->getNextData(info);
323  }
324  }
325  }
326 
327  if(data.id())
328  {
329  std::string goalId;
330  double previousStamp = data.stamp();
331  if(previousStamp == 0)
332  {
333  data.setStamp(UTimer::now());
334  }
335 
336  if(!_goalsIgnored &&
337  data.userDataRaw().type() == CV_8SC1 &&
338  data.userDataRaw().cols >= 7 && // including null str ending
339  data.userDataRaw().rows == 1 &&
340  memcmp(data.userDataRaw().data, "GOAL:", 5) == 0)
341  {
342  //GOAL format detected, remove it from the user data and send it as goal event
343  std::string goalStr = (const char *)data.userDataRaw().data;
344  if(!goalStr.empty())
345  {
346  std::list<std::string> strs = uSplit(goalStr, ':');
347  if(strs.size() == 2)
348  {
349  goalId = *strs.rbegin();
350  data.setUserData(cv::Mat());
351 
352  double delay = 0.0;
353  if(!_ignoreGoalDelay && _currentId != _ids.end())
354  {
355  // get stamp for the next signature to compute the delay
356  // that was used originally for planning
357  int weight;
358  std::string label;
359  double stamp;
360  int mapId;
361  Transform localTransform, pose, groundTruth;
362  std::vector<float> velocity;
363  GPS gps;
364  EnvSensors sensors;
365  _dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors);
366  if(previousStamp && stamp && stamp > previousStamp)
367  {
368  delay = stamp - previousStamp;
369  }
370  }
371 
372  if(delay > 0.0)
373  {
374  UWARN("Goal \"%s\" detected, posting it! Waiting %f seconds before sending next data...",
375  goalId.c_str(), delay);
376  }
377  else
378  {
379  UWARN("Goal \"%s\" detected, posting it!", goalId.c_str());
380  }
381 
382  if(uIsInteger(goalId))
383  {
384  UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, atoi(goalId.c_str())));
385  }
386  else
387  {
388  UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId));
389  }
390 
391  if(delay > 0.0)
392  {
393  uSleep(delay*1000);
394  }
395  }
396  }
397  }
398  }
399  return data;
400 }
401 
402 SensorData DBReader::getNextData(SensorCaptureInfo * info)
403 {
404  SensorData data;
405  if(_dbDriver)
406  {
407  while(_currentId != _ids.end())
408  {
409  std::list<int> signIds;
410  signIds.push_back(*_currentId);
411  std::list<Signature *> signatures;
412  _dbDriver->loadSignatures(signIds, signatures);
413  if(signatures.empty())
414  {
415  return data;
416  }
417  _dbDriver->loadNodeData(signatures);
418  Signature * s = signatures.front();
419 
420  if(_intermediateNodesIgnored && s->getWeight() == -1)
421  {
422  UDEBUG("Ignoring node %d (intermediate nodes ignored)", s->id());
423  ++_currentId;
424  delete s;
425  continue;
426  }
427 
428  if(s->mapId() < _startMapId || (_stopMapId>=0 && s->mapId() > _stopMapId))
429  {
430  UDEBUG("Ignoring node %d (map id=%d, min=%d max=%d)", s->id(), s->mapId(), _startMapId, _stopMapId);
431  ++_currentId;
432  delete s;
433  continue;
434  }
435 
436  data = s->sensorData();
437 
438  // info
439  Transform pose = s->getPose();
440  Transform globalPose;
441  cv::Mat globalPoseCov;
442 
443  std::multimap<int, Link> priorLinks;
444  if(!_priorsIgnored)
445  {
446  _dbDriver->loadLinks(*_currentId, priorLinks, Link::kPosePrior);
447  if( priorLinks.size() &&
448  !priorLinks.begin()->second.transform().isNull() &&
449  priorLinks.begin()->second.infMatrix().cols == 6 &&
450  priorLinks.begin()->second.infMatrix().rows == 6)
451  {
452  globalPose = priorLinks.begin()->second.transform();
453  globalPoseCov = priorLinks.begin()->second.infMatrix().inv();
454  if(data.gps().stamp() != 0.0 &&
455  globalPoseCov.at<double>(3,3)>=9999 &&
456  globalPoseCov.at<double>(4,4)>=9999 &&
457  globalPoseCov.at<double>(5,5)>=9999)
458  {
459  // clear global pose as GPS was used for prior
460  globalPose.setNull();
461  }
462  }
463  }
464 
465  Transform gravityTransform;
466  std::multimap<int, Link> gravityLinks;
467  _dbDriver->loadLinks(*_currentId, gravityLinks, Link::kGravity);
468  if( gravityLinks.size() &&
469  !gravityLinks.begin()->second.transform().isNull() &&
470  gravityLinks.begin()->second.infMatrix().cols == 6 &&
471  gravityLinks.begin()->second.infMatrix().rows == 6)
472  {
473  gravityTransform = gravityLinks.begin()->second.transform();
474  }
475 
476  Landmarks landmarks;
477  if(!_landmarksIgnored)
478  {
479  std::multimap<int, Link> landmarkLinks;
480  _dbDriver->loadLinks(*_currentId, landmarkLinks, Link::kLandmark);
481  for(std::multimap<int, Link>::iterator iter=landmarkLinks.begin(); iter!=landmarkLinks.end(); ++iter)
482  {
483  cv::Mat landmarkSize = iter->second.uncompressUserDataConst();
484  landmarks.insert(std::make_pair(-iter->first,
485  Landmark(-iter->first,
486  !landmarkSize.empty() && landmarkSize.type() == CV_32FC1 && landmarkSize.total()==1?landmarkSize.at<float>(0,0):0.0f,
487  iter->second.transform(),
488  iter->second.infMatrix().inv())));
489  }
490  }
491 
492  cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
493  if(!_odometryIgnored)
494  {
495  std::multimap<int, Link> links;
496  _dbDriver->loadLinks(*_currentId, links, Link::kNeighbor);
497  if(links.size() && links.begin()->first < *_currentId)
498  {
499  // assume the first is the backward neighbor, take its variance
500  infMatrix = links.begin()->second.infMatrix();
501  _previousInfMatrix = infMatrix;
502  }
503  else
504  {
505  if(_previousMapId != s->mapId())
506  {
507  // first node, set high variance to make rtabmap trigger a new map
508  infMatrix /= 9999.0;
509  UDEBUG("First node of map %d, variance set to 9999", s->mapId());
510  }
511  else
512  {
513  // if localization data saved in database, covariance will be set in a prior link
514  _dbDriver->loadLinks(*_currentId, links, Link::kPosePrior);
515  if(links.size())
516  {
517  // assume the first is the backward neighbor, take its variance
518  infMatrix = links.begin()->second.infMatrix();
519  _previousInfMatrix = infMatrix;
520  }
521  else
522  {
523  if(_previousInfMatrix.empty())
524  {
525  _previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
526  }
527  // we have a node not linked to map, use last variance
528  infMatrix = _previousInfMatrix;
529  }
530  }
531  }
532  _previousMapId = s->mapId();
533  }
534  else
535  {
536  pose.setNull();
537  }
538 
539  int seq = *_currentId;
540  ++_currentId;
541 
542  // Frame rate
543  if(this->getImageRate() < 0.0f)
544  {
545  if(s->getStamp() == 0)
546  {
547  UERROR("The option to use database stamps is set (framerate<0), but there are no stamps saved in the database! Aborting...");
548  delete s;
549  return data;
550  }
551  else if(_previousMapID == s->mapId() && _previousStamp > 0)
552  {
553  float ratio = -this->getImageRate();
554  int sleepTime = 1000.0*(s->getStamp()-_previousStamp)/ratio - 1000.0*_timer.getElapsedTime();
555  double stamp = s->getStamp();
556  if(sleepTime > 10000)
557  {
558  UWARN("Detected long delay (%d sec, stamps = %f vs %f). Waiting a maximum of 10 seconds.",
559  sleepTime/1000, _previousStamp, s->getStamp());
560  sleepTime = 10000;
561  stamp = _previousStamp+10;
562  }
563  if(sleepTime > 2)
564  {
565  uSleep(sleepTime-2);
566  }
567 
568  // Add precision at the cost of a small overhead
569  while(_timer.getElapsedTime() < (stamp-_previousStamp)/ratio-0.000001)
570  {
571  //
572  }
573 
574  double slept = _timer.getElapsedTime();
575  _timer.start();
576  UDEBUG("slept=%fs vs target=%fs (ratio=%f)", slept, (stamp-_previousStamp)/ratio, ratio);
577  }
578  _previousStamp = s->getStamp();
579  _previousMapID = s->mapId();
580  }
581 
582  data.uncompressData();
583  std::map<int, int> cameraOldNewIndices;
584  std::vector<CameraModel> dbModels = data.cameraModels();
585  if(dbModels.empty() && !data.stereoCameraModels().empty())
586  {
587  for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
588  {
589  dbModels.push_back(data.stereoCameraModels()[i].left());
590  }
591  }
592 
593  if(!_cameraLocalTransformOverrides.empty() &&
594  !_cameraIndices.empty() &&
595  _cameraIndices.size() != _cameraLocalTransformOverrides.size())
596  {
597  UERROR("Camera local transform overrides (%d) are not the same size than the camera indices (%d). The overrides are ignored.",
598  _cameraLocalTransformOverrides.size(),
599  _cameraIndices.size()
600  );
601  _cameraLocalTransformOverrides.clear();
602  }
603 
604  std::vector<Transform> combinedLocalTransforms;
605  if(dbModels.size() > 1 && !_cameraIndices.empty())
606  {
607  // update images and local transforms
608  cv::Mat combinedImages;
609  cv::Mat combinedDepthImages;
610  std::vector<CameraModel> combinedModels;
611  std::vector<StereoCameraModel> combinedStereoModels;
612  for(size_t i=0; i<_cameraIndices.size(); ++i)
613  {
614  UASSERT_MSG(_cameraIndices[i] < dbModels.size(), uFormat("DBReader: camera index %ld is not valid (should be between 0 and %ld)",
615  _cameraIndices[i], dbModels.size()-1).c_str());
616 
617  int addedCameras = std::max(combinedModels.size(), combinedStereoModels.size());
618 
619  int subImageWidth = data.imageRaw().cols/dbModels.size();
620  UASSERT(!data.imageRaw().empty() &&
621  data.imageRaw().cols % dbModels.size() == 0 &&
622  (int)_cameraIndices[i]*subImageWidth < data.imageRaw().cols);
623  if(combinedImages.empty())
624  {
625  // initialize with first camera
626  combinedImages = cv::Mat(data.imageRaw().rows, subImageWidth*(_cameraIndices.size()-i), data.imageRaw().type());
627  }
628 
629  cv::Mat fromROI = cv::Mat(data.imageRaw(), cv::Rect(_cameraIndices[i]*subImageWidth, 0, subImageWidth, data.imageRaw().rows));
630  cv::Mat toROI = cv::Mat(combinedImages, cv::Rect(addedCameras*subImageWidth, 0, subImageWidth, combinedImages.rows));
631  fromROI.copyTo(toROI);
632 
633  cv::Mat depth;
634  if(!data.depthOrRightRaw().empty())
635  {
636  subImageWidth = data.depthOrRightRaw().cols/dbModels.size();
637  UASSERT(data.depthOrRightRaw().cols % dbModels.size() == 0 &&
638  subImageWidth == data.depthOrRightRaw().cols/(int)dbModels.size() &&
639  (int)_cameraIndices[i]*subImageWidth < data.depthOrRightRaw().cols);
640  if(combinedDepthImages.empty())
641  {
642  // initialize with first camera
643  combinedDepthImages = cv::Mat(data.depthOrRightRaw().rows, subImageWidth*(_cameraIndices.size()-i), data.depthOrRightRaw().type());
644  }
645  fromROI = cv::Mat(data.depthOrRightRaw(), cv::Rect(_cameraIndices[i]*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows));
646  toROI = cv::Mat(combinedDepthImages, cv::Rect(addedCameras*subImageWidth, 0, subImageWidth, combinedDepthImages.rows));
647  fromROI.copyTo(toROI);
648  }
649 
650  if(!data.cameraModels().empty())
651  {
652  CameraModel model = data.cameraModels()[_cameraIndices[i]];
653  if(!_cameraLocalTransformOverrides.empty())
654  {
655  model.setLocalTransform(_cameraLocalTransformOverrides[i] * CameraModel::opticalRotation());
656  }
657  combinedModels.push_back(model);
658  combinedLocalTransforms.push_back(model.localTransform());
659  }
660  else
661  {
662  StereoCameraModel stereoModel = data.stereoCameraModels()[_cameraIndices[i]];
663  if(!_cameraLocalTransformOverrides.empty())
664  {
665  stereoModel.setLocalTransform(_cameraLocalTransformOverrides[i] * CameraModel::opticalRotation());
666  }
667  combinedStereoModels.push_back(stereoModel);
668  combinedLocalTransforms.push_back(stereoModel.localTransform());
669  }
670  cameraOldNewIndices.insert(std::make_pair(_cameraIndices[i], i));
671  }
672  if(!combinedModels.empty())
673  {
674  data.setRGBDImage(combinedImages, combinedDepthImages, combinedModels);
675  }
676  else
677  {
678  data.setStereoImage(combinedImages, combinedDepthImages, combinedStereoModels);
679  }
680  }
681  else if(!_cameraLocalTransformOverrides.empty() &&
682  _cameraLocalTransformOverrides.size() == dbModels.size())
683  {
684  // just update local transforms
685  std::vector<CameraModel> combinedModels;
686  std::vector<StereoCameraModel> combinedStereoModels;
687  for(size_t i=0; i<dbModels.size(); ++i)
688  {
689  if(!data.cameraModels().empty())
690  {
691  CameraModel model = data.cameraModels()[i];
692  model.setLocalTransform(_cameraLocalTransformOverrides[i] * CameraModel::opticalRotation());
693  combinedModels.push_back(model);
694  combinedLocalTransforms.push_back(model.localTransform());
695  }
696  else
697  {
698  StereoCameraModel stereoModel = data.stereoCameraModels()[i];
699  stereoModel.setLocalTransform(_cameraLocalTransformOverrides[i] * CameraModel::opticalRotation());
700  combinedStereoModels.push_back(stereoModel);
701  combinedLocalTransforms.push_back(stereoModel.localTransform());
702  }
703  }
704  if(!combinedModels.empty())
705  {
706  data.setCameraModels(combinedModels);
707  }
708  else
709  {
710  data.setStereoCameraModels(combinedStereoModels);
711  }
712  }
713  data.setId(seq);
714  data.setStamp(s->getStamp());
715  data.setGroundTruth(s->getGroundTruthPose());
716  if(!globalPose.isNull())
717  {
718  data.setGlobalPose(globalPose, globalPoseCov);
719  }
720  if(!gravityTransform.isNull())
721  {
722  Eigen::Quaterniond q = gravityTransform.getQuaterniond();
723  data.setIMU(IMU(
724  cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat::eye(3,3,CV_64FC1),
725  cv::Vec3d(), cv::Mat(),
726  cv::Vec3d(), cv::Mat(),
727  Transform::getIdentity())); // we assume that gravity links are already transformed in base_link
728  }
729  data.setLandmarks(landmarks);
730 
731  UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, Grid=%d, UserData=%d, GlobalPose=%d, GPS=%d, IMU=%d",
732  data.laserScanRaw().isEmpty()?0:1,
733  data.imageRaw().empty()?0:1,
734  data.depthOrRightRaw().empty()?0:1,
735  data.gridCellSize()==0.0f?0:1,
736  data.userDataRaw().empty()?0:1,
737  globalPose.isNull()?0:1,
738  data.gps().stamp()!=0.0?1:0,
739  gravityTransform.isNull()?0:1);
740 
741  cv::Mat descriptors = s->getWordsDescriptors().clone();
742  const std::vector<cv::KeyPoint> & keypoints = s->getWordsKpts();
743  const std::vector<cv::Point3f> & keypoints3D = s->getWords3();
744  if(!_featuresIgnored &&
745  !keypoints.empty() &&
746  (keypoints3D.empty() || keypoints.size() == keypoints3D.size()) &&
747  (descriptors.empty() || (int)keypoints.size() == descriptors.rows))
748  {
749  if(!cameraOldNewIndices.empty())
750  {
751  cv::Mat newDescriptors;
752  std::vector<cv::KeyPoint> newKeypoints;
753  std::vector<cv::Point3f> newKeypoints3D;
754  UASSERT(!dbModels.empty() && dbModels[0].imageWidth()>0);
755  int subImageWidth = dbModels[0].imageWidth();
756  for(size_t i = 0; i<keypoints.size(); ++i)
757  {
758  int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
759  UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)dbModels.size(),
760  uFormat("cameraIndex=%d, db models=%d, kpt.x=%f, image width=%d",
761  cameraIndex, (int)dbModels.size(), keypoints[i].pt.x, subImageWidth).c_str());
762  if(cameraOldNewIndices.find(cameraIndex) != cameraOldNewIndices.end())
763  {
764  int newCameraIndex = cameraOldNewIndices.at(cameraIndex);
765  newKeypoints.push_back(keypoints[i]);
766  newKeypoints.back().pt.x += (newCameraIndex-cameraIndex)*subImageWidth;
767  if(!keypoints3D.empty())
768  {
769  cv::Point3f pt = util3d::transformPoint(keypoints3D.at(i), dbModels[cameraIndex].localTransform().inverse());
770  pt = util3d::transformPoint(pt, combinedLocalTransforms[cameraIndex]);
771  newKeypoints3D.push_back(pt);
772  }
773  if(!descriptors.empty())
774  {
775  newDescriptors.push_back(descriptors.row(i));
776  }
777  }
778  }
779  data.setFeatures(newKeypoints, newKeypoints3D, newDescriptors);
780  }
781  else if(!combinedLocalTransforms.empty())
782  {
783  // We are overriding the camra local transforms, let's move 3D words accordingly
784  UASSERT(dbModels.size() == combinedLocalTransforms.size());
785  std::vector<cv::Point3f> newKeypoints3D;
786  for(size_t i = 0; i<keypoints3D.size(); ++i)
787  {
788  cv::Point3f pt = util3d::transformPoint(keypoints3D.at(i), dbModels[i].localTransform().inverse());
789  pt = util3d::transformPoint(pt, combinedLocalTransforms[i]);
790  newKeypoints3D.push_back(pt);
791  }
792  data.setFeatures(keypoints, newKeypoints3D, descriptors);
793  }
794  else
795  {
796  data.setFeatures(keypoints, keypoints3D, descriptors);
797  }
798  }
799  else if(!_featuresIgnored && !keypoints.empty() && (!keypoints3D.empty() || !descriptors.empty()))
800  {
801  UERROR("Missing feature data, features won't be published.");
802  }
803 
804  if(data.imageRaw().empty() && data.imageCompressed().empty() && s->getWeight()>=0 && keypoints.empty())
805  {
806  UWARN("No image loaded from the database for id=%d!", seq);
807  }
808 
809  if(!_odometryIgnored)
810  {
811  if(pose.isNull())
812  {
813  UWARN("Reading the database: odometry is null! "
814  "Please set \"Ignore odometry = true\" if there is "
815  "no odometry in the database.");
816  }
817  if(info)
818  {
819  info->odomPose = pose;
820  info->odomCovariance = infMatrix.inv();
821  info->odomVelocity = s->getVelocity();
822  UDEBUG("odom variance = %f/%f", info->odomCovariance.at<double>(0,0), info->odomCovariance.at<double>(5,5));
823  }
824  }
825  delete s;
826  break;
827  }
828  }
829  else
830  {
831  UERROR("Not initialized...");
832  }
833  return data;
834 }
835 
836 } /* namespace rtabmap */
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
int
int
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:686
rtabmap::DBReader::_calibrated
bool _calibrated
Definition: DBReader.h:122
rtabmap::DBDriver::getCalibration
bool getCalibration(int signatureId, std::vector< CameraModel > &models, std::vector< StereoCameraModel > &stereoModels) const
Definition: DBDriver.cpp:737
rtabmap::DBReader::_landmarksIgnored
bool _landmarksIgnored
Definition: DBReader.h:107
rtabmap::DBReader::getNextData
SensorData getNextData(SensorCaptureInfo *info=0)
Definition: DBReader.cpp:402
UINFO
#define UINFO(...)
rtabmap::StereoCameraModel
Definition: StereoCameraModel.h:35
rtabmap::RtabmapEventCmd
Definition: RtabmapEvent.h:57
rtabmap::DBReader::_previousMapID
int _previousMapID
Definition: DBReader.h:121
UTimer::now
static double now()
Definition: UTimer.cpp:80
s
RealScalar s
rtabmap::DBReader::_goalsIgnored
bool _goalsIgnored
Definition: DBReader.h:102
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:300
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
size
Index size
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::DBDriver::loadSignature
Signature * loadSignature(int id, bool *loadedFromTrash=0)
Definition: DBDriver.cpp:548
rtabmap::DBReader::_previousInfMatrix
cv::Mat _previousInfMatrix
Definition: DBReader.h:119
rtabmap::DBReader::getPose
virtual bool getPose(double stamp, Transform &pose, cv::Mat &covariance, double maxWaitTime=0.06)
Definition: DBReader.cpp:294
end
end
rtabmap::GPS
Definition: GPS.h:35
rtabmap::DBReader::_startId
int _startId
Definition: DBReader.h:103
rtabmap::DBReader::_ids
std::set< int > _ids
Definition: DBReader.h:116
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:111
rtabmap::DBDriver::loadNodeData
void loadNodeData(Signature *signature, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const
Definition: DBDriver.cpp:659
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::StereoCameraModel::setLocalTransform
void setLocalTransform(const Transform &transform)
Definition: StereoCameraModel.h:118
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::DBReader::_previousStamp
double _previousStamp
Definition: DBReader.h:120
rtabmap::Landmark
Definition: Landmark.h:39
rtabmap::SensorCaptureInfo
Definition: SensorCaptureInfo.h:36
glm::isNull
GLM_FUNC_DECL bool isNull(detail::tmat2x2< T, P > const &m, T const &epsilon)
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_CORE_EXPORT transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::DBReader::_ignoreGoalDelay
bool _ignoreGoalDelay
Definition: DBReader.h:101
rtabmap::DBReader::getSerial
virtual std::string getSerial() const
Definition: DBReader.cpp:289
rtabmap::IMU
Definition: IMU.h:19
rtabmap::DBReader::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: DBReader.cpp:169
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[]
rtabmap::DBReader::DBReader
DBReader(const std::string &databasePath, float frameRate=0.0f, bool odometryIgnored=false, bool ignoreGoalDelay=false, bool goalsIgnored=false, int startId=0, const std::vector< unsigned int > &cameraIndices=std::vector< unsigned int >(), int stopId=0, bool intermediateNodesIgnored=false, bool landmarksIgnored=false, bool featuresIgnored=false, int startMapId=0, int stopMapId=-1, bool priorsIgnored=false, const std::vector< Transform > &cameraLocalTransformOverrides=std::vector< Transform >())
Definition: DBReader.cpp:45
q
EIGEN_DEVICE_FUNC const Scalar & q
UConversion.h
Some conversion functions.
rtabmap::DBReader::isCalibrated
virtual bool isCalibrated() const
Definition: DBReader.cpp:284
rtabmap::DBReader::_stopId
int _stopId
Definition: DBReader.h:104
rtabmap::DBReader::checkArguments
void checkArguments()
Definition: DBReader.cpp:125
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
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:787
info
else if n * info
rtabmap::StereoCameraModel::localTransform
const Transform & localTransform() const
Definition: StereoCameraModel.h:119
UASSERT
#define UASSERT(condition)
rtabmap::RtabmapEventCmd::kCmdGoal
@ kCmdGoal
Definition: RtabmapEvent.h:77
rtabmap::DBReader::_previousMapId
int _previousMapId
Definition: DBReader.h:118
DBDriver.h
rtabmap::DBReader::_dbDriver
DBDriver * _dbDriver
Definition: DBReader.h:114
rtabmap::CameraModel::opticalRotation
static Transform opticalRotation()
Definition: CameraModel.h:45
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:106
rtabmap::DBDriver::loadLinks
void loadLinks(int signatureId, std::multimap< int, Link > &links, Link::Type type=Link::kUndef) const
Definition: DBDriver.cpp:826
rtabmap::DBReader::~DBReader
virtual ~DBReader()
Definition: DBReader.cpp:160
path
path
rtabmap::DBReader::_timer
UTimer _timer
Definition: DBReader.h:115
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
UWARN
#define UWARN(...)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
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:109
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:110
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:887
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:108
rtabmap::Transform::getQuaterniond
Eigen::Quaterniond getQuaterniond() const
Definition: Transform.cpp:406
UERROR
#define UERROR(...)
trace.model
model
Definition: trace.py:4
rtabmap::DBReader::_cameraIndices
std::vector< unsigned int > _cameraIndices
Definition: DBReader.h:105
rtabmap::DBReader::_cameraLocalTransformOverrides
std::vector< Transform > _cameraLocalTransformOverrides
Definition: DBReader.h:112
rtabmap::DBDriver::loadSignatures
void loadSignatures(const std::list< int > &ids, std::list< Signature * > &signatures, std::set< int > *loadedFromTrash=0)
Definition: DBDriver.cpp:565
i
int i
rtabmap::DBReader::_currentId
std::set< int >::iterator _currentId
Definition: DBReader.h:117
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:99
rtabmap::Signature
Definition: Signature.h:48
rtabmap::DBReader::_odometryIgnored
bool _odometryIgnored
Definition: DBReader.h:100
rtabmap::DBDriver::create
static DBDriver * create(const ParametersMap &parameters=ParametersMap())
Definition: DBDriver.cpp:41

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:45:53