DBReader now inherits from Camera (so that CameraThread's post processing stuff can be used with a database stream)

corelib/src/Camera.cpp

@@ -88,7 +88,7 @@ SensorData Camera::takeImage(CameraInfo * info)
 	}
 
 	UTimer timer;
-	SensorData data  = this->captureImage();
+	SensorData data  = this->captureImage(info);
 	double captureTime = timer.ticks();
 	if(warnFrameRateTooHigh)
 	{

corelib/src/CameraRGB.cpp

@@ -438,7 +438,7 @@ std::vector<std::string> CameraImages::filenames() const
 	return std::vector<std::string>();
 }
 
-SensorData CameraImages::captureImage()
+SensorData CameraImages::captureImage(CameraInfo * info)
 {
 	if(syncImageRateWithStamps_ && _captureDelay>0.0)
 	{
@@ -801,7 +801,7 @@ std::string CameraVideo::getSerial() const
 	return _guid;
 }
 
-SensorData CameraVideo::captureImage()
+SensorData CameraVideo::captureImage(CameraInfo * info)
 {
 	cv::Mat img;
 	if(_capture.isOpened())

corelib/src/CameraRGBD.cpp

@@ -203,7 +203,7 @@ std::string CameraOpenni::getSerial() const
 	return "";
 }
 
-SensorData CameraOpenni::captureImage()
+SensorData CameraOpenni::captureImage(CameraInfo * info)
 {
 	SensorData data;
 #ifdef HAVE_OPENNI
@@ -317,7 +317,7 @@ bool CameraOpenNICV::isCalibrated() const
 	return true;
 }
 
-SensorData CameraOpenNICV::captureImage()
+SensorData CameraOpenNICV::captureImage(CameraInfo * info)
 {
 	SensorData data;
 	if(_capture.isOpened())
@@ -671,7 +671,7 @@ std::string CameraOpenNI2::getSerial() const
 	return "";
 }
 
-SensorData CameraOpenNI2::captureImage()
+SensorData CameraOpenNI2::captureImage(CameraInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_OPENNI2
@@ -1033,7 +1033,7 @@ std::string CameraFreenect::getSerial() const
 	return "";
 }
 
-SensorData CameraFreenect::captureImage()
+SensorData CameraFreenect::captureImage(CameraInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_FREENECT
@@ -1307,7 +1307,7 @@ std::string CameraFreenect2::getSerial() const
 	return "";
 }
 
-SensorData CameraFreenect2::captureImage()
+SensorData CameraFreenect2::captureImage(CameraInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_FREENECT2
@@ -1720,12 +1720,12 @@ std::string CameraRGBDImages::getSerial() const
 	return this->cameraModel().name();
 }
 
-SensorData CameraRGBDImages::captureImage()
+SensorData CameraRGBDImages::captureImage(CameraInfo * info)
 {
 	SensorData data;
 
 	SensorData rgb, depth;
-	rgb = CameraImages::captureImage();
+	rgb = CameraImages::captureImage(info);
 	if(!rgb.imageRaw().empty())
 	{
 		depth = cameraDepth_.takeImage();

corelib/src/CameraStereo.cpp

@@ -415,7 +415,7 @@ std::string CameraStereoDC1394::getSerial() const
 	return "";
 }
 
-SensorData CameraStereoDC1394::captureImage()
+SensorData CameraStereoDC1394::captureImage(CameraInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_DC1394
@@ -610,7 +610,7 @@ struct ImageContainer
 } ;
 #endif
 
-SensorData CameraStereoFlyCapture2::captureImage()
+SensorData CameraStereoFlyCapture2::captureImage(CameraInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_FLYCAPTURE2
@@ -887,7 +887,7 @@ std::string CameraStereoZed::getSerial() const
 	return "";
 }
 
-SensorData CameraStereoZed::captureImage()
+SensorData CameraStereoZed::captureImage(CameraInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_ZED
@@ -1072,21 +1072,21 @@ std::string CameraStereoImages::getSerial() const
 	return stereoModel_.name();
 }
 
-SensorData CameraStereoImages::captureImage()
+SensorData CameraStereoImages::captureImage(CameraInfo * info)
 {
 	SensorData data;
 
 	SensorData left, right;
-	left = CameraImages::captureImage();
+	left = CameraImages::captureImage(info);
 	if(!left.imageRaw().empty())
 	{
 		if(camera2_)
 		{
-			right = camera2_->takeImage();
+			right = camera2_->takeImage(info);
 		}
 		else
 		{
-			right = this->takeImage();
+			right = this->takeImage(info);
 		}
 
 		if(!right.imageRaw().empty())
@@ -1235,7 +1235,7 @@ std::string CameraStereoVideo::getSerial() const
 	return cameraName_;
 }
 
-SensorData CameraStereoVideo::captureImage()
+SensorData CameraStereoVideo::captureImage(CameraInfo * info)
 {
 	SensorData data;
 

corelib/src/DBReader.cpp

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UEventsManager.h>
 
 #include "rtabmap/core/CameraEvent.h"
 #include "rtabmap/core/RtabmapEvent.h"
@@ -47,17 +48,20 @@ DBReader::DBReader(const std::string & databasePath,
 				   bool odometryIgnored,
 				   bool ignoreGoalDelay,
 				   bool goalsIgnored,
+				   int startIndex,
 				   int cameraIndex) :
+	Camera(frameRate),
 	_paths(uSplit(databasePath, ';')),
-	_frameRate(frameRate),
 	_odometryIgnored(odometryIgnored),
 	_ignoreGoalDelay(ignoreGoalDelay),
 	_goalsIgnored(goalsIgnored),
+	_startIndex(startIndex),
 	_cameraIndex(cameraIndex),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousStamp(0),
-	_previousMapID(0)
+	_previousMapID(0),
+	_calibrated(false)
 {
 }
 
@@ -66,17 +70,20 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 				   bool odometryIgnored,
 				   bool ignoreGoalDelay,
 				   bool goalsIgnored,
+				   int startIndex,
 				   int cameraIndex) :
+	Camera(frameRate),
    _paths(databasePaths),
-   _frameRate(frameRate),
 	_odometryIgnored(odometryIgnored),
 	_ignoreGoalDelay(ignoreGoalDelay),
 	_goalsIgnored(goalsIgnored),
+	_startIndex(startIndex),
 	_cameraIndex(cameraIndex),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousStamp(0),
-	_previousMapID(0)
+	_previousMapID(0),
+	_calibrated(false)
 {
 }
 
@@ -89,7 +96,9 @@ DBReader::~DBReader()
 	}
 }
 
-bool DBReader::init(int startIndex)
+bool DBReader::init(
+		const std::string & calibrationFolder,
+		const std::string & cameraName)
 {
 	if(_dbDriver)
 	{
@@ -101,6 +110,7 @@ bool DBReader::init(int startIndex)
 	_currentId=_ids.end();
 	_previousStamp = 0;
 	_previousMapID = 0;
+	_calibrated = false;
 
 	if(_paths.size() == 0)
 	{
@@ -133,12 +143,12 @@ bool DBReader::init(int startIndex)
 
 	_dbDriver->getAllNodeIds(_ids);
 	_currentId = _ids.begin();
-	if(startIndex>0 && _ids.size())
+	if(_startIndex>0 && _ids.size())
 	{
-		std::set<int>::iterator iter = uIteratorAt(_ids, startIndex);
+		std::set<int>::iterator iter = uIteratorAt(_ids, _startIndex);
 		if(iter == _ids.end())
 		{
-			UWARN("Start index is too high (%d), the last in database is %d. Starting from beginning...", startIndex, _ids.size()-1);
+			UWARN("Start index is too high (%d), the last in database is %d. Starting from beginning...", _startIndex, _ids.size()-1);
 		}
 		else
 		{
@@ -146,142 +156,139 @@ bool DBReader::init(int startIndex)
 		}
 	}
 
+	if(_ids.size())
+	{
+		std::vector<CameraModel> models;
+		StereoCameraModel stereoModel;
+		if(_dbDriver->getCalibration(*_ids.begin(), models, stereoModel))
+		{
+			if(models.size() && models.at(0).isValidForProjection())
+			{
+				_calibrated = true;
+			}
+			else if(stereoModel.isValidForProjection())
+			{
+				_calibrated = true;
+			}
+		}
+	}
+
+	_timer.start();
+
 	return true;
 }
 
-void DBReader::setFrameRate(float frameRate)
+bool DBReader::isCalibrated() const
 {
-	_frameRate = frameRate;
+	return _calibrated;
 }
 
-void DBReader::mainLoopBegin()
+std::string DBReader::getSerial() const
 {
-	_timer.start();
+	return "DBReader";
 }
 
-void DBReader::mainLoop()
+SensorData DBReader::captureImage(CameraInfo * info)
 {
-	OdometryEvent odom = this->getNextData();
-	if(odom.data().id())
-	{
-		std::string goalId;
-		double previousStamp = odom.data().stamp();
-		if(previousStamp == 0)
-		{
-			odom.data().setStamp(UTimer::now());
-		}
-
-		if(!_goalsIgnored &&
-		   odom.data().userDataRaw().type() == CV_8SC1 &&
-		   odom.data().userDataRaw().cols >= 7 && // including null str ending
-		   odom.data().userDataRaw().rows == 1 &&
-		   memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0)
-		{
-			//GOAL format detected, remove it from the user data and send it as goal event
-			std::string goalStr = (const char *)odom.data().userDataRaw().data;
-			if(!goalStr.empty())
-			{
-				std::list<std::string> strs = uSplit(goalStr, ':');
-				if(strs.size() == 2)
-				{
-					goalId = *strs.rbegin();
-					odom.data().setUserData(cv::Mat());
-				}
-			}
-		}
-		if(!_odometryIgnored)
-		{
-			if(odom.pose().isNull())
-			{
-				UWARN("Reading the database: odometry is null! "
-					  "Please set \"Ignore odometry = true\" if there is "
-					  "no odometry in the database.");
-			}
-			this->post(new OdometryEvent(odom));
-		}
-		else
-		{
-			this->post(new CameraEvent(odom.data()));
-		}
-
-		if(!goalId.empty())
-		{
-			double delay = 0.0;
-			if(!_ignoreGoalDelay && _currentId != _ids.end())
-			{
-				// get stamp for the next signature to compute the delay
-				// that was used originally for planning
-				int weight;
-				std::string label;
-				double stamp;
-				int mapId;
-				Transform localTransform, pose, groundTruth;
-				_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth);
-				if(previousStamp && stamp && stamp > previousStamp)
-				{
-					delay = stamp - previousStamp;
-				}
-			}
-
-			if(delay > 0.0)
-			{
-				UWARN("Goal \"%s\" detected, posting it! Waiting %f seconds before sending next data...",
-					   goalId.c_str(), delay);
-			}
-			else
-			{
-				UWARN("Goal \"%s\" detected, posting it!", goalId.c_str());
-			}
-
-			if(uIsInteger(goalId))
-			{
-				this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, atoi(goalId.c_str())));
-			}
-			else
-			{
-				this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId));
-			}
-
-			if(delay > 0.0)
-			{
-				uSleep(delay*1000);
-			}
-		}
-
-	}
-	else if(!this->isKilled())
+	SensorData data = this->getNextData(info);
+	if(data.id() == 0)
 	{
 		UINFO("no more images...");
-		if(_paths.size() > 1)
+		while(_paths.size() > 1 && data.id() == 0)
 		{
 			_paths.pop_front();
 			UWARN("Loading next database \"%s\"...", _paths.front().c_str());
 			if(!this->init())
 			{
 				UERROR("Failed to initialize the next database \"%s\"", _paths.front().c_str());
-				this->kill();
-				this->post(new CameraEvent());
+				return data;
+			}
+			else
+			{
+				data = this->getNextData(info);
 			}
 		}
-		else
+	}
+	if(data.id())
+	{
+		std::string goalId;
+		double previousStamp = data.stamp();
+		if(previousStamp == 0)
 		{
-			this->kill();
-			this->post(new CameraEvent());
+			data.setStamp(UTimer::now());
 		}
 
-	}
+		if(!_goalsIgnored &&
+		   data.userDataRaw().type() == CV_8SC1 &&
+		   data.userDataRaw().cols >= 7 && // including null str ending
+		   data.userDataRaw().rows == 1 &&
+		   memcmp(data.userDataRaw().data, "GOAL:", 5) == 0)
+		{
+			//GOAL format detected, remove it from the user data and send it as goal event
+			std::string goalStr = (const char *)data.userDataRaw().data;
+			if(!goalStr.empty())
+			{
+				std::list<std::string> strs = uSplit(goalStr, ':');
+				if(strs.size() == 2)
+				{
+					goalId = *strs.rbegin();
+					data.setUserData(cv::Mat());
 
+					double delay = 0.0;
+					if(!_ignoreGoalDelay && _currentId != _ids.end())
+					{
+						// get stamp for the next signature to compute the delay
+						// that was used originally for planning
+						int weight;
+						std::string label;
+						double stamp;
+						int mapId;
+						Transform localTransform, pose, groundTruth;
+						_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth);
+						if(previousStamp && stamp && stamp > previousStamp)
+						{
+							delay = stamp - previousStamp;
+						}
+					}
+
+					if(delay > 0.0)
+					{
+						UWARN("Goal \"%s\" detected, posting it! Waiting %f seconds before sending next data...",
+							   goalId.c_str(), delay);
+					}
+					else
+					{
+						UWARN("Goal \"%s\" detected, posting it!", goalId.c_str());
+					}
+
+					if(uIsInteger(goalId))
+					{
+						UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, atoi(goalId.c_str())));
+					}
+					else
+					{
+						UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId));
+					}
+
+					if(delay > 0.0)
+					{
+						uSleep(delay*1000);
+					}
+				}
+			}
+		}
+	}
+	return data;
 }
 
