CameraStereoVideo: added left and right separated video streams. Fixed issue #117

corelib/include/rtabmap/core/CameraStereo.h

@@ -202,7 +202,13 @@ public:
 
 public:
 	CameraStereoVideo(
-			const std::string & path,
+			const std::string & pathSideBySide,
+			bool rectifyImages = false,
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity());
+	CameraStereoVideo(
+			const std::string & pathLeft,
+			const std::string & pathRight,
 			bool rectifyImages = false,
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity());
@@ -222,7 +228,9 @@ protected:
 
 private:
 	cv::VideoCapture capture_;
+	cv::VideoCapture capture2_;
 	std::string path_;
+	std::string path2_;
 	bool rectifyImages_;
 	StereoCameraModel stereoModel_;
 	std::string cameraName_;

corelib/src/CameraStereo.cpp

@@ -1192,13 +1192,27 @@ CameraStereoVideo::CameraStereoVideo(
 {
 }
 
+CameraStereoVideo::CameraStereoVideo(
+		const std::string & pathLeft,
+		const std::string & pathRight,
+		bool rectifyImages,
+		float imageRate,
+		const Transform & localTransform) :
+		Camera(imageRate, localTransform),
+		path_(pathLeft),
+		path2_(pathRight),
+		rectifyImages_(rectifyImages),
+		src_(CameraVideo::kVideoFile),
+		usbDevice_(0)
+{
+}
+
 CameraStereoVideo::CameraStereoVideo(
 	int device,
 	bool rectifyImages,
 	float imageRate,
 	const Transform & localTransform) :
 	Camera(imageRate, localTransform),
-	path_(""),
 	rectifyImages_(rectifyImages),
 	src_(CameraVideo::kUsbDevice),
 	usbDevice_(device)
@@ -1208,6 +1222,7 @@ CameraStereoVideo::CameraStereoVideo(
 CameraStereoVideo::~CameraStereoVideo()
 {
 	capture_.release();
+	capture2_.release();
 }
 
 bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::string & cameraName)
@@ -1217,6 +1232,10 @@ bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::s
 	{
 		capture_.release();
 	}
+	if(capture2_.isOpened())
+	{
+		capture2_.release();
+	}
 
 	if (src_ == CameraVideo::kUsbDevice)
 	{
@@ -1225,55 +1244,63 @@ bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::s
 	}
 	else if (src_ == CameraVideo::kVideoFile)
 	{
-		ULOGGER_DEBUG("CameraStereoVideo: filename=\"%s\"", path_.c_str());
+		if(path2_.empty())
-		capture_.open(path_.c_str());
+		{
+			ULOGGER_DEBUG("CameraStereoVideo: filename=\"%s\"", path_.c_str());
+			capture_.open(path_.c_str());
+		}
+		else
+		{
+			ULOGGER_DEBUG("CameraStereoVideo: filenames=\"%s\" and \"%s\"", path_.c_str(), path2_.c_str());
+			capture_.open(path_.c_str());
+			capture2_.open(path2_.c_str());
+		}
 	}
 	else
 	{
 		ULOGGER_ERROR("CameraStereoVideo: Unknown source...");
 	}
 
-	if(!capture_.isOpened())
+	if(!capture_.isOpened() || (!path2_.empty() && !capture2_.isOpened()))
 	{
 		ULOGGER_ERROR("CameraStereoVideo: Failed to create a capture object!");
 		capture_.release();
+		capture2_.release();
 		return false;
 	}
-	else
+
+	if (cameraName_.empty())
 	{
-		if (cameraName_.empty())
+		unsigned int guid = (unsigned int)capture_.get(CV_CAP_PROP_GUID);
+		if (guid != 0 && guid != 0xffffffff)
 		{
-			unsigned int guid = (unsigned int)capture_.get(CV_CAP_PROP_GUID);
+			cameraName_ = uFormat("%08x", guid);
-			if (guid != 0 && guid != 0xffffffff)
-			{
-				cameraName_ = uFormat("%08x", guid);
-			}
 		}
+	}
 
-		// look for calibration files
+	// look for calibration files
-		if(!calibrationFolder.empty() && !cameraName_.empty())
+	if(!calibrationFolder.empty() && !cameraName_.empty())
+	{
+		if(!stereoModel_.load(calibrationFolder, cameraName_))
 		{
-			if(!stereoModel_.load(calibrationFolder, cameraName_))
+			UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
-			{
+				cameraName_.c_str(), calibrationFolder.c_str());
-				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
-					cameraName_.c_str(), calibrationFolder.c_str());
-			}
-			else
-			{
-				UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
-						stereoModel_.left().fx(),
-						stereoModel_.left().cx(),
-						stereoModel_.left().cy(),
-						stereoModel_.baseline());
-			}
 		}
