Refactoring of the cameraStereoImages and cameraRGBDImages classes (now inheriting from CameraImages) for easy setting of laser scan path, timestamps path and ground truth path.

Added graph::importPoses().
Can now have a ground truth published with SensorData (filled optionally by CameraImages classes).
Increased database closing time performance when the database is not saved.
Added ParametersToolBox widget in DatabaseViewer for core parameters (refactoring done to make easy access to all rtabmap parameters in DatabaseViewer).
Added Parameters::getType(key).
Added Transform::interpolate() to interpolate between two transforms (SLERP)
Updated pf_filter.m and added test_pf_filter.m MATLAB scripts (making easier to compare with a ground truth)
UPlot: can now save all curve data of a figure in one action (see right-click on legend area->"Copy all curve data to clipboard")

corelib/src/CameraStereo.cpp

@@ -733,44 +733,34 @@ bool CameraStereoImages::available()
 CameraStereoImages::CameraStereoImages(
 		const std::string & pathLeftImages,
 		const std::string & pathRightImages,
-		bool filenamesAreTimestamps,
-		const std::string & timestampsPath,
 		bool rectifyImages,
 		float imageRate,
 		const Transform & localTransform) :
-		Camera(imageRate, localTransform),
-		camera_(0),
-		camera2_(0),
-		filenamesAreTimestamps_(filenamesAreTimestamps),
-		timestampsPath_(timestampsPath),
-		rectifyImages_(rectifyImages)
+		CameraImages(pathLeftImages, imageRate, localTransform),
+		camera2_(new CameraImages(pathRightImages))
 {
-	camera_ = new CameraImages(pathLeftImages);
-	camera2_ = new CameraImages(pathRightImages);
+	this->setImagesRectified(rectifyImages);
+	camera2_->setImagesRectified(rectifyImages);
 }
 
 CameraStereoImages::CameraStereoImages(
 		const std::string & pathLeftRightImages,
-		bool filenamesAreTimestamps,
-		const std::string & timestampsPath,
 		bool rectifyImages,
 		float imageRate,
 		const Transform & localTransform) :
-		Camera(imageRate, localTransform),
-		camera_(0),
-		camera2_(0),
-		filenamesAreTimestamps_(filenamesAreTimestamps),
-		timestampsPath_(timestampsPath),
-		rectifyImages_(rectifyImages)
+		CameraImages("", imageRate, localTransform),
+		camera2_(0)
 {
 	std::vector<std::string> paths = uListToVector(uSplit(pathLeftRightImages, uStrContains(pathLeftRightImages, ":")?':':';'));
 	if(paths.size() >= 1)
 	{
-		camera_ = new CameraImages(paths[0]);
+		this->setPath(paths[0]);
+		this->setImagesRectified(rectifyImages);
 
 		if(paths.size() >= 2)
 		{
 			camera2_ = new CameraImages(paths[1]);
+			camera2_->setImagesRectified(rectifyImages);
 		}
 	}
 	else
@@ -779,44 +769,19 @@ CameraStereoImages::CameraStereoImages(
 	}
 }
 
-CameraStereoImages::CameraStereoImages(
-		const std::string & scanPath,
-		const Transform & scanLocalTransform,
-		int scanMaxPts,
-		const std::string & pathLeftImages,
-		const std::string & pathRightImages,
-		bool filenamesAreTimestamps,
-		const std::string & timestampsPath,
-		bool rectifyImages,
-		float imageRate,
-		const Transform & localTransform) :
-		Camera(imageRate, localTransform),
-		camera_(0),
-		camera2_(0),
-		filenamesAreTimestamps_(filenamesAreTimestamps),
-		timestampsPath_(timestampsPath),
-		rectifyImages_(rectifyImages)
-{
-	camera_ = new CameraImages(scanPath, scanLocalTransform, scanMaxPts, pathLeftImages);
-	camera2_ = new CameraImages(pathRightImages);
-}
-
 CameraStereoImages::~CameraStereoImages()
 {
-	if(camera_)
-	{
-		delete camera_;
-	}
+	UDEBUG("");
 	if(camera2_)
 	{
 		delete camera2_;
 	}
+	UDEBUG("");
 }
 
 bool CameraStereoImages::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	// look for calibration files
-	cameraName_ = cameraName;
 	if(!calibrationFolder.empty() && !cameraName.empty())
 	{
 		if(!stereoModel_.load(calibrationFolder, cameraName))
@@ -835,31 +800,32 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std::
 	}
 
 	stereoModel_.setLocalTransform(this->getLocalTransform());
-	if(rectifyImages_ && !stereoModel_.isValid())
+	stereoModel_.setName(cameraName);
+	if(this->isImagesRectified() && !stereoModel_.isValid())
 	{
 		UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
 		return false;
 	}
 
+	//desactivate before init as we will do it in this class instead for convenience
+	this->setImagesRectified(false);
+
 	bool success = false;