-OdometryEvent DBReader::getNextData()
+SensorData DBReader::getNextData(CameraInfo * info)
 {
-	OdometryEvent odom;
+	SensorData data;
 	if(_dbDriver)
 	{
-		if(!this->isKilled() && _currentId != _ids.end())
+		if(_currentId != _ids.end())
 		{
 			int mapId;
-			SensorData data;
 			_dbDriver->getNodeData(*_currentId, data);
 
 			// info
@@ -316,12 +323,12 @@ OdometryEvent DBReader::getNextData()
 			}
 
 			// Frame rate
-			if(_frameRate < 0.0f)
+			if(this->getImageRate() < 0.0f)
 			{
 				if(stamp == 0)
 				{
 					UERROR("The option to use database stamps is set (framerate<0), but there are no stamps saved in the database! Aborting...");
-					this->kill();
+					return data;
 				}
 				else if(_previousMapID == mapId && _previousStamp > 0)
 				{
@@ -350,69 +357,61 @@ OdometryEvent DBReader::getNextData()
 				_previousStamp = stamp;
 				_previousMapID = mapId;
 			}
-			else if(_frameRate>0.0f)
+
+			data.uncompressData();
+			if(data.cameraModels().size() > 1 &&
+				_cameraIndex >= 0)
 			{
-				int sleepTime = (1000.0f/_frameRate - 1000.0f*_timer.getElapsedTime());
-				if(sleepTime > 2)
+				if(_cameraIndex < (int)data.cameraModels().size())
 				{
-					uSleep(sleepTime-2);
-				}
+					// select one camera
+					int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
+					UASSERT(!data.imageRaw().empty() &&
+							data.imageRaw().cols % data.cameraModels().size() == 0 &&
+							_cameraIndex*subImageWidth < data.imageRaw().cols);
+					data.setImageRaw(
+							cv::Mat(data.imageRaw(),
+							cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.imageRaw().rows)).clone());
 
-				// Add precision at the cost of a small overhead
-				while(_timer.getElapsedTime() < 1.0/double(_frameRate)-0.000001)
+					if(!data.depthOrRightRaw().empty())
+					{
+						UASSERT(data.depthOrRightRaw().cols % data.cameraModels().size() == 0 &&
+								subImageWidth == data.depthOrRightRaw().cols/(int)data.cameraModels().size() &&
+								_cameraIndex*subImageWidth < data.depthOrRightRaw().cols);
+						data.setDepthOrRightRaw(
+								cv::Mat(data.depthOrRightRaw(),
+								cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows)).clone());
+					}
+					CameraModel model = data.cameraModels().at(_cameraIndex);
+					data.setCameraModel(model);
+				}
+				else
 				{
-					//
+					UWARN("DBReader: Camera index %d doesn't exist! Camera models = %d.", _cameraIndex, (int)data.cameraModels().size());
 				}
-
-				double slept = _timer.getElapsedTime();
-				_timer.start();
-				UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(_frameRate));
 			}
