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 #include <rtabmap/utilite/UEventsManager.h>
00037
00038 #include "rtabmap/core/CameraEvent.h"
00039 #include "rtabmap/core/RtabmapEvent.h"
00040 #include "rtabmap/core/OdometryEvent.h"
00041 #include "rtabmap/core/util3d.h"
00042 #include "rtabmap/core/Compression.h"
00043
00044 namespace rtabmap {
00045
00046 DBReader::DBReader(const std::string & databasePath,
00047 float frameRate,
00048 bool odometryIgnored,
00049 bool ignoreGoalDelay,
00050 bool goalsIgnored,
00051 int startIndex,
00052 int cameraIndex) :
00053 Camera(frameRate),
00054 _paths(uSplit(databasePath, ';')),
00055 _odometryIgnored(odometryIgnored),
00056 _ignoreGoalDelay(ignoreGoalDelay),
00057 _goalsIgnored(goalsIgnored),
00058 _startIndex(startIndex),
00059 _cameraIndex(cameraIndex),
00060 _dbDriver(0),
00061 _currentId(_ids.end()),
00062 _previousStamp(0),
00063 _previousMapID(0),
00064 _calibrated(false)
00065 {
00066 }
00067
00068 DBReader::DBReader(const std::list<std::string> & databasePaths,
00069 float frameRate,
00070 bool odometryIgnored,
00071 bool ignoreGoalDelay,
00072 bool goalsIgnored,
00073 int startIndex,
00074 int cameraIndex) :
00075 Camera(frameRate),
00076 _paths(databasePaths),
00077 _odometryIgnored(odometryIgnored),
00078 _ignoreGoalDelay(ignoreGoalDelay),
00079 _goalsIgnored(goalsIgnored),
00080 _startIndex(startIndex),
00081 _cameraIndex(cameraIndex),
00082 _dbDriver(0),
00083 _currentId(_ids.end()),
00084 _previousStamp(0),
00085 _previousMapID(0),
00086 _calibrated(false)
00087 {
00088 }
00089
00090 DBReader::~DBReader()
00091 {
00092 if(_dbDriver)
00093 {
00094 _dbDriver->closeConnection();
00095 delete _dbDriver;
00096 }
00097 }
00098
00099 bool DBReader::init(
00100 const std::string & calibrationFolder,
00101 const std::string & cameraName)
00102 {
00103 if(_dbDriver)
00104 {
00105 _dbDriver->closeConnection();
00106 delete _dbDriver;
00107 _dbDriver = 0;
00108 }
00109 _ids.clear();
00110 _currentId=_ids.end();
00111 _previousStamp = 0;
00112 _previousMapID = 0;
00113 _calibrated = false;
00114
00115 if(_paths.size() == 0)
00116 {
00117 UERROR("No database path set...");
00118 return false;
00119 }
00120
00121 std::string path = _paths.front();
00122 if(!UFile::exists(path))
00123 {
00124 UERROR("Database path does not exist (%s)", path.c_str());
00125 return false;
00126 }
00127
00128 rtabmap::ParametersMap parameters;
00129 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
00130 _dbDriver = new DBDriverSqlite3(parameters);
00131 if(!_dbDriver)
00132 {
00133 UERROR("Driver doesn't exist.");
00134 return false;
00135 }
00136 if(!_dbDriver->openConnection(path))
00137 {
00138 UERROR("Can't open database %s", path.c_str());
00139 delete _dbDriver;
00140 _dbDriver = 0;
00141 return false;
00142 }
00143
00144 _dbDriver->getAllNodeIds(_ids);
00145 _currentId = _ids.begin();
00146 if(_startIndex>0 && _ids.size())
00147 {
00148 std::set<int>::iterator iter = uIteratorAt(_ids, _startIndex);
00149 if(iter == _ids.end())
00150 {
00151 UWARN("Start index is too high (%d), the last in database is %d. Starting from beginning...", _startIndex, _ids.size()-1);
00152 }
00153 else
00154 {
00155 _currentId = iter;
00156 }
00157 }
00158
00159 if(_ids.size())
00160 {
00161 std::vector<CameraModel> models;
00162 StereoCameraModel stereoModel;
00163 if(_dbDriver->getCalibration(*_ids.begin(), models, stereoModel))
00164 {
00165 if(models.size() && models.at(0).isValidForProjection())
00166 {
00167 _calibrated = true;
00168 }
00169 else if(stereoModel.isValidForProjection())
00170 {
00171 _calibrated = true;
00172 }
00173 }
00174 }
00175
00176 _timer.start();
00177
00178 return true;
00179 }
00180
00181 bool DBReader::isCalibrated() const
00182 {
00183 return _calibrated;
00184 }
00185
00186 std::string DBReader::getSerial() const
00187 {
00188 return "DBReader";
00189 }
00190
00191 SensorData DBReader::captureImage(CameraInfo * info)
00192 {
00193 SensorData data = this->getNextData(info);
00194 if(data.id() == 0)
00195 {
00196 UINFO("no more images...");
00197 while(_paths.size() > 1 && data.id() == 0)
00198 {
00199 _paths.pop_front();
00200 UWARN("Loading next database \"%s\"...", _paths.front().c_str());
00201 if(!this->init())
00202 {
00203 UERROR("Failed to initialize the next database \"%s\"", _paths.front().c_str());
00204 return data;
00205 }
00206 else
00207 {
00208 data = this->getNextData(info);
00209 }
00210 }
00211 }
00212 if(data.id())
00213 {
00214 std::string goalId;
00215 double previousStamp = data.stamp();
00216 if(previousStamp == 0)
00217 {
00218 data.setStamp(UTimer::now());
00219 }
00220
00221 if(!_goalsIgnored &&
00222 data.userDataRaw().type() == CV_8SC1 &&
00223 data.userDataRaw().cols >= 7 &&
00224 data.userDataRaw().rows == 1 &&
00225 memcmp(data.userDataRaw().data, "GOAL:", 5) == 0)
00226 {
00227
00228 std::string goalStr = (const char *)data.userDataRaw().data;
00229 if(!goalStr.empty())
00230 {
00231 std::list<std::string> strs = uSplit(goalStr, ':');
00232 if(strs.size() == 2)
00233 {
00234 goalId = *strs.rbegin();
00235 data.setUserData(cv::Mat());
00236
00237 double delay = 0.0;
00238 if(!_ignoreGoalDelay && _currentId != _ids.end())
00239 {
00240
00241
00242 int weight;
00243 std::string label;
00244 double stamp;
00245 int mapId;
00246 Transform localTransform, pose, groundTruth;
00247 _dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth);
00248 if(previousStamp && stamp && stamp > previousStamp)
00249 {
00250 delay = stamp - previousStamp;
00251 }
00252 }
00253
00254 if(delay > 0.0)
00255 {
00256 UWARN("Goal \"%s\" detected, posting it! Waiting %f seconds before sending next data...",
00257 goalId.c_str(), delay);
00258 }
00259 else
00260 {
00261 UWARN("Goal \"%s\" detected, posting it!", goalId.c_str());
00262 }
00263
00264 if(uIsInteger(goalId))
00265 {
00266 UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, atoi(goalId.c_str())));
00267 }
00268 else
00269 {
00270 UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId));
00271 }
00272
00273 if(delay > 0.0)
00274 {
00275 uSleep(delay*1000);
00276 }
00277 }
00278 }
00279 }
00280 }
00281 return data;
00282 }
00283
00284 SensorData DBReader::getNextData(CameraInfo * info)
00285 {
00286 SensorData data;
00287 if(_dbDriver)
00288 {
00289 if(_currentId != _ids.end())
00290 {
00291 int mapId;
00292 _dbDriver->getNodeData(*_currentId, data);
00293
00294
00295 Transform pose;
00296 int weight;
00297 std::string label;
00298 double stamp;
00299 Transform groundTruth;
00300 _dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth);
00301
00302 cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
00303 if(!_odometryIgnored)
00304 {
00305 std::map<int, Link> links;
00306 _dbDriver->loadLinks(*_currentId, links, Link::kNeighbor);
00307 if(links.size())
00308 {
00309
00310 infMatrix = links.begin()->second.infMatrix();
00311 }
00312 }
00313 else
00314 {
00315 pose.setNull();
00316 }
00317
00318 int seq = *_currentId;
00319 ++_currentId;
00320 if(data.imageCompressed().empty())
00321 {
00322 UWARN("No image loaded from the database for id=%d!", *_currentId);
00323 }
00324
00325
00326 if(this->getImageRate() < 0.0f)
00327 {
00328 if(stamp == 0)
00329 {
00330 UERROR("The option to use database stamps is set (framerate<0), but there are no stamps saved in the database! Aborting...");
00331 return data;
00332 }
00333 else if(_previousMapID == mapId && _previousStamp > 0)
00334 {
00335 int sleepTime = 1000.0*(stamp-_previousStamp) - 1000.0*_timer.getElapsedTime();
00336 if(sleepTime > 10000)
00337 {
00338 UWARN("Detected long delay (%d sec, stamps = %f vs %f). Waiting a maximum of 10 seconds.",
00339 sleepTime/1000, _previousStamp, stamp);
00340 sleepTime = 10000;
00341 }
00342 if(sleepTime > 2)
00343 {
00344 uSleep(sleepTime-2);
00345 }
00346
00347
00348 while(_timer.getElapsedTime() < (stamp-_previousStamp)-0.000001)
00349 {
00350
00351 }
00352
00353 double slept = _timer.getElapsedTime();
00354 _timer.start();
00355 UDEBUG("slept=%fs vs target=%fs", slept, stamp-_previousStamp);
00356 }
00357 _previousStamp = stamp;
00358 _previousMapID = mapId;
00359 }
00360
00361 data.uncompressData();
00362 if(data.cameraModels().size() > 1 &&
00363 _cameraIndex >= 0)
00364 {
00365 if(_cameraIndex < (int)data.cameraModels().size())
00366 {
00367
00368 int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
00369 UASSERT(!data.imageRaw().empty() &&
00370 data.imageRaw().cols % data.cameraModels().size() == 0 &&
00371 _cameraIndex*subImageWidth < data.imageRaw().cols);
00372 data.setImageRaw(
00373 cv::Mat(data.imageRaw(),
00374 cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.imageRaw().rows)).clone());
00375
00376 if(!data.depthOrRightRaw().empty())
00377 {
00378 UASSERT(data.depthOrRightRaw().cols % data.cameraModels().size() == 0 &&
00379 subImageWidth == data.depthOrRightRaw().cols/(int)data.cameraModels().size() &&
00380 _cameraIndex*subImageWidth < data.depthOrRightRaw().cols);
00381 data.setDepthOrRightRaw(
00382 cv::Mat(data.depthOrRightRaw(),
00383 cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows)).clone());
00384 }
00385 CameraModel model = data.cameraModels().at(_cameraIndex);
00386 data.setCameraModel(model);
00387 }
00388 else
00389 {
00390 UWARN("DBReader: Camera index %d doesn't exist! Camera models = %d.", _cameraIndex, (int)data.cameraModels().size());
00391 }
00392 }
00393 data.setId(seq);
00394 data.setStamp(stamp);
00395 data.setGroundTruth(groundTruth);
00396 UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, UserData=%d",
00397 data.laserScanRaw().empty()?0:1,
00398 data.imageRaw().empty()?0:1,
00399 data.depthOrRightRaw().empty()?0:1,
00400 data.userDataRaw().empty()?0:1);
00401
00402 if(!_odometryIgnored)
00403 {
00404 if(pose.isNull())
00405 {
00406 UWARN("Reading the database: odometry is null! "
00407 "Please set \"Ignore odometry = true\" if there is "
00408 "no odometry in the database.");
00409 }
00410 if(info)
00411 {
00412 info->odomPose = pose;
00413 info->odomCovariance = infMatrix.inv();
00414 }
00415 }
00416 }
00417 }
00418 else
00419 {
00420 UERROR("Not initialized...");
00421 }
00422 return data;
00423 }
00424
00425 }