Skip to content

Commit

Permalink
Adding FOURCC usb camera support (#1366)
Browse files Browse the repository at this point in the history 
* Adding FOURCC usb camera support

* Improved warnings

* more checks and warnings
  • Loading branch information
@matlabbe
matlabbe authored Oct 28, 2024
1 parent 2454b04 commit 8b21c27
Show file tree
Hide file tree
Showing 6 changed files with 310 additions and 60 deletions.
2 changes: 2 additions & 0 deletions corelib/include/rtabmap/core/camera/CameraStereoVideo.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,7 @@ class RTABMAP_CORE_EXPORT CameraStereoVideo :
virtual std::string getSerial() const;

void setResolution(int width, int height) {_width=width, _height=height;}
void setFOURCC(const std::string & fourcc) { _fourcc = fourcc; }

protected:
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
Expand All @@ -86,6 +87,7 @@ class RTABMAP_CORE_EXPORT CameraStereoVideo :
int usbDevice2_;
int _width;
int _height;
std::string _fourcc;
};

} // namespace rtabmap
2 changes: 2 additions & 0 deletions corelib/include/rtabmap/core/camera/CameraVideo.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,7 @@ class RTABMAP_CORE_EXPORT CameraVideo :
* has been loaded, thus resolution from calibration is used.
* */
void setResolution(int width, int height) {_width=width, _height=height;}
void setFOURCC(const std::string & fourcc) { _fourcc = fourcc; }

protected:
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
Expand All @@ -79,6 +80,7 @@ class RTABMAP_CORE_EXPORT CameraVideo :
std::string _guid;
int _width;
int _height;
std::string _fourcc;