-	if(camera_ == 0)
-	{
-		UERROR("Cannot initialize the camera.");
-	}
-	else if(camera_->init())
+	if(CameraImages::init())
 	{
 		if(camera2_)
 		{
+			camera2_->setImagesRectified(false);
 			if(camera2_->init())
 			{
-				if(camera_->imagesCount() == camera2_->imagesCount())
+				if(this->imagesCount() == camera2_->imagesCount())
 				{
 					success = true;
 				}
 				else
 				{
 					UERROR("Cameras don't have the same number of images (%d vs %d)",
-							camera_->imagesCount(), camera2_->imagesCount());
+							this->imagesCount(), camera2_->imagesCount());
 				}
 			}
 			else
@@ -872,68 +838,6 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std::
 			success = true;
 		}
 	}
-
-	stamps_.clear();
-	if(success)
-	{
-		if(filenamesAreTimestamps_)
-		{
-			std::vector<std::string> filenames = camera_->filenames();
-			for(unsigned int i=0; i<filenames.size(); ++i)
-			{
-				// format is 12234456.12334.png
-				std::list<std::string> list = uSplit(filenames.at(i), '.');
-				if(list.size() == 3)
-				{
-					list.pop_back(); // remove extension
-					double stamp = uStr2Double(uJoin(list, "."));
-					if(stamp > 0.0)
-					{
-						stamps_.push_back(stamp);
-					}
-					else
-					{
-						UERROR("Conversion filename to timestamp failed! (filename=%s)", filenames.at(i).c_str());
-					}
-				}
-			}
-			if(stamps_.size() != camera_->imagesCount())
-			{
-				UERROR("The stamps count is not the same as the images (%d vs %d)! "
-					   "Converting filenames to timestamps is activated.",
-						(int)stamps_.size(), camera_->imagesCount());
-				stamps_.clear();
-				success = false;
-			}
-		}
-		else if(timestampsPath_.size())
-		{
-			FILE * file = 0;
-#ifdef _MSC_VER
-			fopen_s(&file, timestampsPath_.c_str(), "r");
-#else
-			file = fopen(timestampsPath_.c_str(), "r");
-#endif
-			if(file)
-			{
-				char line[16];
-				while ( fgets (line , 16 , file) != NULL )
-				{
-					stamps_.push_back(uStr2Double(uReplaceChar(line, '\n', 0)));
-				}
-				fclose(file);
-			}
-			if(stamps_.size() != camera_->imagesCount())
-			{
-				UERROR("The stamps count is not the same as the images (%d vs %d)! Please remove "
-						"the timestamps file path if you don't want to use them (current file path=%s).",
-						(int)stamps_.size(), camera_->imagesCount(), timestampsPath_.c_str());
-				stamps_.clear();
-				success = false;
-			}
-		}
-	}
-
 	return success;
 }
 
@@ -944,55 +848,44 @@ bool CameraStereoImages::isCalibrated() const
 
 std::string CameraStereoImages::getSerial() const
 {
-	return cameraName_;
+	return stereoModel_.name();
 }
 
 SensorData CameraStereoImages::captureImage()
 {
 	SensorData data;
-	if(camera_)
+
+	SensorData left, right;
+	left = CameraImages::captureImage();
+	if(!left.imageRaw().empty())
 	{
-		double stamp;
-		if(stamps_.size())
+		if(camera2_)
 		{
-			stamp = stamps_.front();
-			stamps_.pop_front();
+			right = camera2_->takeImage();
 		}
 		else
 		{
-			stamp = UTimer::now();
+			right = this->takeImage();
 		}
-		SensorData left, right;
-		left = camera_->takeImage();
-		if(!left.imageRaw().empty())
-		{
-			if(camera2_)
-			{
-				right = camera2_->takeImage();
-			}
-			else
-			{
-				right = camera_->takeImage();
-			}
 
-			if(!right.imageRaw().empty())
+		if(!right.imageRaw().empty())
+		{
+			// Rectification
+			cv::Mat leftImage = left.imageRaw();
+			cv::Mat rightImage = right.imageRaw();
+			if(rightImage.type() != CV_8UC1)
 			{
-				// Rectification
-				cv::Mat leftImage = left.imageRaw();
-				cv::Mat rightImage = right.imageRaw();
-				if(rightImage.type() != CV_8UC1)
-				{
-					cv::Mat tmp;
-					cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
-					rightImage = tmp;
-				}
-				if(rectifyImages_ && stereoModel_.left().isValid() && stereoModel_.right().isValid())
-				{
-					leftImage = stereoModel_.left().rectifyImage(leftImage);
-					rightImage = stereoModel_.right().rectifyImage(rightImage);
-				}
-				data = SensorData(left.laserScanRaw(), left.laserScanMaxPts(), 0, leftImage, rightImage, stereoModel_, this->getNextSeqID(), stamp);
+				cv::Mat tmp;
+				cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
+				rightImage = tmp;
 			}
+			if(this->isImagesRectified() && stereoModel_.left().isValid() && stereoModel_.right().isValid())
+			{
+				leftImage = stereoModel_.left().rectifyImage(leftImage);
+				rightImage = stereoModel_.right().rectifyImage(rightImage);
+			}
+			data = SensorData(left.laserScanRaw(), left.laserScanMaxPts(), 0, leftImage, rightImage, stereoModel_, left.id()/(camera2_?1:2), left.stamp());
+			data.setGroundTruth(left.groundTruth());
 		}
 	}
 	return data;