DBViewer: added feature/line color menu option. CameraImages/DBReader: added maxFrames parameter

corelib/include/rtabmap/core/DBReader.h

@@ -51,14 +51,16 @@ public:
 			 bool ignoreGoalDelay = false,
 			 bool goalsIgnored = false,
 			 int startIndex = 0,
-			 int cameraIndex = -1);
+			 int cameraIndex = -1,
+			 int maxFrames = 0);
 	DBReader(const std::list<std::string> & databasePaths,
 			 float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
 			 bool odometryIgnored = false,
 			 bool ignoreGoalDelay = false,
 			 bool goalsIgnored = false,
 			 int startIndex = 0,
-			 int cameraIndex = -1);
+			 int cameraIndex = -1,
+			 int maxFrames = 0);
 	virtual ~DBReader();
 
 	virtual bool init(
@@ -81,6 +83,7 @@ private:
 	bool _ignoreGoalDelay;
 	bool _goalsIgnored;
 	int _startIndex;
+	int _maxFrames;
 	int _cameraIndex;
 
 	DBDriver * _dbDriver;
@@ -92,6 +95,7 @@ private:
 	double _previousStamp;
 	int _previousMapID;
 	bool _calibrated;
+	int _framesPublished;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/camera/CameraImages.h

@@ -62,6 +62,7 @@ public:
 
 	void setPath(const std::string & dir) {_path=dir;}
 	virtual void setStartIndex(int index) {_startAt = index;} // negative means last
+	virtual void setMaxFrames(int value) {_maxFrames = value;}
 	void setDirRefreshed(bool enabled) {_refreshDir = enabled;}
 	void setImagesRectified(bool enabled) {_rectifyImages = enabled;}
 	void setBayerMode(int mode) {_bayerMode = mode;} // -1=disabled (default) 0=BayerBG, 1=BayerGB, 2=BayerRG, 3=BayerGR
@@ -125,6 +126,7 @@ protected:
 private:
 	std::string _path;
 	int _startAt;
+	int _maxFrames;
 	// If the list of files in the directory is refreshed
 	// on each call of takeImage()
 	bool _refreshDir;
@@ -133,6 +135,7 @@ private:
 	bool _isDepth;
 	float _depthScaleFactor;
 	int _count;
+	int _framesPublished;
 	UDirectory * _dir;
 	std::string _lastFileName;
 

corelib/include/rtabmap/core/camera/CameraRGBDImages.h

@@ -52,6 +52,7 @@ public:
 	virtual std::string getSerial() const;
 
 	virtual void setStartIndex(int index) {CameraImages::setStartIndex(index);cameraDepth_.setStartIndex(index);} // negative means last
+	virtual void setMaxFrames(int value) {CameraImages::setMaxFrames(value);cameraDepth_.setMaxFrames(value);}
 
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);

corelib/include/rtabmap/core/camera/CameraStereoImages.h

@@ -62,6 +62,7 @@ public:
 	virtual std::string getSerial() const;
 
 	virtual void setStartIndex(int index) {CameraImages::setStartIndex(index);camera2_->setStartIndex(index);} // negative means last
+	virtual void setMaxFrames(int value) {CameraImages::setMaxFrames(value);camera2_->setMaxFrames(value);}
 
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);

corelib/src/DBReader.cpp

@@ -48,20 +48,23 @@ DBReader::DBReader(const std::string & databasePath,
 				   bool ignoreGoalDelay,
 				   bool goalsIgnored,
 				   int startIndex,
-				   int cameraIndex) :
+				   int cameraIndex,
+				   int maxFrames) :
 	Camera(frameRate),
 	_paths(uSplit(databasePath, ';')),
 	_odometryIgnored(odometryIgnored),
 	_ignoreGoalDelay(ignoreGoalDelay),
 	_goalsIgnored(goalsIgnored),
 	_startIndex(startIndex),
+	_maxFrames(maxFrames),
 	_cameraIndex(cameraIndex),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousMapId(-1),
 	_previousStamp(0),
 	_previousMapID(0),
-	_calibrated(false)
+	_calibrated(false),
+	_framesPublished(0)
 {
 }
 
@@ -71,20 +74,23 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 				   bool ignoreGoalDelay,
 				   bool goalsIgnored,
 				   int startIndex,
-				   int cameraIndex) :
+				   int cameraIndex,
+				   int maxFrames) :
 	Camera(frameRate),
    _paths(databasePaths),
 	_odometryIgnored(odometryIgnored),
 	_ignoreGoalDelay(ignoreGoalDelay),
 	_goalsIgnored(goalsIgnored),
 	_startIndex(startIndex),
+	_maxFrames(maxFrames),
 	_cameraIndex(cameraIndex),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousMapId(-1),
 	_previousStamp(0),
 	_previousMapID(0),
