CameraImages: added configForEachFrame option (added to GUI too). CameraThread: for decimation, if depth is smaller than RGB, RGB is decimated first and if the resulting RGB image is smaller than the original depth, we then decimate the depth.

corelib/include/rtabmap/core/Graph.h

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/Link.h>
 #include <rtabmap/core/GPS.h>
+#include <rtabmap/core/CameraModel.h>
 
 namespace rtabmap {
 class Memory;
@@ -55,10 +56,10 @@ namespace graph {
 
 bool RTABMAP_EXP importPoses(
 		const std::string & filePath,
-		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
+		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
-		std::map<int, double> * stamps = 0); // optional for format 1
+		std::map<int, double> * stamps = 0); // optional for format 1 and 9
 
 bool RTABMAP_EXP exportGPS(
 		const std::string & filePath,

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

@@ -74,6 +74,11 @@ public:
 		_syncImageRateWithStamps = syncImageRateWithStamps;
 	}
 
+	void setConfigForEachFrame(bool value)
+	{
+		_hasConfigForEachFrame = value;
+	}
+
 	void setScanPath(
 			const std::string & dir,
 			int maxScanPts = 0,
@@ -116,12 +121,14 @@ public:
 
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
+
+private:
 	bool readPoses(
-			std::list<Transform> & outputPoses,
+		std::list<Transform> & outputPoses,
-			std::list<double> & stamps,
+		std::list<double> & stamps,
-			const std::string & filePath,
+		const std::string & filePath,
-			int format,
+		int format,
-			double maxTimeDiff) const;
+		double maxTimeDiff) const;
 
 private:
 	std::string _path;
@@ -151,6 +158,7 @@ private:
 	bool _depthFromScanFillHolesFromBorder;
 
 	bool _filenamesAreTimestamps;
+	bool _hasConfigForEachFrame;
 	std::string _timestampsPath;
 	bool _syncImageRateWithStamps;
 
@@ -162,8 +170,10 @@ private:
 
 	std::list<double> _stamps;
 	std::list<Transform> odometry_;
+	std::list<cv::Mat> covariances_;
 	std::list<Transform> groundTruth_;
 	CameraModel _model;
+	std::list<CameraModel> _models;
 
 	UTimer _captureTimer;
 	double _captureDelay;

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

@@ -48,8 +48,6 @@ public:
 	virtual ~CameraRGBDImages();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	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);}

corelib/src/CameraThread.cpp

