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/DBReader.h"
29 #include "rtabmap/core/DBDriver.h"
30 
31 #include <rtabmap/utilite/ULogger.h>
32 #include <rtabmap/utilite/UFile.h>
33 #include <rtabmap/utilite/UStl.h>
34 #include <rtabmap/utilite/UConversion.h>
35 #include <rtabmap/utilite/UEventsManager.h>
36 
37 #include "rtabmap/core/CameraEvent.h"
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 startIndex,
51  int cameraIndex) :
52  Camera(frameRate),
53  _paths(uSplit(databasePath, ';')),
54  _odometryIgnored(odometryIgnored),
55  _ignoreGoalDelay(ignoreGoalDelay),
56  _goalsIgnored(goalsIgnored),
57  _startIndex(startIndex),
58  _cameraIndex(cameraIndex),
59  _dbDriver(0),
60  _currentId(_ids.end()),
61  _previousMapId(-1),
62  _previousStamp(0),
63  _previousMapID(0),
64  _calibrated(false)
65 {
66 }
67 
68 DBReader::DBReader(const std::list<std::string> & databasePaths,
69  float frameRate,
70  bool odometryIgnored,
71  bool ignoreGoalDelay,
72  bool goalsIgnored,
73  int startIndex,
74  int cameraIndex) :
75  Camera(frameRate),
76  _paths(databasePaths),
77  _odometryIgnored(odometryIgnored),
78  _ignoreGoalDelay(ignoreGoalDelay),
79  _goalsIgnored(goalsIgnored),
80  _startIndex(startIndex),
81  _cameraIndex(cameraIndex),
82  _dbDriver(0),
83  _currentId(_ids.end()),
84  _previousMapId(-1),
85  _previousStamp(0),
86  _previousMapID(0),
87  _calibrated(false)
88 {
89 }
90 
91 DBReader::~DBReader()
92 {
93  if(_dbDriver)
94  {
95  _dbDriver->closeConnection();
96  delete _dbDriver;
97  }
98 }
99 
100 bool DBReader::init(
101  const std::string & calibrationFolder,
102  const std::string & cameraName)
103 {
104  if(_dbDriver)
105  {
106  _dbDriver->closeConnection();
107  delete _dbDriver;
108  _dbDriver = 0;
109  }
110  _ids.clear();
111  _currentId=_ids.end();
112  _previousMapId = -1;
113  _previousInfMatrix = cv::Mat();
114  _previousStamp = 0;
115  _previousMapID = 0;
116  _calibrated = false;
117 
118  if(_paths.size() == 0)
119  {
120  UERROR("No database path set...");
121  return false;
122  }
123 
124  std::string path = _paths.front();
125  if(!UFile::exists(path))
126  {
127  UERROR("Database path does not exist (%s)", path.c_str());
128  return false;
129  }
130 
131  rtabmap::ParametersMap parameters;
132  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
133  _dbDriver = DBDriver::create(parameters);
134  if(!_dbDriver)
135  {
136  UERROR("Driver doesn't exist.");
137  return false;
138  }
139  if(!_dbDriver->openConnection(path))
140  {
141  UERROR("Can't open database %s", path.c_str());
142  delete _dbDriver;
143  _dbDriver = 0;
144  return false;
145  }
146 
147  _dbDriver->getAllNodeIds(_ids);
148  _currentId = _ids.begin();
149  if(_startIndex>0 && _ids.size())
150  {
151  std::set<int>::iterator iter = uIteratorAt(_ids, _startIndex);
152  if(iter == _ids.end())
153  {
154  UWARN("Start index is too high (%d), the last in database is %d. Starting from beginning...", _startIndex, _ids.size()-1);
155  }
156  else
157  {
158  _currentId = iter;
159  }
160  }
161 
162  if(_ids.size())
163  {
164  std::vector<CameraModel> models;
165  StereoCameraModel stereoModel;
166  if(_dbDriver->getCalibration(*_ids.begin(), models, stereoModel))
167  {
168  if(models.size())
169  {
170  if(models.at(0).isValidForProjection())
171  {
172  _calibrated = true;
173  }
174  else if(models.at(0).fx() && models.at(0).fy() && models.at(0).imageWidth() == 0)
175  {
176  // backward compatibility for databases not saving cx,cy and imageSize
177  SensorData data;
178  _dbDriver->getNodeData(*_ids.begin(), data, true, false, false, false);
179  cv::Mat rgb;
180  data.uncompressData(&rgb, 0); // this will update camera models if old format
181  if(data.cameraModels().size() && data.cameraModels().at(0).isValidForProjection())
182  {
183  _calibrated = true;
184  }
185  }
186  }
187  else if(stereoModel.isValidForProjection())
188  {
189  _calibrated = true;
190  }
191  }
192  }
193  else
194  {
195  _calibrated = true; // database is empty, make sure calibration warning is not shown.
196  }
197 
198  _timer.start();
199 
200  return true;
201 }
202 
203 bool DBReader::isCalibrated() const
204 {
205  return _calibrated;
206 }
207 
208 std::string DBReader::getSerial() const
209 {
210  return "DBReader";
211 }
212 
213 SensorData DBReader::captureImage(CameraInfo * info)
214 {
215  SensorData data = this->getNextData(info);
216  if(data.id() == 0)
217  {
218  UINFO("no more images...");
219  while(_paths.size() > 1 && data.id() == 0)
220  {
221  _paths.pop_front();
222  UWARN("Loading next database \"%s\"...", _paths.front().c_str());
223  if(!this->init())
224  {
225  UERROR("Failed to initialize the next database \"%s\"", _paths.front().c_str());
226  return data;
227  }
228  else
229  {
230  data = this->getNextData(info);
231  }
232  }
233  }
234  if(data.id())
235  {
236  std::string goalId;
237  double previousStamp = data.stamp();
238  if(previousStamp == 0)
239  {
240  data.setStamp(UTimer::now());
241  }
242 
243  if(!_goalsIgnored &&
244  data.userDataRaw().type() == CV_8SC1 &&
245  data.userDataRaw().cols >= 7 && // including null str ending
246  data.userDataRaw().rows == 1 &&
247  memcmp(data.userDataRaw().data, "GOAL:", 5) == 0)
248  {
249  //GOAL format detected, remove it from the user data and send it as goal event
250  std::string goalStr = (const char *)data.userDataRaw().data;
251  if(!goalStr.empty())
252  {
253  std::list<std::string> strs = uSplit(goalStr, ':');
254  if(strs.size() == 2)
255  {
256  goalId = *strs.rbegin();
257  data.setUserData(cv::Mat());
258 
259  double delay = 0.0;
260  if(!_ignoreGoalDelay && _currentId != _ids.end())
261  {
262  // get stamp for the next signature to compute the delay
263  // that was used originally for planning
264  int weight;
265  std::string label;
266  double stamp;
267  int mapId;
268  Transform localTransform, pose, groundTruth;
269  std::vector<float> velocity;
270  GPS gps;
271  _dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps);
272  if(previousStamp && stamp && stamp > previousStamp)
273  {
274  delay = stamp - previousStamp;
275  }
276  }
277 
278  if(delay > 0.0)
279  {
280  UWARN("Goal \"%s\" detected, posting it! Waiting %f seconds before sending next data...",
281  goalId.c_str(), delay);
282  }
283  else
284  {
285  UWARN("Goal \"%s\" detected, posting it!", goalId.c_str());
286  }
287 
288  if(uIsInteger(goalId))
289  {
290  UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, atoi(goalId.c_str())));
291  }
292  else
293  {
294  UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId));
295  }
296 
297  if(delay > 0.0)
298  {
299  uSleep(delay*1000);
300  }
301  }
302  }
303  }
304  }
305  return data;
306 }
307 
308 SensorData DBReader::getNextData(CameraInfo * info)
309 {
310  SensorData data;
311  if(_dbDriver)
312  {
313  if(_currentId != _ids.end())
314  {
315  int mapId;
316  _dbDriver->getNodeData(*_currentId, data);
317 
318  // info
319  Transform pose;
320  int weight;
321  std::string label;
322  double stamp;
323  Transform groundTruth;
324  std::vector<float> velocity;
325  GPS gps;
326  _dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps);
327 
328  cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
329  if(!_odometryIgnored)
330  {
331  std::map<int, Link> links;
332  _dbDriver->loadLinks(*_currentId, links, Link::kNeighbor);
333  if(links.size() && links.begin()->first < *_currentId)
334  {
335  // assume the first is the backward neighbor, take its variance
336  infMatrix = links.begin()->second.infMatrix();
337  _previousInfMatrix = infMatrix;
338  }
339  else if(_previousMapId != mapId)
340  {
341  // first node, set high variance to make rtabmap trigger a new map
342  infMatrix /= 9999.0;
343  UDEBUG("First node of map %d, variance set to 9999", mapId);
344  }
345  else
346  {
347  if(_previousInfMatrix.empty())
348  {
349  _previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
350  }
351  // we have a node not linked to map, use last variance
352  infMatrix = _previousInfMatrix;
353  }
354  _previousMapId = mapId;
355  }
356  else
357  {
358  pose.setNull();
359  }
360 
361  int seq = *_currentId;
362  ++_currentId;
363  if(data.imageCompressed().empty() && weight>=0)
364  {
365  UWARN("No image loaded from the database for id=%d!", *_currentId);
366  }
367 
368  // Frame rate
369  if(this->getImageRate() < 0.0f)
370  {
371  if(stamp == 0)
372  {
373  UERROR("The option to use database stamps is set (framerate<0), but there are no stamps saved in the database! Aborting...");
374  return data;
375  }
376  else if(_previousMapID == mapId && _previousStamp > 0)
377  {
378  int sleepTime = 1000.0*(stamp-_previousStamp) - 1000.0*_timer.getElapsedTime();
379  if(sleepTime > 10000)
380  {
381  UWARN("Detected long delay (%d sec, stamps = %f vs %f). Waiting a maximum of 10 seconds.",
382  sleepTime/1000, _previousStamp, stamp);
383  sleepTime = 10000;
384  }
385  if(sleepTime > 2)
386  {
387  uSleep(sleepTime-2);
388  }
389 
390  // Add precision at the cost of a small overhead
391  while(_timer.getElapsedTime() < (stamp-_previousStamp)-0.000001)
392  {
393  //
394  }
395 
396  double slept = _timer.getElapsedTime();
397  _timer.start();
398  UDEBUG("slept=%fs vs target=%fs", slept, stamp-_previousStamp);
399  }
400  _previousStamp = stamp;
401  _previousMapID = mapId;
402  }
403  else
404  {
405  stamp = 0;
406  }
407 
408  data.uncompressData();
409  if(data.cameraModels().size() > 1 &&
410  _cameraIndex >= 0)
411  {
412  if(_cameraIndex < (int)data.cameraModels().size())
413  {
414  // select one camera
415  int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
416  UASSERT(!data.imageRaw().empty() &&
417  data.imageRaw().cols % data.cameraModels().size() == 0 &&
418  _cameraIndex*subImageWidth < data.imageRaw().cols);
419  data.setImageRaw(
420  cv::Mat(data.imageRaw(),
421  cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.imageRaw().rows)).clone());
422 
423  if(!data.depthOrRightRaw().empty())
424  {
425  UASSERT(data.depthOrRightRaw().cols % data.cameraModels().size() == 0 &&
426  subImageWidth == data.depthOrRightRaw().cols/(int)data.cameraModels().size() &&
427  _cameraIndex*subImageWidth < data.depthOrRightRaw().cols);
428  data.setDepthOrRightRaw(
429  cv::Mat(data.depthOrRightRaw(),
430  cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows)).clone());
431  }
432  CameraModel model = data.cameraModels().at(_cameraIndex);
433  data.setCameraModel(model);
434  }
435  else
436  {
437  UWARN("DBReader: Camera index %d doesn't exist! Camera models = %d.", _cameraIndex, (int)data.cameraModels().size());
438  }
439  }
440  data.setId(seq);
441  data.setStamp(stamp);
442  data.setGroundTruth(groundTruth);
443  data.setGPS(gps);
444  UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, UserData=%d",
445  data.laserScanRaw().isEmpty()?0:1,
446  data.imageRaw().empty()?0:1,
447  data.depthOrRightRaw().empty()?0:1,
448  data.userDataRaw().empty()?0:1);
449 
450  if(!_odometryIgnored)
451  {
452  if(pose.isNull())
453  {
454  UWARN("Reading the database: odometry is null! "
455  "Please set \"Ignore odometry = true\" if there is "
456  "no odometry in the database.");
457  }
458  if(info)
459  {
460  info->odomPose = pose;
461  info->odomCovariance = infMatrix.inv();
462  info->odomVelocity = velocity;
463  UDEBUG("odom variance = %f/%f", info->odomCovariance.at<double>(0,0), info->odomCovariance.at<double>(5,5));
464  }
465  }
466  }
467  }
468  else
469  {
470  UERROR("Not initialized...");
471  }
472  return data;
473 }
474 
475 } /* namespace rtabmap */
uIsInteger
bool uIsInteger(const std::string &str, bool checkForSign=true)
Definition: UStl.h:632
rtabmap::CameraInfo::odomCovariance
cv::Mat odomCovariance
Definition: CameraInfo.h:69
rtabmap::DBReader::_paths
std::list< std::string > _paths
Definition: DBReader.h:79
rtabmap::DBReader::_previousInfMatrix
cv::Mat _previousInfMatrix
Definition: DBReader.h:91
UEventsManager::post
static void post(UEvent *event, bool async=true, const UEventsSender *sender=0)
Definition: UEventsManager.cpp:54
rtabmap::DBReader::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: DBReader.cpp:100
rtabmap::DBReader::getNextData
SensorData getNextData(CameraInfo *info=0)
Definition: DBReader.cpp:308
f
f
rtabmap::DBDriver::loadLinks
void loadLinks(int signatureId, std::map< int, Link > &links, Link::Type type=Link::kUndef) const
Definition: DBDriver.cpp:763
rtabmap::DBDriver::getAllNodeIds
void getAllNodeIds(std::set< int > &ids, bool ignoreChildren=false, bool ignoreBadSignatures=false) const
Definition: DBDriver.cpp:813
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:42
rtabmap::DBReader::_goalsIgnored
bool _goalsIgnored
Definition: DBReader.h:82
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:69
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:642
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:41
rtabmap::DBReader::_previousStamp
double _previousStamp
Definition: DBReader.h:92
rtabmap::SensorData::setGPS
void setGPS(const GPS &gps)
Definition: SensorData.h:238
rtabmap::SensorData::setId
void setId(int id)
Definition: SensorData.h:153
rtabmap::CameraInfo::odomVelocity
std::vector< float > odomVelocity
Definition: CameraInfo.h:70
UConversion.h
Some conversion functions.
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) const
Definition: DBDriver.cpp:727
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:163
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:161
rtabmap::SensorData::setUserData
void setUserData(const cv::Mat &userData)
Definition: SensorData.cpp:452
rtabmap::DBReader::DBReader
DBReader(const std::string &databasePath, float frameRate=0.0f, bool odometryIgnored=false, bool ignoreGoalDelay=false, bool goalsIgnored=false, int startIndex=0, int cameraIndex=-1)
Definition: DBReader.cpp:45
rtabmap::DBReader::~DBReader
virtual ~DBReader()
Definition: DBReader.cpp:91
UTimer::getElapsedTime
double getElapsedTime()
Definition: UTimer.cpp:90
rtabmap::SensorData::setImageRaw
void setImageRaw(const cv::Mat &imageRaw)
Definition: SensorData.h:164
uSplit
std::list< std::string > uSplit(const std::string &str, char separator= ' ')
Definition: UStl.h:566
rtabmap::SensorData::setCameraModel
void setCameraModel(const CameraModel &model)
Definition: SensorData.h:167
rtabmap::DBDriver::openConnection
bool openConnection(const std::string &url, bool overwritten=false)
Definition: DBDriver.cpp:86
UASSERT
#define UASSERT(condition)
UStl.h
Wrappers of STL for convenient functions.
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::DBReader::_ignoreGoalDelay
bool _ignoreGoalDelay
Definition: DBReader.h:81
rtabmap::DBDriver::closeConnection
void closeConnection(bool save=true, const std::string &outputUrl="")
Definition: DBDriver.cpp:64
rtabmap::DBReader::_dbDriver
DBDriver * _dbDriver
Definition: DBReader.h:86
rtabmap::DBReader::_ids
std::set< int > _ids
Definition: DBReader.h:88
rtabmap::DBDriver::getCalibration
bool getCalibration(int signatureId, std::vector< CameraModel > &models, StereoCameraModel &stereoModel) const
Definition: DBDriver.cpp:677
rtabmap::SensorData::depthOrRightRaw
const cv::Mat & depthOrRightRaw() const
Definition: SensorData.h:162
UTimer::start
void start()
Definition: UTimer.cpp:80
rtabmap::SensorData::userDataRaw
const cv::Mat & userDataRaw() const
Definition: SensorData.h:205
rtabmap::SensorData::id
int id() const
Definition: SensorData.h:152
rtabmap::SensorData::stamp
double stamp() const
Definition: SensorData.h:154
rtabmap::DBDriver::create
static DBDriver * create(const ParametersMap &parameters=ParametersMap())
Definition: DBDriver.cpp:41
false
#define false
Definition: ConvertUTF.c:56
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
rtabmap::Camera::getImageRate
float getImageRate() const
Definition: Camera.h:62
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:193
uIteratorAt
std::list< V >::iterator uIteratorAt(std::list< V > &list, const unsigned int &pos)
Definition: UStl.h:284
UDEBUG
#define UDEBUG(...)
UFile::exists
bool exists()
Definition: UFile.h:104
rtabmap::DBReader::isCalibrated
virtual bool isCalibrated() const
Definition: DBReader.cpp:203
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
UTimer::now
static double now()
Definition: UTimer.cpp:73
rtabmap::SensorData::setStamp
void setStamp(double stamp)
Definition: SensorData.h:155
rtabmap::SensorData::imageCompressed
const cv::Mat & imageCompressed() const
Definition: SensorData.h:157
rtabmap::DBReader::_currentId
std::set< int >::iterator _currentId
Definition: DBReader.h:89
rtabmap::SensorData::setGroundTruth
void setGroundTruth(const Transform &pose)
Definition: SensorData.h:231
rtabmap::DBReader::_odometryIgnored
bool _odometryIgnored
Definition: DBReader.h:80
rtabmap::CameraInfo::odomPose
Transform odomPose
Definition: CameraInfo.h:68
rtabmap::SensorData::setDepthOrRightRaw
void setDepthOrRightRaw(const cv::Mat &depthOrImageRaw)
Definition: SensorData.h:165
rtabmap::DBReader::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: DBReader.cpp:213
rtabmap::DBReader::getSerial
virtual std::string getSerial() const
Definition: DBReader.cpp:208
UINFO
#define UINFO(...)

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