+		else
+		{
+			UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
+					stereoModel_.left().fx(),
+					stereoModel_.left().cx(),
+					stereoModel_.left().cy(),
+					stereoModel_.baseline());
+		}
+	}
 
-		stereoModel_.setLocalTransform(this->getLocalTransform());
+	stereoModel_.setLocalTransform(this->getLocalTransform());
-		if(rectifyImages_ && !stereoModel_.isValidForRectification())
+	if(rectifyImages_ && !stereoModel_.isValidForRectification())
-		{
+	{
-			UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
+		UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
-			return false;
+		return false;
-		}
 	}
 	return true;
 }
@@ -1293,43 +1320,61 @@ SensorData CameraStereoVideo::captureImage(CameraInfo * info)
 	SensorData data;
 
 	cv::Mat img;
-	if(capture_.isOpened())
+	if(capture_.isOpened() && (path2_.empty() || capture2_.isOpened()))
 	{
-		if(capture_.read(img))
+		cv::Mat leftImage;
+		cv::Mat rightImage;
+		if(path2_.empty())
 		{
-			// Rectification
+			if(!capture_.read(img))
-			cv::Mat leftImage(img, cv::Rect( 0, 0, img.size().width/2, img.size().height ));
-			cv::Mat rightImage(img, cv::Rect( img.size().width/2, 0, img.size().width/2, img.size().height ));
-			bool rightCvt = false;
-			if(rightImage.type() != CV_8UC1)
 			{
-				cv::Mat tmp;
+				return data;
-				cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
-				rightImage = tmp;
-				rightCvt = true;
 			}
-
+			// Side by side stream
-			if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
+			leftImage = cv::Mat(img, cv::Rect( 0, 0, img.size().width/2, img.size().height ));
-			{
+			rightImage = cv::Mat(img, cv::Rect( img.size().width/2, 0, img.size().width/2, img.size().height ));
-				leftImage = stereoModel_.left().rectifyImage(leftImage);
-				rightImage = stereoModel_.right().rectifyImage(rightImage);
-			}
-			else
-			{
-				leftImage = leftImage.clone();
-				if(!rightCvt)
-				{
-					rightImage = rightImage.clone();
-				}
-			}
-
-			if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
-			{
-				stereoModel_.setImageSize(leftImage.size());
-			}
-
-			data = SensorData(leftImage, rightImage, stereoModel_, this->getNextSeqID(), UTimer::now());
 		}
+		else if(!capture_.read(leftImage) || !capture2_.read(rightImage))
+		{
+			return data;
+		}
+		else if(leftImage.cols != rightImage.cols || leftImage.rows != rightImage.rows)
+		{
+			UERROR("Left and right streams don't have image of the same size: left=%dx%d right=%dx%d",
+					leftImage.cols, leftImage.rows, rightImage.cols, rightImage.rows);
+			return data;
+		}
+
+		// Rectification
+		bool rightCvt = false;
+		if(rightImage.type() != CV_8UC1)
+		{
+			cv::Mat tmp;
+			cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
+			rightImage = tmp;
+			rightCvt = true;
+		}
+
+		if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
+		{
+			leftImage = stereoModel_.left().rectifyImage(leftImage);
+			rightImage = stereoModel_.right().rectifyImage(rightImage);
+		}
+		else
+		{
+			leftImage = leftImage.clone();
+			if(!rightCvt)
+			{
+				rightImage = rightImage.clone();
+			}
+		}
+
+		if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
+		{
+			stereoModel_.setImageSize(leftImage.size());
+		}
+
+		data = SensorData(leftImage, rightImage, stereoModel_, this->getNextSeqID(), UTimer::now());
 	}
 	else
 	{

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -304,6 +304,7 @@ private slots:
 	void selectSourceImagesPath();
 	void selectSourceVideoPath();
 	void selectSourceStereoVideoPath();
+	void selectSourceStereoVideoPath2();
 	void selectSourceOniPath();
 	void selectSourceOni2Path();
 	void selectSourceSvoPath();

guilib/src/PreferencesDialog.cpp

@@ -483,7 +483,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->checkBox_stereoImages_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
 	connect(_ui->toolButton_cameraStereoVideo_path, SIGNAL(clicked()), this, SLOT(selectSourceStereoVideoPath()));
+	connect(_ui->toolButton_cameraStereoVideo_path_2, SIGNAL(clicked()), this, SLOT(selectSourceStereoVideoPath2()));
 	connect(_ui->lineEdit_cameraStereoVideo_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->lineEdit_cameraStereoVideo_path_2, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkBox_stereoVideo_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
 	connect(_ui->comboBox_stereoZed_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1340,6 +1342,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->lineEdit_cameraStereoImages_path_right->setText("");
 		_ui->checkBox_stereoImages_rectify->setChecked(false);
 		_ui->lineEdit_cameraStereoVideo_path->setText("");
+		_ui->lineEdit_cameraStereoVideo_path_2->setText("");
 		_ui->checkBox_stereoVideo_rectify->setChecked(false);
 		_ui->comboBox_stereoZed_resolution->setCurrentIndex(2);
 		_ui->comboBox_stereoZed_quality->setCurrentIndex(1);
@@ -1689,6 +1692,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 
 	settings.beginGroup("StereoVideo");
 	_ui->lineEdit_cameraStereoVideo_path->setText(settings.value("path", _ui->lineEdit_cameraStereoVideo_path->text()).toString());
+	_ui->lineEdit_cameraStereoVideo_path_2->setText(settings.value("path2", _ui->lineEdit_cameraStereoVideo_path_2->text()).toString());
 	_ui->checkBox_stereoVideo_rectify->setChecked(settings.value("rectify",_ui->checkBox_stereoVideo_rectify->isChecked()).toBool());
 	settings.endGroup(); // StereoVideo
 
@@ -2115,6 +2119,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 
 	settings.beginGroup("StereoVideo");
 	settings.setValue("path", 			_ui->lineEdit_cameraStereoVideo_path->text());
+	settings.setValue("path2", 			_ui->lineEdit_cameraStereoVideo_path_2->text());
 	settings.setValue("rectify", 	    _ui->checkBox_stereoVideo_rectify->isChecked());
 	settings.endGroup(); // StereoVideo
 
@@ -2932,7 +2937,7 @@ void PreferencesDialog::selectSourceVideoPath()
 	{
 		dir = getWorkingDirectory();
 	}
-	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->source_video_lineEdit_path->text(), tr("Videos (*.avi *.mpg *.mp4)"));
+	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->source_video_lineEdit_path->text(), tr("Videos (*.avi *.mpg *.mp4 *.mov *.mkv)"));
 	if(!path.isEmpty())
 	{
 		_ui->source_video_lineEdit_path->setText(path);
@@ -2946,13 +2951,27 @@ void PreferencesDialog::selectSourceStereoVideoPath()
 	{
 		dir = getWorkingDirectory();
 	}
-	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->lineEdit_cameraStereoVideo_path->text(), tr("Videos (*.avi *.mpg *.mp4)"));
+	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->lineEdit_cameraStereoVideo_path->text(), tr("Videos (*.avi *.mpg *.mp4 *.mov *.mkv)"));
 	if(!path.isEmpty())
 	{
 		_ui->lineEdit_cameraStereoVideo_path->setText(path);
 	}
 }
 
