i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-09 13:00:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
matlabbe
2016-09-11 12:45:12 -04:00
parent 26c7078923
commit 44caa12fa5
5 changed files with 224 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/CameraStereo.h
+9 -1
View File
@@ -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
+109 -64
View File
@@ -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
{
guilib/include/rtabmap/gui/PreferencesDialog.h
+1
View File
@@ -304,6 +304,7 @@ private slots:
void selectSourceImagesPath();
void selectSourceVideoPath();
void selectSourceStereoVideoPath();
void selectSourceStereoVideoPath2();
void selectSourceOniPath();
void selectSourceOni2Path();
void selectSourceSvoPath();
guilib/src/PreferencesDialog.cpp
+40 -7
View File
@@ -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)
{
guilib/src/ui/preferencesDialog.ui
+65 -25
View File
@@ -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">
<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>
<layout class="QGridLayout" name="gridLayout_66" columnstretch="0,0,1">
<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">
<widget class="QCheckBox" name="checkBox_stereoVideo_rectify">
<item row="0" column="2">
<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>