-	_calibrated(false)
+	_calibrated(false),
+	_framesPublished(0)
 {
 }
 
@@ -114,6 +120,7 @@ bool DBReader::init(
 	_previousStamp = 0;
 	_previousMapID = 0;
 	_calibrated = false;
+	_framesPublished = 0;
 
 	if(_paths.size() == 0)
 	{
@@ -212,6 +219,12 @@ std::string DBReader::getSerial() const
 
 SensorData DBReader::captureImage(CameraInfo * info)
 {
+	if(_maxFrames>0 && ++_framesPublished > _maxFrames)
+	{
+		UINFO("Maximum frames (%d) reached!", _maxFrames);
+		return SensorData();
+	}
+
 	SensorData data = this->getNextData(info);
 	if(data.id() == 0)
 	{
@@ -231,6 +244,7 @@ SensorData DBReader::captureImage(CameraInfo * info)
 			}
 		}
 	}
+
 	if(data.id())
 	{
 		std::string goalId;

corelib/src/camera/CameraImages.cpp

@@ -41,12 +41,14 @@ namespace rtabmap
 
 CameraImages::CameraImages() :
 		_startAt(0),
+		_maxFrames(0),
 		_refreshDir(false),
 		_rectifyImages(false),
 		_bayerMode(-1),
 		_isDepth(false),
 		_depthScaleFactor(1.0f),
 		_count(0),
+		_framesPublished(0),
 		_dir(0),
 		_countScan(0),
 		_scanDir(0),
@@ -68,12 +70,14 @@ CameraImages::CameraImages(const std::string & path,
 	Camera(imageRate, localTransform),
 	_path(path),
 	_startAt(0),
+	_maxFrames(0),
 	_refreshDir(false),
 	_rectifyImages(false),
 	_bayerMode(-1),
 	_isDepth(false),
 	_depthScaleFactor(1.0f),
 	_count(0),
+	_framesPublished(0),
 	_dir(0),
 	_countScan(0),
 	_scanDir(0),
@@ -106,6 +110,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 	_count = 0;
 	_countScan = 0;
 	_captureDelay = 0.0;
+	_framesPublished=0;
 
 	UDEBUG("");
 	if(_dir)
@@ -617,7 +622,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 				if(!fileName.empty())
 				{
 					scanFilePath = _scanPath + fileName;
-					while(++_countScan < _startAt && (fileName = _scanDir->getNextFileName()).size())
+					while(_countScan++ < _startAt && (fileName = _scanDir->getNextFileName()).size())
 					{
 						scanFilePath = _scanPath + fileName;
 					}
@@ -625,99 +630,103 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 			}
 		}
 
-		if(!imageFilePath.empty())
+		if(_maxFrames <=0 || ++_framesPublished <= _maxFrames)
 		{
-			ULOGGER_DEBUG("Loading image : %s", imageFilePath.c_str());
+
+			if(!imageFilePath.empty())
+			{
+				ULOGGER_DEBUG("Loading image : %s", imageFilePath.c_str());
 
 #if CV_MAJOR_VERSION >2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
-			img = cv::imread(imageFilePath.c_str(), cv::IMREAD_UNCHANGED);
+				img = cv::imread(imageFilePath.c_str(), cv::IMREAD_UNCHANGED);
 #else
-			img = cv::imread(imageFilePath.c_str(), -1);
+				img = cv::imread(imageFilePath.c_str(), -1);
 #endif
-			UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d",
-					img.cols, img.rows, img.channels(), img.elemSize(), img.total());
+				UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d",
+						img.cols, img.rows, img.channels(), img.elemSize(), img.total());
 
-			if(_isDepth)
-			{
-				if(img.type() != CV_16UC1 && img.type() != CV_32FC1)
+				if(_isDepth)
 				{
-					UERROR("Depth is on and the loaded image has not a format supported (file = \"%s\", type=%d). "
-							"Formats supported are 16 bits 1 channel (mm) and 32 bits 1 channel (m).",
-							imageFilePath.c_str(), img.type());
-					img = cv::Mat();
-				}
-
-				if(_depthScaleFactor > 1.0f)
-				{
-					img /= _depthScaleFactor;
-				}
-			}
-			else
-			{
-#if CV_MAJOR_VERSION < 3
-				// FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works...
-				if(img.depth() != CV_8U)
-				{
-					// The depth should be 8U
-					UWARN("Cannot read the image correctly, falling back to old OpenCV C interface...");
-					IplImage * i = cvLoadImage(imageFilePath.c_str());
-					img = cv::Mat(i, true);
-					cvReleaseImage(&i);
-				}
-#endif
-				if(img.channels()>3)
-				{
-					UWARN("Conversion from 4 channels to 3 channels (file=%s)", imageFilePath.c_str());
-					cv::Mat out;
-					cv::cvtColor(img, out, CV_BGRA2BGR);
-					img = out;
-				}
-				else if(_bayerMode >= 0 && _bayerMode <=3)
-				{
-					cv::Mat debayeredImg;
-					try
+					if(img.type() != CV_16UC1 && img.type() != CV_32FC1)
 					{
-						cv::cvtColor(img, debayeredImg, CV_BayerBG2BGR + _bayerMode);
-						img = debayeredImg;
+						UERROR("Depth is on and the loaded image has not a format supported (file = \"%s\", type=%d). "
+								"Formats supported are 16 bits 1 channel (mm) and 32 bits 1 channel (m).",
+								imageFilePath.c_str(), img.type());
+						img = cv::Mat();
 					}
-					catch(const cv::Exception & e)
+
+					if(_depthScaleFactor > 1.0f)
 					{
-						UWARN("Error debayering images: \"%s\". Please set bayer mode to -1 if images are not bayered!", e.what());
+						img /= _depthScaleFactor;
 					}
 				}
-
-			}
-
-			if(!img.empty() && _model.isValidForRectification() && _rectifyImages)
-			{
-				img = _model.rectifyImage(img);
-			}
-		}
-
-		if(!scanFilePath.empty())
-		{
-			// load without filtering
-			scan = util3d::loadScan(scanFilePath);
-			scan = LaserScan(scan.data(), _scanMaxPts, 0.0f, scan.format(), _scanLocalTransform);
-			UDEBUG("Loaded scan=%d points", (int)scan.size());
-			if(_depthFromScan && !img.empty())
-			{
-				UDEBUG("Computing depth from scan...");
-				if(!_model.isValidForProjection())
-				{
-					UWARN("Depth from laser scan: Camera model should be valid.");
-				}
-				else if(_isDepth)
-				{
-					UWARN("Depth from laser scan: Loading already a depth image.");
-				}
 				else
 				{
-					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
-					depthFromScan = util3d::projectCloudToCamera(img.size(), _model.K(), cloud, _model.localTransform());
-					if(_depthFromScanFillHoles!=0)
+#if CV_MAJOR_VERSION < 3
+					// FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works...
+					if(img.depth() != CV_8U)
 					{
-						util3d::fillProjectedCloudHoles(depthFromScan, _depthFromScanFillHoles>0, _depthFromScanFillHolesFromBorder);
+						// The depth should be 8U
+						UWARN("Cannot read the image correctly, falling back to old OpenCV C interface...");
+						IplImage * i = cvLoadImage(imageFilePath.c_str());
+						img = cv::Mat(i, true);
+						cvReleaseImage(&i);
+					}
+#endif
+					if(img.channels()>3)
+					{
+						UWARN("Conversion from 4 channels to 3 channels (file=%s)", imageFilePath.c_str());
+						cv::Mat out;
+						cv::cvtColor(img, out, CV_BGRA2BGR);
+						img = out;
+					}
+					else if(_bayerMode >= 0 && _bayerMode <=3)
+					{
+						cv::Mat debayeredImg;
+						try
+						{
+							cv::cvtColor(img, debayeredImg, CV_BayerBG2BGR + _bayerMode);
+							img = debayeredImg;
+						}
+						catch(const cv::Exception & e)
+						{
+							UWARN("Error debayering images: \"%s\". Please set bayer mode to -1 if images are not bayered!", e.what());
+						}
+					}
+
+				}
+
+				if(!img.empty() && _model.isValidForRectification() && _rectifyImages)
+				{
+					img = _model.rectifyImage(img);
+				}
+			}
+
+			if(!scanFilePath.empty())
+			{
+				// load without filtering
+				scan = util3d::loadScan(scanFilePath);
+				scan = LaserScan(scan.data(), _scanMaxPts, 0.0f, scan.format(), _scanLocalTransform);
+				UDEBUG("Loaded scan=%d points", (int)scan.size());
+				if(_depthFromScan && !img.empty())
+				{
+					UDEBUG("Computing depth from scan...");
+					if(!_model.isValidForProjection())
+					{
+						UWARN("Depth from laser scan: Camera model should be valid.");
+					}
+					else if(_isDepth)
+					{
+						UWARN("Depth from laser scan: Loading already a depth image.");
+					}
+					else
+					{
+						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
+						depthFromScan = util3d::projectCloudToCamera(img.size(), _model.K(), cloud, _model.localTransform());
+						if(_depthFromScanFillHoles!=0)
+						{
+							util3d::fillProjectedCloudHoles(depthFromScan, _depthFromScanFillHoles>0, _depthFromScanFillHolesFromBorder);
+						}
 					}
 				}
 			}