00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "rtabmap/core/DBReader.h"
00029 #include "rtabmap/core/DBDriver.h"
00030 #include "DBDriverSqlite3.h"
00031
00032 #include <rtabmap/utilite/ULogger.h>
00033 #include <rtabmap/utilite/UFile.h>
00034 #include <rtabmap/utilite/UStl.h>
00035 #include <rtabmap/utilite/UConversion.h>
00036
00037 #include "rtabmap/core/CameraEvent.h"
00038 #include "rtabmap/core/RtabmapEvent.h"
00039 #include "rtabmap/core/OdometryEvent.h"
00040 #include "rtabmap/core/util3d.h"
00041 #include "rtabmap/core/Compression.h"
00042
00043 namespace rtabmap {
00044
00045 DBReader::DBReader(const std::string & databasePath,
00046 float frameRate,
00047 bool odometryIgnored,
00048 bool ignoreGoalDelay) :
00049 _paths(uSplit(databasePath, ';')),
00050 _frameRate(frameRate),
00051 _odometryIgnored(odometryIgnored),
00052 _ignoreGoalDelay(ignoreGoalDelay),
00053 _dbDriver(0),
00054 _currentId(_ids.end())
00055 {
00056 }
00057
00058 DBReader::DBReader(const std::list<std::string> & databasePaths,
00059 float frameRate,
00060 bool odometryIgnored,
00061 bool ignoreGoalDelay) :
00062 _paths(databasePaths),
00063 _frameRate(frameRate),
00064 _odometryIgnored(odometryIgnored),
00065 _ignoreGoalDelay(ignoreGoalDelay),
00066 _dbDriver(0),
00067 _currentId(_ids.end())
00068 {
00069 }
00070
00071 DBReader::~DBReader()
00072 {
00073 if(_dbDriver)
00074 {
00075 _dbDriver->closeConnection();
00076 delete _dbDriver;
00077 }
00078 }
00079
00080 bool DBReader::init(int startIndex)
00081 {
00082 if(_dbDriver)
00083 {
00084 _dbDriver->closeConnection();
00085 delete _dbDriver;
00086 _dbDriver = 0;
00087 }
00088 _ids.clear();
00089 _currentId=_ids.end();
00090 _previousStamp = 0;
00091
00092 if(_paths.size() == 0)
00093 {
00094 UERROR("No database path set...");
00095 return false;
00096 }
00097
00098 std::string path = _paths.front();
00099 if(!UFile::exists(path))
00100 {
00101 UERROR("Database path does not exist (%s)", path.c_str());
00102 return false;
00103 }
00104
00105 rtabmap::ParametersMap parameters;
00106 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
00107 _dbDriver = new DBDriverSqlite3(parameters);
00108 if(!_dbDriver)
00109 {
00110 UERROR("Driver doesn't exist.");
00111 return false;
00112 }
00113 if(!_dbDriver->openConnection(path))
00114 {
00115 UERROR("Can't open database %s", path.c_str());
00116 delete _dbDriver;
00117 _dbDriver = 0;
00118 return false;
00119 }
00120
00121 _dbDriver->getAllNodeIds(_ids);
00122 _currentId = _ids.begin();
00123 if(startIndex>0 && _ids.size())
00124 {
00125 std::set<int>::iterator iter = uIteratorAt(_ids, startIndex);
00126 if(iter == _ids.end())
00127 {
00128 UWARN("Start index is too high (%d), the last in database is %d. Starting from beginning...", startIndex, _ids.size()-1);
00129 }
00130 else
00131 {
00132 _currentId = iter;
00133 }
00134 }
00135
00136 return true;
00137 }
00138
00139 void DBReader::setFrameRate(float frameRate)
00140 {
00141 _frameRate = frameRate;
00142 }
00143
00144 void DBReader::mainLoopBegin()
00145 {
00146 _timer.start();
00147 }
00148
00149 void DBReader::mainLoop()
00150 {
00151 SensorData data = this->getNextData();
00152 if(data.isValid())
00153 {
00154 int goalId = 0;
00155 double previousStamp = data.stamp();
00156 data.setStamp(UTimer::now());
00157 if(data.userData().size() >= 6 && memcmp(data.userData().data(), "GOAL:", 5) == 0)
00158 {
00159
00160 std::string goalStr = uBytes2Str(data.userData());
00161 if(!goalStr.empty())
00162 {
00163 std::list<std::string> strs = uSplit(goalStr, ':');
00164 if(strs.size() == 2)
00165 {
00166 goalId = atoi(strs.rbegin()->c_str());
00167 data.setUserData(std::vector<unsigned char>());
00168 }
00169 }
00170 }
00171 if(!_odometryIgnored)
00172 {
00173 if(data.pose().isNull())
00174 {
00175 UWARN("Reading the database: odometry is null! "
00176 "Please set \"Ignore odometry = true\" if there is "
00177 "no odometry in the database.");
00178 }
00179 this->post(new OdometryEvent(data));
00180 }
00181 else
00182 {
00183 this->post(new CameraEvent(data));
00184 }
00185
00186 if(goalId > 0)
00187 {
00188 this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
00189
00190 if(!_ignoreGoalDelay && _currentId != _ids.end())
00191 {
00192
00193
00194 int weight;
00195 std::string label;
00196 double stamp;
00197 int mapId;
00198 Transform localTransform, pose;
00199 std::vector<unsigned char> userData;
00200 _dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, userData);
00201 if(previousStamp && stamp && stamp > previousStamp)
00202 {
00203 double delay = stamp - previousStamp;
00204 UWARN("Goal %d detected, posting it! Waiting %f seconds before sending next data...",
00205 goalId, delay);
00206 uSleep(delay*1000);
00207 }
00208 }
00209 else
00210 {
00211 UWARN("Goal %d detected, posting it!", goalId);
00212 }
00213 }
00214
00215 }
00216 else if(!this->isKilled())
00217 {
00218 UINFO("no more images...");
00219 if(_paths.size() > 1)
00220 {
00221 _paths.pop_front();
00222 UWARN("Loading next database \"%s\"...", _paths.front().c_str());
00223 if(!this->init())
00224 {
00225 UERROR("Failed to initialize the next database \"%s\"", _paths.front().c_str());
00226 this->kill();
00227 this->post(new CameraEvent());
00228 }
00229 }
00230 else
00231 {
00232 this->kill();
00233 this->post(new CameraEvent());
00234 }
00235
00236 }
00237
00238 }
00239
00240 SensorData DBReader::getNextData()
00241 {
00242 SensorData data;
00243 if(_dbDriver)
00244 {
00245 if(!this->isKilled() && _currentId != _ids.end())
00246 {
00247 cv::Mat imageBytes;
00248 cv::Mat depthBytes;
00249 cv::Mat laserScanBytes;
00250 int mapId;
00251 float fx,fy,cx,cy;
00252 Transform localTransform, pose;
00253 float rotVariance = 1.0f;
00254 float transVariance = 1.0f;
00255 std::vector<unsigned char> userData;
00256 int laserScanMaxPts = 0;
00257 _dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform, laserScanMaxPts);
00258
00259
00260 int weight;
00261 std::string label;
00262 double stamp;
00263 _dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, userData);
00264
00265 if(!_odometryIgnored)
00266 {
00267 std::map<int, Link> links;
00268 _dbDriver->loadLinks(*_currentId, links, Link::kNeighbor);
00269 if(links.size())
00270 {
00271
00272 rotVariance = links.begin()->second.rotVariance();
00273 transVariance = links.begin()->second.transVariance();
00274 }
00275 }
00276 else
00277 {
00278 pose.setNull();
00279 }
00280
00281 int seq = *_currentId;
00282 ++_currentId;
00283 if(imageBytes.empty())
00284 {
00285 UWARN("No image loaded from the database for id=%d!", *_currentId);
00286 }
00287
00288
00289 if(_frameRate < 0.0f)
00290 {
00291 if(stamp == 0)
00292 {
00293 UERROR("The option to use database stamps is set (framerate<0), but there are no stamps saved in the database! Aborting...");
00294 this->kill();
00295 }
00296 else if(_previousStamp > 0)
00297 {
00298 int sleepTime = 1000.0*(stamp-_previousStamp) - 1000.0*_timer.getElapsedTime();
00299 if(sleepTime > 2)
00300 {
00301 uSleep(sleepTime-2);
00302 }
00303
00304
00305 while(_timer.getElapsedTime() < (stamp-_previousStamp)-0.000001)
00306 {
00307
00308 }
00309
00310 double slept = _timer.getElapsedTime();
00311 _timer.start();
00312 UDEBUG("slept=%fs vs target=%fs", slept, stamp-_previousStamp);
00313 }
00314 _previousStamp = stamp;
00315 }
00316 else if(_frameRate>0.0f)
00317 {
00318 int sleepTime = (1000.0f/_frameRate - 1000.0f*_timer.getElapsedTime());
00319 if(sleepTime > 2)
00320 {
00321 uSleep(sleepTime-2);
00322 }
00323
00324
00325 while(_timer.getElapsedTime() < 1.0/double(_frameRate)-0.000001)
00326 {
00327
00328 }
00329
00330 double slept = _timer.getElapsedTime();
00331 _timer.start();
00332 UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(_frameRate));
00333 }
00334
00335 if(!this->isKilled())
00336 {
00337 rtabmap::CompressionThread ctImage(imageBytes, true);
00338 rtabmap::CompressionThread ctDepth(depthBytes, true);
00339 rtabmap::CompressionThread ctLaserScan(laserScanBytes, false);
00340 ctImage.start();
00341 ctDepth.start();
00342 ctLaserScan.start();
00343 ctImage.join();
00344 ctDepth.join();
00345 ctLaserScan.join();
00346 data = SensorData(
00347 ctLaserScan.getUncompressedData(),
00348 laserScanMaxPts,
00349 ctImage.getUncompressedData(),
00350 ctDepth.getUncompressedData(),
00351 fx,fy,cx,cy,
00352 localTransform,
00353 pose,
00354 rotVariance,
00355 transVariance,
00356 seq,
00357 stamp,
00358 userData);
00359 UDEBUG("Laser=%d RGB/Left=%d Depth=%d Right=%d",
00360 data.laserScan().empty()?0:1,
00361 data.image().empty()?0:1,
00362 data.depth().empty()?0:1,
00363 data.rightImage().empty()?0:1);
00364 }
00365 }
00366 }
00367 else
00368 {
00369 UERROR("Not initialized...");
00370 }
00371 return data;
00372 }
00373
00374 }