From 44caa12fa5cc1ef26c725b1c7357d794ed23662d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 11 Sep 2016 12:45:12 -0400 Subject: [PATCH] CameraStereoVideo: added left and right separated video streams. Fixed issue #117 --- corelib/include/rtabmap/core/CameraStereo.h | 10 +- corelib/src/CameraStereo.cpp | 173 +++++++++++------- .../include/rtabmap/gui/PreferencesDialog.h | 1 + guilib/src/PreferencesDialog.cpp | 47 ++++- guilib/src/ui/preferencesDialog.ui | 90 ++++++--- 5 files changed, 224 insertions(+), 97 deletions(-) diff --git a/corelib/include/rtabmap/core/CameraStereo.h b/corelib/include/rtabmap/core/CameraStereo.h index 714e6491..1e434bb5 100644 --- a/corelib/include/rtabmap/core/CameraStereo.h +++ b/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_; diff --git a/corelib/src/CameraStereo.cpp b/corelib/src/CameraStereo.cpp index ac5d26b3..d4a12732 100644 --- a/corelib/src/CameraStereo.cpp +++ b/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()); - capture_.open(path_.c_str()); + if(path2_.empty()) + { + 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); - if (guid != 0 && guid != 0xffffffff) - { - cameraName_ = uFormat("%08x", guid); - } + cameraName_ = uFormat("%08x", guid); } + } - // look for calibration files - if(!calibrationFolder.empty() && !cameraName_.empty()) + // look for calibration files + 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()); - } - else - { - UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f", - stereoModel_.left().fx(), - stereoModel_.left().cx(), - stereoModel_.left().cy(), - stereoModel_.baseline()); - } + 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()); + } + } - stereoModel_.setLocalTransform(this->getLocalTransform()); - if(rectifyImages_ && !stereoModel_.isValidForRectification()) - { - UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid."); - return false; - } + stereoModel_.setLocalTransform(this->getLocalTransform()); + if(rectifyImages_ && !stereoModel_.isValidForRectification()) + { + UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid."); + 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 - 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) + if(!capture_.read(img)) { - cv::Mat tmp; - cv::cvtColor(rightImage, tmp, CV_BGR2GRAY); - rightImage = tmp; - rightCvt = true; + return data; } - - 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()); + // Side by side stream + 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 )); } + 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 { diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 82735613..79e9df36 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/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(); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 261ebd42..67dd49f7 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/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( - _ui->lineEdit_cameraStereoVideo_path->text().toStdString(), - _ui->checkBox_stereoVideo_rectify->isChecked() && !useRawImages, - this->getGeneralInputRate(), - this->getSourceLocalTransform()); + if(!_ui->lineEdit_cameraStereoVideo_path_2->text().isEmpty()) + { + // left and right videos + camera = new CameraStereoVideo( + _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) { diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 539102cf..eaefd5f9 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -279 + -490 673 2496 @@ -86,7 +86,7 @@ QFrame::Raised - 8 + 5 @@ -2357,7 +2357,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 0 + 1 @@ -2467,7 +2467,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 6 + 2 @@ -3219,7 +3219,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - Video (Side-by-Side) + Video @@ -3266,7 +3266,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 4 + 3 @@ -3419,22 +3419,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - Stereo Side-By-Side Video (*.avi) + Stereo Video (*.avi, *.mp4) - - - - - Qt::Vertical - - - - 20 - 0 - - - - + @@ -3449,14 +3436,33 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - + Side-by-Side (SBS) video or left video. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - + + + + Qt::Vertical + + + + 20 + 0 + + + + + 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. @@ -3469,6 +3475,40 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + + + + + + + + Right video. Should be empty if a SBS video is set above. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + ... + + +