+			data.setId(seq);
+			data.setStamp(stamp);
+			data.setGroundTruth(groundTruth);
+			UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, UserData=%d",
+					data.laserScanRaw().empty()?0:1,
+					data.imageRaw().empty()?0:1,
+					data.depthOrRightRaw().empty()?0:1,
+					data.userDataRaw().empty()?0:1);
 
-			if(!this->isKilled())
+			if(!_odometryIgnored)
 			{
-				data.uncompressData();
-				if(data.cameraModels().size() > 1 &&
-					_cameraIndex >= 0)
+				if(pose.isNull())
 				{
-					if(_cameraIndex < (int)data.cameraModels().size())
-					{
-						// select one camera
-						int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
-						UASSERT(!data.imageRaw().empty() &&
-								data.imageRaw().cols % data.cameraModels().size() == 0 &&
-								_cameraIndex*subImageWidth < data.imageRaw().cols);
-						data.setImageRaw(
-								cv::Mat(data.imageRaw(),
-								cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.imageRaw().rows)).clone());
-
-						if(!data.depthOrRightRaw().empty())
-						{
-							UASSERT(data.depthOrRightRaw().cols % data.cameraModels().size() == 0 &&
-									subImageWidth == data.depthOrRightRaw().cols/(int)data.cameraModels().size() &&
-									_cameraIndex*subImageWidth < data.depthOrRightRaw().cols);
-							data.setDepthOrRightRaw(
-									cv::Mat(data.depthOrRightRaw(),
-									cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows)).clone());
-						}
-						CameraModel model = data.cameraModels().at(_cameraIndex);
-						data.setCameraModel(model);
-					}
-					else
-					{
-						UWARN("DBReader: Camera index %d doesn't exist! Camera models = %d.", _cameraIndex, (int)data.cameraModels().size());
-					}
+					UWARN("Reading the database: odometry is null! "
+						  "Please set \"Ignore odometry = true\" if there is "
+						  "no odometry in the database.");
+				}
+				if(info)
+				{
+					info->odomPose = pose;
+					info->odomCovariance = infMatrix.inv();
 				}
-				data.setId(seq);
-				data.setStamp(stamp);
-				data.setGroundTruth(groundTruth);
-				UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, UserData=%d",
-						data.laserScanRaw().empty()?0:1,
-						data.imageRaw().empty()?0:1,
-						data.depthOrRightRaw().empty()?0:1,
-						data.userDataRaw().empty()?0:1);
-
-				odom = OdometryEvent(data, pose, infMatrix.inv());
 			}
 		}
 	}
@@ -420,7 +419,7 @@ OdometryEvent DBReader::getNextData()
 	{
 		UERROR("Not initialized...");
 	}
-	return odom;
+	return data;
 }
 
 } /* namespace rtabmap */

corelib/src/RtabmapThread.cpp

@@ -333,7 +333,7 @@ void RtabmapThread::handleEvent(UEvent* event)
 			CameraEvent * e = (CameraEvent*)event;
 			if(e->getCode() == CameraEvent::kCodeData)
 			{
-				this->addData(OdometryEvent(e->data(), Transform(), 1, 1));
+				this->addData(OdometryEvent(e->data(), e->info().odomPose, e->info().odomCovariance));
 			}
 		}
 		else if(event->getClassName().compare("OdometryEvent") == 0)