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