CameraModel _model;
};
Expand Down
120 changes: 112 additions & 8 deletions corelib/src/camera/CameraStereoVideo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -199,30 +199,134 @@ bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::s
{
if(stereoModel_.isValidForProjection())
{
if(_width > 0 && _height > 0 && (_width!=stereoModel_.left().imageWidth() || _height != stereoModel_.left().imageHeight()))
{
UWARN("Desired resolution of %dx%d is set but calibration has "
"been loaded with resolution %dx%d, using calibration resolution.",
_width, _height,
stereoModel_.left().imageWidth(), stereoModel_.left().imageHeight());
}

if(capture_.isOpened())
{
capture_.set(CV_CAP_PROP_FRAME_WIDTH, stereoModel_.left().imageWidth()*(capture2_.isOpened()?1:2));
capture_.set(CV_CAP_PROP_FRAME_HEIGHT, stereoModel_.left().imageHeight());
bool resolutionSet = false;
resolutionSet = capture_.set(CV_CAP_PROP_FRAME_WIDTH, stereoModel_.left().imageWidth()*(capture2_.isOpened()?1:2));
resolutionSet = resolutionSet && capture_.set(CV_CAP_PROP_FRAME_HEIGHT, stereoModel_.left().imageHeight());
if(capture2_.isOpened())
{
capture2_.set(CV_CAP_PROP_FRAME_WIDTH, stereoModel_.right().imageWidth());
capture2_.set(CV_CAP_PROP_FRAME_HEIGHT, stereoModel_.right().imageHeight());
resolutionSet = resolutionSet && capture2_.set(CV_CAP_PROP_FRAME_WIDTH, stereoModel_.right().imageWidth());
resolutionSet = resolutionSet && capture2_.set(CV_CAP_PROP_FRAME_HEIGHT, stereoModel_.right().imageHeight());
}

// Check if the resolution was set successfully
int actualWidth = int(capture_.get(CV_CAP_PROP_FRAME_WIDTH));
int actualHeight = int(capture_.get(CV_CAP_PROP_FRAME_HEIGHT));
if(!resolutionSet ||
actualWidth != stereoModel_.left().imageWidth()*(capture2_.isOpened()?1:2) ||
actualHeight != stereoModel_.left().imageHeight())
{
UERROR("Calibration resolution (%dx%d) cannot be set to camera driver, "
"actual resolution is %dx%d. You would have to re-calibrate with one "
"supported format by your camera. "
"Do \"v4l2-ctl --list-formats-ext\" to list all supported "
"formats by your camera. For side-by-side format, you should set listed width/2.",
stereoModel_.left().imageWidth(), stereoModel_.left().imageHeight(),
actualWidth/(capture2_.isOpened()?1:2), actualHeight);
}
}
}
else if(_width > 0 && _height > 0)
{
if(capture_.isOpened())
{
capture_.set(CV_CAP_PROP_FRAME_WIDTH, _width*(capture2_.isOpened()?1:2));
capture_.set(CV_CAP_PROP_FRAME_HEIGHT, _height);
bool resolutionSet = false;
resolutionSet = capture_.set(CV_CAP_PROP_FRAME_WIDTH, _width*(capture2_.isOpened()?1:2));
resolutionSet = resolutionSet && capture_.set(CV_CAP_PROP_FRAME_HEIGHT, _height);
if(capture2_.isOpened())
{
capture2_.set(CV_CAP_PROP_FRAME_WIDTH, _width);
capture2_.set(CV_CAP_PROP_FRAME_HEIGHT, _height);
resolutionSet = resolutionSet && capture2_.set(CV_CAP_PROP_FRAME_WIDTH, _width);
resolutionSet = resolutionSet && capture2_.set(CV_CAP_PROP_FRAME_HEIGHT, _height);
}

// Check if the resolution was set successfully
int actualWidth = int(capture_.get(CV_CAP_PROP_FRAME_WIDTH));
int actualHeight = int(capture_.get(CV_CAP_PROP_FRAME_HEIGHT));
if(!resolutionSet ||
actualWidth != _width*(capture2_.isOpened()?1:2) ||
actualHeight != _height)
{
UWARN("Desired resolution (%dx%d) cannot be set to camera driver, "
"actual resolution is %dx%d. "
"Do \"v4l2-ctl --list-formats-ext\" to list all supported "
"formats by your camera. For side-by-side format, you should set listed width/2.",
_width, _height,
actualWidth/(capture2_.isOpened()?1:2), actualHeight);
}
}
}

// Set FPS
if (this->getFrameRate() > 0)
{
bool fpsSupported = false;
fpsSupported = capture_.set(CV_CAP_PROP_FPS, this->getFrameRate());
if (capture2_.isOpened())
{
fpsSupported = fpsSupported && capture2_.set(CV_CAP_PROP_FPS, this->getFrameRate());
}
if(fpsSupported)
{
// Check if the FPS was set successfully
double actualFPS = capture_.get(cv::CAP_PROP_FPS);

if(fabs(actualFPS - this->getFrameRate()) < 0.01)
{
this->setFrameRate(0);
}
else
{
UWARN("Desired FPS (%f Hz) cannot be set to camera driver, "
"actual FPS is %f Hz. We will throttle to lowest FPS. "
"Do \"v4l2-ctl --list-formats-ext\" to list all supported "
"formats by your camera.",
this->getFrameRate(), actualFPS);
if(this->getFrameRate() > actualFPS)
{
this->setFrameRate(0);
}
}
}
}

// Set FOURCC
if (!_fourcc.empty())
{
if(_fourcc.size() == 4)
{
std::string fourccUpperCase = uToUpperCase(_fourcc);
int fourcc = cv::VideoWriter::fourcc(fourccUpperCase.at(0), fourccUpperCase.at(1), fourccUpperCase.at(2), fourccUpperCase.at(3));
bool fourccSupported = false;
fourccSupported = capture_.set(CV_CAP_PROP_FOURCC, fourcc);
if (capture2_.isOpened())
{
fourccSupported = fourccSupported && capture2_.set(CV_CAP_PROP_FOURCC, fourcc);
}

// Check if the FOURCC was set successfully
int actualFourcc = int(capture_.get(CV_CAP_PROP_FOURCC));

if(!fourccSupported || actualFourcc != fourcc)
{
UWARN("Camera doesn't support provided FOURCC \"%s\". "
"Do \"v4l2-ctl --list-formats-ext\" to list all supported "
"formats by your camera.", fourccUpperCase.c_str());
}
}
else
{
UERROR("FOURCC parameter should be 4 characters, current value is \"%s\"", _fourcc.c_str());
}
}
}