+void PreferencesDialog::selectSourceStereoVideoPath2()
+{
+	QString dir = _ui->lineEdit_cameraStereoVideo_path_2->text();
+	if(dir.isEmpty())
+	{
+		dir = getWorkingDirectory();
+	}
+	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->lineEdit_cameraStereoVideo_path_2->text(), tr("Videos (*.avi *.mpg *.mp4 *.mov *.mkv)"));
+	if(!path.isEmpty())
+	{
+		_ui->lineEdit_cameraStereoVideo_path_2->setText(path);
+	}
+}
+
 void PreferencesDialog::selectSourceOniPath()
 {
 	QString dir = _ui->lineEdit_openniOniPath->text();
@@ -4243,11 +4262,25 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 	}
 	else if(driver == kSrcStereoVideo)
 	{
-		camera = new CameraStereoVideo(
+		if(!_ui->lineEdit_cameraStereoVideo_path_2->text().isEmpty())
-			_ui->lineEdit_cameraStereoVideo_path->text().toStdString(),
+		{
-			_ui->checkBox_stereoVideo_rectify->isChecked() && !useRawImages,
+			// left and right videos
-			this->getGeneralInputRate(),
+			camera = new CameraStereoVideo(
-			this->getSourceLocalTransform());
+					_ui->lineEdit_cameraStereoVideo_path->text().toStdString(),
+					_ui->lineEdit_cameraStereoVideo_path_2->text().toStdString(),
+					_ui->checkBox_stereoVideo_rectify->isChecked() && !useRawImages,
+					this->getGeneralInputRate(),
+					this->getSourceLocalTransform());
+		}
+		else
+		{
+			// side-by-side video
+			camera = new CameraStereoVideo(
+					_ui->lineEdit_cameraStereoVideo_path->text().toStdString(),
+					_ui->checkBox_stereoVideo_rectify->isChecked() && !useRawImages,
+					this->getGeneralInputRate(),
+					this->getSourceLocalTransform());
+		}
 	}
 	else if (driver == kSrcStereoZed)
 	{

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-279</y>
+            <y>-490</y>
             <width>673</width>
             <height>2496</height>
            </rect>
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>8</number>
+                <number>5</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -2357,7 +2357,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                     <item>
                      <widget class="QStackedWidget" name="stackedWidget_src">
                       <property name="currentIndex">
-                       <number>0</number>
+                       <number>1</number>
                       </property>
                       <widget class="QWidget" name="page_41">
                        <layout class="QVBoxLayout" name="verticalLayout_64">
@@ -2467,7 +2467,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_rgbd">
                              <property name="currentIndex">
-                              <number>6</number>
+                              <number>2</number>
                              </property>
                              <widget class="QWidget" name="page_32">
                               <layout class="QVBoxLayout" name="verticalLayout_63">
@@ -3219,7 +3219,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                </item>
                                <item>
                                 <property name="text">
-                                 <string>Video (Side-by-Side)</string>
+                                 <string>Video</string>
                                 </property>
                                </item>
                                <item>
@@ -3266,7 +3266,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_stereo">
                              <property name="currentIndex">
-                              <number>4</number>
+                              <number>3</number>
                              </property>
                              <widget class="QWidget" name="page_49">
                               <layout class="QVBoxLayout" name="verticalLayout_91">
@@ -3419,22 +3419,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                   </sizepolicy>
                                  </property>
                                  <property name="title">
-                                  <string>Stereo Side-By-Side Video (*.avi)</string>
+                                  <string>Stereo Video (*.avi, *.mp4)</string>
                                  </property>
-                                 <layout class="QGridLayout" name="gridLayout_66" columnstretch="0,0">
+                                 <layout class="QGridLayout" name="gridLayout_66" columnstretch="0,0,1">
-                                  <item row="2" column="0">
-                                   <spacer name="verticalSpacer_39">
-                                    <property name="orientation">
-                                     <enum>Qt::Vertical</enum>
-                                    </property>
-                                    <property name="sizeHint" stdset="0">
-                                     <size>
-                                      <width>20</width>
-                                      <height>0</height>
-                                     </size>
-                                    </property>
-                                   </spacer>
-                                  </item>
                                   <item row="0" column="0">
                                    <widget class="QToolButton" name="toolButton_cameraStereoVideo_path">
                                     <property name="text">
@@ -3449,14 +3436,33 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="1" column="0">
+                                  <item row="0" column="2">
-                                   <widget class="QCheckBox" name="checkBox_stereoVideo_rectify">
+                                   <widget class="QLabel" name="label_339">
                                     <property name="text">
-                                     <string/>
+                                     <string>Side-by-Side (SBS) video or left video.</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">
+                                  <item row="3" column="0">
+                                   <spacer name="verticalSpacer_39">
+                                    <property name="orientation">
+                                     <enum>Qt::Vertical</enum>
+                                    </property>
+                                    <property name="sizeHint" stdset="0">
+                                     <size>
+                                      <width>20</width>
+                                      <height>0</height>
+                                     </size>
+                                    </property>
+                                   </spacer>
+                                  </item>
+                                  <item row="2" column="2">
                                    <widget class="QLabel" name="label_234">
                                     <property name="text">
                                      <string>Rectify images. If checked, the images will be rectified using the calibration file (if its name is set above). If not checked, we assume that images are already rectified.</string>
@@ -3469,6 +3475,40 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
+                                  <item row="2" column="1">
+                                   <widget class="QCheckBox" name="checkBox_stereoVideo_rectify">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="1" column="2">
+                                   <widget class="QLabel" name="label_340">
+                                    <property name="text">
+                                     <string>Right video. Should be empty if a SBS video is set above.</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="QLineEdit" name="lineEdit_cameraStereoVideo_path_2">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="1" column="0">
+                                   <widget class="QToolButton" name="toolButton_cameraStereoVideo_path_2">
+                                    <property name="text">
+                                     <string>...</string>
+                                    </property>
+                                   </widget>
+                                  </item>
                                  </layout>
                                 </widget>
                                </item>