@@ -237,7 +237,26 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 		else
 		{
 			cv::Mat image = util2d::decimate(data.imageRaw(), _imageDecimation);
-			cv::Mat depthOrRight = util2d::decimate(data.depthOrRightRaw(), _imageDecimation);
+
+			int depthDecimation = _imageDecimation;
+			if(data.depthOrRightRaw().rows <= image.rows || data.depthOrRightRaw().cols <= image.cols)
+			{
+				depthDecimation = 1;
+			}
+			else
+			{
+				depthDecimation = 2;
+				while(data.depthOrRightRaw().rows / depthDecimation > image.rows ||
+					  data.depthOrRightRaw().cols / depthDecimation > image.cols ||
+					  data.depthOrRightRaw().rows % depthDecimation != 0 ||
+					  data.depthOrRightRaw().cols % depthDecimation != 0)
+				{
+					++depthDecimation;
+				}
+				UDEBUG("depthDecimation=%d", depthDecimation);
+			}
+			cv::Mat depthOrRight = util2d::decimate(data.depthOrRightRaw(), depthDecimation);
+
 			std::vector<CameraModel> models = data.cameraModels();
 			for(unsigned int i=0; i<models.size(); ++i)
 			{

corelib/src/Graph.cpp

@@ -169,7 +169,7 @@ bool exportPoses(
 
 bool importPoses(
 		const std::string & filePath,
-		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAC
+		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> * constraints, // optional for formats 3 and 4
 		std::map<int, double> * stamps) // optional for format 1 and 9

corelib/src/camera/CameraImages.cpp

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UThreadC.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_filtering.h>
@@ -58,6 +59,7 @@ CameraImages::CameraImages() :
 		_depthFromScanFillHoles(1),
 		_depthFromScanFillHolesFromBorder(false),
 		_filenamesAreTimestamps(false),
+		_hasConfigForEachFrame(false),
 		_syncImageRateWithStamps(true),
 		_odometryFormat(0),
 		_groundTruthFormat(0),
@@ -87,6 +89,7 @@ CameraImages::CameraImages(const std::string & path,
 	_depthFromScanFillHoles(1),
 	_depthFromScanFillHolesFromBorder(false),
 	_filenamesAreTimestamps(false),
+	_hasConfigForEachFrame(false),
 	_syncImageRateWithStamps(true),
 	_odometryFormat(0),
 	_groundTruthFormat(0),
@@ -111,6 +114,9 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 	_countScan = 0;
 	_captureDelay = 0.0;
 	_framesPublished=0;
+	_model = cameraModel();
+	_models.clear();
+	covariances_.clear();
 
 	UDEBUG("");
 	if(_dir)
@@ -213,7 +219,99 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 	groundTruth_.clear();
 	if(success)
 	{
-		if(_filenamesAreTimestamps)
+		if(_hasConfigForEachFrame)
+		{
+			UDirectory dirJson(_path, "json");
+			if(dirJson.getFileNames().size() == _dir->getFileNames().size())
+			{
+				bool modelsWarned = false;
+				bool firstFrame = true;
+				for(std::list<std::string>::const_iterator iter=dirJson.getFileNames().begin(); iter!=dirJson.getFileNames().end() && success; ++iter)
+				{
+					// Assuming 3DScannerApp(iOS) format (only this one supported...)
+					std::string filePath = _path+"/"+*iter;
+					cv::FileStorage fs(filePath, 0);
+					cv::FileNode poseNode = fs["cameraPoseARFrame"];
+					cv::FileNode timeNode = fs["time"];
+					cv::FileNode intrinsicsNode = fs["intrinsics"];
+					if(poseNode.isNone() || poseNode.size() != 16)
+					{
+						UERROR("Failed reading \"cameraPoseARFrame\" parameter, it should have 16 values (file=%s)", filePath.c_str());
+						success = false;
+						break;
+					}
+					else if(timeNode.isNone() || !timeNode.isReal())
+					{
+						UERROR("Failed reading \"time\" parameter (file=%s)", filePath.c_str());
+						success = false;
+						break;
+					}
+					else if(intrinsicsNode.isNone() || intrinsicsNode.size()!=9)
+					{
+						UERROR("Failed reading \"intrinsics\" parameter (file=%s)", filePath.c_str());
+						success = false;
+						break;
+					}
+					else
+					{
+						_stamps.push_back(timeNode.real());
+						if(_model.isValidForProjection() && !modelsWarned)
+						{
+							UWARN("Camera model loaded for each frame is overridden by "
+									"general calibration file provided. Remove general calibration "
+									"file to use camera model of each frame. This warning will "
+									"be shown only one time.");
+							modelsWarned = true;
+						}
+						else
+						{
+							_models.push_back(CameraModel(
+									intrinsicsNode[0].real(), //fx
+									intrinsicsNode[4].real(), //fy
+									intrinsicsNode[2].real(), //cx
+									intrinsicsNode[5].real(), //cy
+									CameraModel::opticalRotation()));
+						}
+						// we need to rotate from opengl world to rtabmap world
+						Transform pose(
+								poseNode[0].real(), poseNode[1].real(), poseNode[2].real(), poseNode[3].real(),
+								poseNode[4].real(), poseNode[5].real(), poseNode[6].real(), poseNode[7].real(),
+								poseNode[8].real(), poseNode[9].real(), poseNode[10].real(), poseNode[11].real());
+						pose =  Transform::rtabmap_T_opengl() * pose * Transform::opengl_T_rtabmap();
+						odometry_.push_back(pose);
+						// linear cov = 0.0001
+						cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.0001);
+						if(!firstFrame)
+						{
+							// angular cov = 0.000001
+							covariance.at<double>(3,3) *= 0.01;
+							covariance.at<double>(4,4) *= 0.01;
+							covariance.at<double>(5,5) *= 0.01;
+						}
+						firstFrame = false;
+						covariances_.push_back(covariance);
+					}
+				}
+				if(!success)
+				{
+					odometry_.clear();
+					_stamps.clear();
+					_models.clear();
+					covariances_.clear();
+				}
+			}
+			else
+			{
+				UERROR("Parameter \"Config for each frame\" is true, but the "
+						"number of config files (%d) is not equal to number "
+						"of images (%d) in this directory \"%s\"",
+						(int)dirJson.getFileNames().size(),
+						(int)_dir->getFileNames().size(),
+						_path.c_str());
+				success = false;
+			}
+		}
+		else if(_filenamesAreTimestamps)
 		{
 			const std::list<std::string> & filenames = _dir->getFileNames();
 			for(std::list<std::string>::const_iterator iter=filenames.begin(); iter!=filenames.end(); ++iter)
@@ -316,7 +414,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 			}
 		}
 
-		if(success && _odometryPath.size())
+		if(success && _odometryPath.size() && odometry_.empty())
 		{
 			success = readPoses(odometry_, _stamps, _odometryPath, _odometryFormat, _maxPoseTimeDiff);
 		}
@@ -332,7 +430,12 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 	return success;
 }
 
-bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<double> & inOutStamps, const std::string & filePath, int format, double maxTimeDiff) const
+bool CameraImages::readPoses(
+		std::list<Transform> & outputPoses,
+		std::list<double> & inOutStamps,
+		const std::string & filePath,
+		int format,
+		double maxTimeDiff) const
 {
 	outputPoses.clear();
 	std::map<int, Transform> poses;
@@ -448,7 +551,7 @@ bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<doubl
 
 bool CameraImages::isCalibrated() const
 {
-	return _model.isValidForProjection();
+	return _model.isValidForProjection() || (_models.size() && _models.front().isValidForProjection());
 }
 
 std::string CameraImages::getSerial() const
@@ -511,8 +614,10 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 	LaserScan scan(cv::Mat(), _scanMaxPts, 0, LaserScan::kUnknown, _scanLocalTransform);
 	double stamp = UTimer::now();
 	Transform odometryPose;
+	cv::Mat covariance;
 	Transform groundTruthPose;
 	cv::Mat depthFromScan;
+	CameraModel model = _model;
 	UDEBUG("");
 	if(_dir->isValid())
 	{
@@ -558,17 +663,27 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 				{
 					_captureDelay = _stamps.front() - stamp;
 				}
-				if(odometry_.size())
+			}
+			if(odometry_.size())
+			{
+				odometryPose = odometry_.front();
+				odometry_.pop_front();
+				if(covariances_.size())
 				{
-					odometryPose = odometry_.front();
+					covariance = covariances_.front();
-					odometry_.pop_front();
+					covariances_.pop_front();
-				}
-				if(groundTruth_.size())
-				{
-					groundTruthPose = groundTruth_.front();
-					groundTruth_.pop_front();
 				}
 			}
+			if(groundTruth_.size())
+			{
+				groundTruthPose = groundTruth_.front();
+				groundTruth_.pop_front();
+			}
+			if(_models.size() && !model.isValidForProjection())
+			{
+				model = _models.front();
+				_models.pop_front();
+			}
 		}
 		else
 		{
@@ -585,17 +700,27 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 					{
 						_captureDelay = _stamps.front() - stamp;
 					}
-					if(odometry_.size())
+				}
+				if(odometry_.size())
+				{
+					odometryPose = odometry_.front();
+					odometry_.pop_front();
+					if(covariances_.size())
 					{
-						odometryPose = odometry_.front();
+						covariance = covariances_.front();
-						odometry_.pop_front();
+						covariances_.pop_front();
-					}
-					if(groundTruth_.size())
-					{
-						groundTruthPose = groundTruth_.front();
-						groundTruth_.pop_front();
 					}
 				}
+				if(groundTruth_.size())
+				{
+					groundTruthPose = groundTruth_.front();
+					groundTruth_.pop_front();
+				}
+				if(_models.size() && !model.isValidForProjection())
+				{
+					model = _models.front();
+					_models.pop_front();
+				}
 
 				while(_count++ < _startAt && (fileName = _dir->getNextFileName()).size())
 				{
@@ -608,17 +733,27 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 						{
 							_captureDelay = _stamps.front() - stamp;
 						}
-						if(odometry_.size())
+					}
+					if(odometry_.size())
+					{
+						odometryPose = odometry_.front();
+						odometry_.pop_front();
+						if(covariances_.size())
 						{
-							odometryPose = odometry_.front();
+							covariance = covariances_.front();
-							odometry_.pop_front();
+							covariances_.pop_front();
-						}
-						if(groundTruth_.size())
-						{
-							groundTruthPose = groundTruth_.front();
-							groundTruth_.pop_front();
 						}
 					}
+					if(groundTruth_.size())
+					{
+						groundTruthPose = groundTruth_.front();
+						groundTruth_.pop_front();
+					}
+					if(_models.size() && !model.isValidForProjection())
+					{
+						model = _models.front();
+						_models.pop_front();
+					}
 				}
 			}
 			if(_scanDir)