if(rectifyImages_ && !stereoModel_.isValidForRectification())
Expand Down
96 changes: 92 additions & 4 deletions corelib/src/camera/CameraVideo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -134,13 +134,101 @@ bool CameraVideo::init(const std::string & calibrationFolder, const std::string
{
if(_model.isValidForProjection())
{
_capture.set(CV_CAP_PROP_FRAME_WIDTH, _model.imageWidth());
_capture.set(CV_CAP_PROP_FRAME_HEIGHT, _model.imageHeight());
if(_width > 0 && _height > 0 && (_width!=_model.imageWidth() || _height != _model.imageHeight()))
{
UWARN("Desired resolution of %dx%d is set but calibration has "
"been loaded with resolution %dx%d, using calibration resolution.",
_width, _height,
_model.imageWidth(), _model.imageHeight());
}

bool resolutionSet = false;
resolutionSet = _capture.set(CV_CAP_PROP_FRAME_WIDTH, _model.imageWidth());
resolutionSet = resolutionSet && _capture.set(CV_CAP_PROP_FRAME_HEIGHT, _model.imageHeight());

// Check if the resolution was set successfully
int actualWidth = int(_capture.get(CV_CAP_PROP_FRAME_WIDTH));
int actualHeight = int(_capture.get(CV_CAP_PROP_FRAME_HEIGHT));
if(!resolutionSet ||
actualWidth != _model.imageWidth() ||
actualHeight != _model.imageHeight())
{
UERROR("Calibration resolution (%dx%d) cannot be set to camera driver, "
"actual resolution is %dx%d. You would have to re-calibrate with one "
"supported format by your camera. "
"Do \"v4l2-ctl --list-formats-ext\" to list all supported "
"formats by your camera.",
_model.imageWidth(), _model.imageHeight(),
actualWidth, actualHeight);
}
}
else if(_width > 0 && _height > 0)
{
_capture.set(CV_CAP_PROP_FRAME_WIDTH, _width);
_capture.set(CV_CAP_PROP_FRAME_HEIGHT, _height);
int resolutionSet = false;
resolutionSet = _capture.set(CV_CAP_PROP_FRAME_WIDTH, _width);
resolutionSet = resolutionSet && _capture.set(CV_CAP_PROP_FRAME_HEIGHT, _height);

// Check if the resolution was set successfully
int actualWidth = int(_capture.get(CV_CAP_PROP_FRAME_WIDTH));
int actualHeight = int(_capture.get(CV_CAP_PROP_FRAME_HEIGHT));
if(!resolutionSet || actualWidth != _width || actualHeight != _height)
{
UWARN("Desired resolution (%dx%d) cannot be set to camera driver, "
"actual resolution is %dx%d. "
"Do \"v4l2-ctl --list-formats-ext\" to list all supported "
"formats by your camera.",
_width, _height, actualWidth, actualHeight);
}
}

// Set FPS
if (this->getFrameRate() > 0 && _capture.set(CV_CAP_PROP_FPS, this->getFrameRate()))
{
// Check if the FPS was set successfully
double actualFPS = _capture.get(cv::CAP_PROP_FPS);

if(fabs(actualFPS - this->getFrameRate()) < 0.01)
{
this->setFrameRate(0);
}
else
{
UWARN("Desired FPS (%f Hz) cannot be set to camera driver, "
"actual FPS is %f Hz. We will throttle to lowest FPS. "
"Do \"v4l2-ctl --list-formats-ext\" to list all supported "
"formats by your camera.",
this->getFrameRate(), actualFPS);
if(this->getFrameRate() > actualFPS)
{
this->setFrameRate(0);
}
}
}

// Set FOURCC
if (!_fourcc.empty())
{
if(_fourcc.size() == 4)
{
std::string fourccUpperCase = uToUpperCase(_fourcc);
int fourcc = cv::VideoWriter::fourcc(fourccUpperCase.at(0), fourccUpperCase.at(1), fourccUpperCase.at(2), fourccUpperCase.at(3));

bool fourccSupported = _capture.set(CV_CAP_PROP_FOURCC, fourcc);

// Check if the FOURCC was set successfully
int actualFourcc = int(_capture.get(CV_CAP_PROP_FOURCC));

if(!fourccSupported || actualFourcc != fourcc)
{
UWARN("Camera doesn't support provided FOURCC \"%s\". "
"Do \"v4l2-ctl --list-formats-ext\" to list all supported "
"formats by your camera.", fourccUpperCase.c_str());
}
}
else
{
UERROR("FOURCC parameter should be 4 characters, current value is \"%s\"", _fourcc.c_str());
}
}
}
if(_rectifyImages && !_model.isValidForRectification())
Expand Down
14 changes: 14 additions & 0 deletions guilib/src/PreferencesDialog.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -724,6 +724,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
// usb group
connect(_ui->spinBox_usbcam_streamWidth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_usbcam_streamHeight, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_usbcam_fourcc, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
//video group
connect(_ui->source_video_toolButton_selectSource, SIGNAL(clicked()), this, SLOT(selectSourceVideoPath()));
connect(_ui->source_video_lineEdit_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
Expand Down Expand Up @@ -836,6 +837,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->spinBox_stereo_right_device, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_stereousbcam_streamWidth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_stereousbcam_streamHeight, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_stereousbcam_fourcc, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));

connect(_ui->comboBox_stereoZed_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->comboBox_stereoZed_quality, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
Expand Down Expand Up @@ -2732,6 +2734,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
_ui->spinBox_stereo_right_device->setValue(settings.value("device2", _ui->spinBox_stereo_right_device->value()).toInt());
_ui->spinBox_stereousbcam_streamWidth->setValue(settings.value("width", _ui->spinBox_stereousbcam_streamWidth->value()).toInt());
_ui->spinBox_stereousbcam_streamHeight->setValue(settings.value("height", _ui->spinBox_stereousbcam_streamHeight->value()).toInt());
_ui->lineEdit_stereousbcam_fourcc->setText(settings.value("fourcc", _ui->lineEdit_stereousbcam_fourcc->text()).toString());
settings.endGroup(); // StereoVideo

settings.beginGroup("StereoZed");
Expand Down Expand Up @@ -2809,6 +2812,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
settings.beginGroup("UsbCam");
_ui->spinBox_usbcam_streamWidth->setValue(settings.value("width", _ui->spinBox_usbcam_streamWidth->value()).toInt());
_ui->spinBox_usbcam_streamHeight->setValue(settings.value("height", _ui->spinBox_usbcam_streamHeight->value()).toInt());
_ui->lineEdit_usbcam_fourcc->setText(settings.value("fourcc", _ui->lineEdit_usbcam_fourcc->text()).toString());
settings.endGroup(); // UsbCam

settings.beginGroup("Video");
Expand Down Expand Up @@ -3332,6 +3336,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.setValue("device2", _ui->spinBox_stereo_right_device->value());
settings.setValue("width", _ui->spinBox_stereousbcam_streamWidth->value());
settings.setValue("height", _ui->spinBox_stereousbcam_streamHeight->value());
settings.setValue("fourcc", _ui->lineEdit_stereousbcam_fourcc->text());
settings.endGroup(); // StereoVideo

settings.beginGroup("StereoZed");
Expand Down Expand Up @@ -3407,6 +3412,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.beginGroup("UsbCam");
settings.setValue("width", _ui->spinBox_usbcam_streamWidth->value());
settings.setValue("height", _ui->spinBox_usbcam_streamHeight->value());
settings.setValue("fourcc", _ui->lineEdit_usbcam_fourcc->text());
settings.endGroup(); // UsbCam

settings.beginGroup("Video");
Expand Down Expand Up @@ -6738,6 +6744,10 @@ Camera * PreferencesDialog::createCamera(
this->getSourceLocalTransform());
}
((CameraStereoVideo*)camera)->setResolution(_ui->spinBox_stereousbcam_streamWidth->value(), _ui->spinBox_stereousbcam_streamHeight->value());
if(!_ui->lineEdit_stereousbcam_fourcc->text().isEmpty())
{
((CameraStereoVideo*)camera)->setFOURCC(_ui->lineEdit_stereousbcam_fourcc->text().toStdString());
}
}
else if(driver == kSrcStereoVideo)
{
Expand Down Expand Up @@ -6866,6 +6876,10 @@ Camera * PreferencesDialog::createCamera(
this->getGeneralInputRate(),
this->getSourceLocalTransform());
((CameraVideo*)camera)->setResolution(_ui->spinBox_usbcam_streamWidth->value(), _ui->spinBox_usbcam_streamHeight->value());
if(!_ui->lineEdit_usbcam_fourcc->text().isEmpty())
{
((CameraVideo*)camera)->setFOURCC(_ui->lineEdit_usbcam_fourcc->text().toStdString());
}
}
else if(driver == kSrcVideo)
{
Expand Down
Loading

0 comments on commit 8b21c27

Please sign in to comment.