@@ -698,12 +833,11 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 							UWARN("Error debayering images: \"%s\". Please set bayer mode to -1 if images are not bayered!", e.what());
 						}
 					}
-
 				}
 
-				if(!img.empty() && _model.isValidForRectification() && _rectifyImages)
+				if(!img.empty() && model.isValidForRectification() && _rectifyImages)
 				{
-					img = _model.rectifyImage(img);
+					img = model.rectifyImage(img);
 				}
 			}
 
@@ -716,7 +850,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 				if(_depthFromScan && !img.empty())
 				{
 					UDEBUG("Computing depth from scan...");
-					if(!_model.isValidForProjection())
+					if(!model.isValidForProjection())
 					{
 						UWARN("Depth from laser scan: Camera model should be valid.");
 					}
@@ -727,7 +861,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 					else
 					{
 						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
-						depthFromScan = util3d::projectCloudToCamera(img.size(), _model.K(), cloud, _model.localTransform());
+						depthFromScan = util3d::projectCloudToCamera(img.size(), model.K(), cloud, model.localTransform());
 						if(_depthFromScanFillHoles!=0)
 						{
 							util3d::fillProjectedCloudHoles(depthFromScan, _depthFromScanFillHoles>0, _depthFromScanFillHolesFromBorder);
@@ -742,18 +876,18 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 		UWARN("Directory is not set, camera must be initialized.");
 	}
 
-	if(_model.imageHeight() == 0 || _model.imageWidth() == 0)
+	if(model.imageHeight() == 0 || model.imageWidth() == 0)
 	{
-		_model.setImageSize(img.size());
+		model.setImageSize(img.size());
 	}
 
-	SensorData data(scan, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, _model, this->getNextSeqID(), stamp);
+	SensorData data(scan, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, model, this->getNextSeqID(), stamp);
 	data.setGroundTruth(groundTruthPose);
 
 	if(info && !odometryPose.isNull())
 	{
 		info->odomPose = odometryPose;
-		info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1); // Note that with TORO and g2o file formats, we could get the covariance
+		info->odomCovariance = covariance.empty()?cv::Mat::eye(6,6,CV_64FC1):covariance; // Note that with TORO and g2o file formats, we could get the covariance
 	}
 
 	return data;

corelib/src/camera/CameraRGBDImages.cpp

@@ -70,16 +70,6 @@ bool CameraRGBDImages::init(const std::string & calibrationFolder, const std::st
 	return success;
 }
 
-bool CameraRGBDImages::isCalibrated() const
-{
-	return this->cameraModel().isValidForProjection();
-}
-
-std::string CameraRGBDImages::getSerial() const
-{
-	return this->cameraModel().name();
-}
-
 SensorData CameraRGBDImages::captureImage(CameraInfo * info)
 {
 	SensorData data;

guilib/src/PreferencesDialog.cpp

@@ -697,6 +697,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->toolButton_cameraImages_gt, SIGNAL(clicked()), this, SLOT(selectSourceImagesPathGt()));
 	connect(_ui->lineEdit_cameraRGBDImages_path_rgb, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_cameraRGBDImages_path_depth, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->checkBox_cameraImages_configForEachFrame, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkBox_cameraImages_timestamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkBox_cameraImages_syncTimeStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->doubleSpinBox_cameraRGBDImages_scale, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1946,6 +1947,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->spinBox_stereoMyntEye_contrast->setValue(116);
 		_ui->spinBox_stereoMyntEye_irControl->setValue(0);
 
+		_ui->checkBox_cameraImages_configForEachFrame->setChecked(false);
 		_ui->checkBox_cameraImages_timestamps->setChecked(false);
 		_ui->checkBox_cameraImages_syncTimeStamps->setChecked(true);
 		_ui->lineEdit_cameraImages_timestamps->setText("");
@@ -2411,6 +2413,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->source_images_spinBox_maxFrames->setValue(settings.value("maxFrames",_ui->source_images_spinBox_maxFrames->value()).toInt());
 	_ui->comboBox_cameraImages_bayerMode->setCurrentIndex(settings.value("bayerMode",_ui->comboBox_cameraImages_bayerMode->currentIndex()).toInt());
 
+	_ui->checkBox_cameraImages_configForEachFrame->setChecked(settings.value("config_each_frame",_ui->checkBox_cameraImages_configForEachFrame->isChecked()).toBool());
 	_ui->checkBox_cameraImages_timestamps->setChecked(settings.value("filenames_as_stamps",_ui->checkBox_cameraImages_timestamps->isChecked()).toBool());
 	_ui->checkBox_cameraImages_syncTimeStamps->setChecked(settings.value("sync_stamps",_ui->checkBox_cameraImages_syncTimeStamps->isChecked()).toBool());
 	_ui->lineEdit_cameraImages_timestamps->setText(settings.value("stamps", _ui->lineEdit_cameraImages_timestamps->text()).toString());
@@ -2891,6 +2894,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("startPos", 		_ui->source_images_spinBox_startPos->value());
 	settings.setValue("maxFrames", 		_ui->source_images_spinBox_maxFrames->value());
 	settings.setValue("bayerMode", 	    _ui->comboBox_cameraImages_bayerMode->currentIndex());
+	settings.setValue("config_each_frame", _ui->checkBox_cameraImages_configForEachFrame->isChecked());
 	settings.setValue("filenames_as_stamps", _ui->checkBox_cameraImages_timestamps->isChecked());
 	settings.setValue("sync_stamps",    _ui->checkBox_cameraImages_syncTimeStamps->isChecked());
 	settings.setValue("stamps",              _ui->lineEdit_cameraImages_timestamps->text());
@@ -5783,6 +5787,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 				_ui->checkBox_cameraImages_timestamps->isChecked(),
 				_ui->lineEdit_cameraImages_timestamps->text().toStdString(),
 				_ui->checkBox_cameraImages_syncTimeStamps->isChecked());
+		((CameraRGBDImages*)camera)->setConfigForEachFrame(_ui->checkBox_cameraImages_configForEachFrame->isChecked());
 	}
 	else if(driver == kSrcDC1394)
 	{
@@ -5828,6 +5833,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 				_ui->checkBox_cameraImages_timestamps->isChecked(),
 				_ui->lineEdit_cameraImages_timestamps->text().toStdString(),
 				_ui->checkBox_cameraImages_syncTimeStamps->isChecked());
+		((CameraRGBDImages*)camera)->setConfigForEachFrame(_ui->checkBox_cameraImages_configForEachFrame->isChecked());
 
 	}
 	else if (driver == kSrcStereoUsb)
@@ -5966,6 +5972,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 				_ui->checkBox_cameraImages_timestamps->isChecked(),
 				_ui->lineEdit_cameraImages_timestamps->text().toStdString(),
 				_ui->checkBox_cameraImages_syncTimeStamps->isChecked());
+		((CameraRGBDImages*)camera)->setConfigForEachFrame(_ui->checkBox_cameraImages_configForEachFrame->isChecked());
 	}
 	else if(driver == kSrcDatabase)
 	{

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-498</y>
+            <y>0</y>
             <width>686</width>
-            <height>3236</height>
+            <height>3286</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -3064,7 +3064,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                       <item row="5" column="1">
                        <widget class="QLabel" name="label_36">
                         <property name="text">
-                         <string>Image decimation. RGB/Mono and depth images will be resized according to this value (size*1/decimation). Note that if depth images are captured, decimation should be a multiple of the depth image size.</string>
+                         <string>Image decimation. RGB/Mono and depth images will be resized according to this value (size*1/decimation). Note that if depth images are captured, decimation should be a multiple of the depth image size. If depth images are smaller than RGB images, the decimation is first applied on RGB, if the resulting RGB image is still bigger than depth image, the depth is not decimated.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -6087,7 +6087,20 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        </property>
                        <item>
                         <layout class="QGridLayout" name="gridLayout_68" columnstretch="0,0,1">
-                         <item row="7" column="2">
+                         <item row="8" column="2">
+                          <widget class="QLabel" name="label_288">
+                           <property name="text">
+                            <string>Ground truth file. Select the correct format below.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="9" column="2">
                           <widget class="QLabel" name="label_289">
                            <property name="text">
                             <string>Ground truth format. See tool tip for more details on formats. Note that formats without stamps should have the same number of values than the source images.</string>
@@ -6113,21 +6126,21 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="6" column="0">
+                         <item row="8" column="0">
                           <widget class="QToolButton" name="toolButton_cameraImages_gt">
                            <property name="text">
                             <string>...</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="4" column="1">
+                         <item row="6" column="1">
                           <widget class="QLineEdit" name="lineEdit_cameraImages_odom">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="9" column="2">
+                         <item row="11" column="2">
                           <widget class="QLabel" name="label_293">
                            <property name="text">
                             <string>Path to directory containing optional laser scans (*.pcd, *.ply, *.bin [KITTI format]). The directory should have the same size has the images directory. </string>
@@ -6140,7 +6153,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="4" column="2">
+                         <item row="6" column="2">
                           <widget class="QLabel" name="label_348">
                            <property name="text">
                             <string>Odometry file. Select the correct format below.</string>
@@ -6153,7 +6166,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="1" column="2">
+                         <item row="2" column="2">
                           <widget class="QLabel" name="label_255">
                            <property name="text">
                             <string>Use file names as timestamps. Format is epoch time. Example: &quot;1305031102.175304.png&quot;.</string>
@@ -6166,55 +6179,42 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="3" column="0">
+                         <item row="4" column="0">
                           <widget class="QToolButton" name="toolButton_cameraImages_timestamps">
                            <property name="text">
                             <string>...</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="3" column="1">
+                         <item row="4" column="1">
                           <widget class="QLineEdit" name="lineEdit_cameraImages_timestamps">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="6" column="1">
+                         <item row="8" column="1">
                           <widget class="QLineEdit" name="lineEdit_cameraImages_gt">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="1">
+                         <item row="3" column="1">
                           <widget class="QCheckBox" name="checkBox_cameraImages_syncTimeStamps">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="6" column="2">
+                         <item row="2" column="1">
-                          <widget class="QLabel" name="label_288">
-                           <property name="text">
-                            <string>Ground truth file. Select the correct format below.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="1" column="1">
                           <widget class="QCheckBox" name="checkBox_cameraImages_timestamps">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="11" column="2">
+                         <item row="13" column="2">
                           <widget class="QLabel" name="label_292">
                            <property name="text">
                             <string>Maximum laser scan points.</string>
@@ -6227,7 +6227,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="2">
+                         <item row="3" column="2">
                           <widget class="QLabel" name="label_256">
                            <property name="text">
                             <string>Synchronize capture rate with timestamps.</string>
@@ -6240,14 +6240,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="9" column="1">
+                         <item row="11" column="1">
                           <widget class="QLineEdit" name="lineEdit_cameraImages_path_scans">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="3" column="2">
+                         <item row="4" column="2">
                           <widget class="QLabel" name="label_251">
                            <property name="text">
                             <string>Timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. Not used if &quot;Use file names as timestamps&quot; above is checked. </string>
@@ -6260,7 +6260,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="11" column="1">
+                         <item row="13" column="1">
                           <widget class="QSpinBox" name="spinBox_cameraImages_max_scan_pts">
                            <property name="toolTip">
                             <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;KITTI: 130 000 points&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
@@ -6270,7 +6270,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="7" column="1">
+                         <item row="9" column="1">
                           <widget class="QComboBox" name="comboBox_cameraImages_gtFormat">
                            <property name="toolTip">
                             <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Raw Format (3 values): x y z&lt;br/&gt;Raw Format (6 values): x y z roll pitch yaw&lt;br/&gt;Raw Format (7 values): x y z qx qy qz qw&lt;br/&gt;Raw Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Raw Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;RGBD-SLAM (stamp tx ty tz qx qy qz qw)&lt;br/&gt;KITTI (stamp + 12 values transform)&lt;br/&gt;TORO&lt;br/&gt;g2o&lt;br/&gt;NewCollege (stamp x y)&lt;br/&gt;Malaga Urban (GPS)&lt;br/&gt;St Lucia Stereo (INS)&lt;br/&gt;EuRoC MAV (stamp,tx,ty,tz,qw,qx,qy,qz...)&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
@@ -6335,14 +6335,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </item>
                           </widget>
                          </item>
-                         <item row="9" column="0">
+                         <item row="11" column="0">
                           <widget class="QToolButton" name="toolButton_cameraImages_path_scans">
                            <property name="text">
                             <string>...</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="10" column="1">
+                         <item row="12" column="1">
                           <widget class="QLineEdit" name="lineEdit_cameraImages_laser_transform">
                            <property name="toolTip">
                             <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;KITTI: /base_link to /scan = -0.27 0 0.08 0 0 0&lt;br/&gt;KITTI: /base_footprint to /scan = -0.27 0 1.75 0 0 0&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
@@ -6352,7 +6352,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="10" column="2">
+                         <item row="12" column="2">
                           <widget class="QLabel" name="label_294">
                            <property name="text">
                             <string>Local transform from /base_link to /scan_link. Mouse over the box to show formats.</string>
@@ -6397,7 +6397,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </item>
                           </widget>
                          </item>
-                         <item row="5" column="2">
+                         <item row="7" column="2">
                           <widget class="QLabel" name="label_349">
                            <property name="text">
                             <string>Odometry format. See tool tip for more details on formats. Note that formats without stamps should have the same number of values than the source images.</string>
@@ -6410,7 +6410,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="5" column="1">
+                         <item row="7" column="1">
                           <widget class="QComboBox" name="comboBox_cameraImages_odomFormat">
                            <property name="toolTip">
                             <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Raw Format (3 values): x y z&lt;br/&gt;Raw Format (6 values): x y z roll pitch yaw&lt;br/&gt;Raw Format (7 values): x y z qx qy qz qw&lt;br/&gt;Raw Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Raw Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;RGBD-SLAM (stamp tx ty tz qx qy qz qw)&lt;br/&gt;KITTI (stamp + 12 values transform)&lt;br/&gt;TORO&lt;br/&gt;g2o&lt;br/&gt;NewCollege (stamp x y)&lt;br/&gt;Malaga Urban (GPS)&lt;br/&gt;St Lucia Stereo (INS)&lt;br/&gt;EuRoC MAV (stamp,tx,ty,tz,qw,qx,qy,qz...)&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
@@ -6475,14 +6475,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </item>
                           </widget>
                          </item>
-                         <item row="4" column="0">
+                         <item row="6" column="0">
                           <widget class="QToolButton" name="toolButton_cameraImages_odom">
                            <property name="text">
                             <string>...</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="8" column="2">
+                         <item row="10" column="2">
                           <widget class="QLabel" name="label_443">
                            <property name="text">
                             <string>Max time difference between data and corresponding pose for format with stamps. If delay is over this threshold, the pose won't be set on data loaded. This is used when odometry and/or ground truth files are set.</string>
@@ -6495,7 +6495,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="8" column="1">
+                         <item row="10" column="1">
                           <widget class="QDoubleSpinBox" name="doubleSpinBox_maxPoseTimeDiff">
                            <property name="suffix">
                             <string> s</string>
@@ -6514,7 +6514,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="13" column="2">
+                         <item row="15" column="2">
                           <widget class="QLabel" name="label_464">
                            <property name="text">
                             <string>Local transform from /base_link to /imu_link. Mouse over the box to show formats.</string>
@@ -6527,7 +6527,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="12" column="2">
+                         <item row="14" column="2">
                           <widget class="QLabel" name="label_463">
                            <property name="text">
                             <string>Path to file containing optional IMU data (*.csv [EuRoC format]).</string>
@@ -6540,21 +6540,21 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="12" column="0">
+                         <item row="14" column="0">
                           <widget class="QToolButton" name="toolButton_cameraImages_path_imu">
                            <property name="text">
                             <string>...</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="12" column="1">
+                         <item row="14" column="1">
                           <widget class="QLineEdit" name="lineEdit_cameraImages_path_imu">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="13" column="1">
+                         <item row="15" column="1">
                           <widget class="QLineEdit" name="lineEdit_cameraImages_imu_transform">
                            <property name="toolTip">
                             <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;EuRoC: /base_link to /imu = 0 0 1 0 -1 0 1 0 0&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
@@ -6564,7 +6564,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="14" column="2">
+                         <item row="16" column="2">
                           <widget class="QLabel" name="label_465">
                            <property name="text">
                             <string>IMU Rate. To synchronize capture rate with IMU timestamps, set to 0. This can be set a little over the actual IMU rate to keep up with camera capture rate if images are dropped by odometry.</string>
@@ -6577,7 +6577,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="14" column="1">
+                         <item row="16" column="1">
                           <widget class="QSpinBox" name="spinBox_cameraImages_max_imu_rate">
                            <property name="toolTip">
                             <string>EuRoC: 200 Hz -&gt; 250 Hz</string>
@@ -6587,6 +6587,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
+                         <item row="1" column="2">
+                          <widget class="QLabel" name="label_605">
+                           <property name="text">
+                            <string>Load config file for each frame (intrinsics, pose and stamp). Config files should be in the same directory than RGB frames and they should have the same name than the corresponding frame file. Currently supporting only 3DScannerApp for iOS export config format (JSON).</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="1" column="1">
+                          <widget class="QCheckBox" name="checkBox_cameraImages_configForEachFrame">
+                           <property name="text">
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                        <item>