From 71f7515775025ad0936ad2660488e74571a4422c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 15 May 2019 19:21:32 -0400 Subject: [PATCH] Added resolution parameters for Usb camera and realsense2 sources --- .../rtabmap/core/camera/CameraRealSense2.h | 7 +- .../include/rtabmap/core/camera/CameraVideo.h | 9 ++ corelib/src/camera/CameraRealSense2.cpp | 36 ++++- corelib/src/camera/CameraVideo.cpp | 21 ++- guilib/src/PreferencesDialog.cpp | 35 ++++- guilib/src/ui/preferencesDialog.ui | 129 +++++++++++++++++- 6 files changed, 219 insertions(+), 18 deletions(-) diff --git a/corelib/include/rtabmap/core/camera/CameraRealSense2.h b/corelib/include/rtabmap/core/camera/CameraRealSense2.h index b6b85275..ed7b333f 100644 --- a/corelib/include/rtabmap/core/camera/CameraRealSense2.h +++ b/corelib/include/rtabmap/core/camera/CameraRealSense2.h @@ -59,7 +59,6 @@ public: static bool available(); public: - // default local transform z in, x right, y down)); CameraRealSense2( const std::string & deviceId = "", float imageRate = 0, @@ -72,8 +71,11 @@ public: bool odomProvided() const; // parameters are set during initialization + // D400 series void setEmitterEnabled(bool enabled); void setIRDepthFormat(bool enabled); + void setResolution(int width, int height, int fps = 30); + // T265 related parameters void setImagesRectified(bool enabled); void setOdomProvided(bool enabled); @@ -117,6 +119,9 @@ private: bool irDepth_; bool rectifyImages_; bool odometryProvided_; + int cameraWidth_; + int cameraHeight_; + int cameraFps_; static Transform realsense2PoseRotation_; static Transform realsense2PoseRotationInv_; diff --git a/corelib/include/rtabmap/core/camera/CameraVideo.h b/corelib/include/rtabmap/core/camera/CameraVideo.h index b5334ee0..675cc082 100644 --- a/corelib/include/rtabmap/core/camera/CameraVideo.h +++ b/corelib/include/rtabmap/core/camera/CameraVideo.h @@ -58,6 +58,13 @@ public: int getUsbDevice() const {return _usbDevice;} const std::string & getFilePath() const {return _filePath;} + /** + * Set wanted usb resolution, should be set before initialization. 0 means + * default resolution. It won't be applied if a valid camera calibration + * has been loaded, thus resolution from calibration is used. + * */ + void setResolution(int width, int height) {_width=width, _height=height;} + protected: virtual SensorData captureImage(CameraInfo * info = 0); @@ -72,6 +79,8 @@ private: // Usb camera int _usbDevice; std::string _guid; + int _width; + int _height; CameraModel _model; }; diff --git a/corelib/src/camera/CameraRealSense2.cpp b/corelib/src/camera/CameraRealSense2.cpp index dacf3d6e..94078dc3 100644 --- a/corelib/src/camera/CameraRealSense2.cpp +++ b/corelib/src/camera/CameraRealSense2.cpp @@ -67,7 +67,10 @@ CameraRealSense2::CameraRealSense2( emitterEnabled_(true), irDepth_(false), rectifyImages_(true), - odometryProvided_(false) + odometryProvided_(false), + cameraWidth_(640), + cameraHeight_(480), + cameraFps_(30) #endif { UDEBUG(""); @@ -564,21 +567,21 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st { //D400 series: if (video_profile.format() == (i==1?RS2_FORMAT_Z16:irDepth_?RS2_FORMAT_Y8:RS2_FORMAT_RGB8) && - video_profile.width() == 640 && - video_profile.height() == 480 && - video_profile.fps() == 30) + video_profile.width() == cameraWidth_ && + video_profile.height() == cameraHeight_ && + video_profile.fps() == cameraFps_) { profilesPerSensor[irDepth_?1:i].push_back(profile); auto intrinsic = video_profile.get_intrinsics(); if(i==1) { - depthBuffer_ = cv::Mat(cv::Size(640, 480), CV_16UC1, cv::Scalar(0)); + depthBuffer_ = cv::Mat(cv::Size(cameraWidth_, cameraHeight_), CV_16UC1, cv::Scalar(0)); depthStreamProfile = profile; *depthIntrinsics_ = intrinsic; } else { - rgbBuffer_ = cv::Mat(cv::Size(640, 480), irDepth_?CV_8UC1:CV_8UC3, irDepth_?cv::Scalar(0):cv::Scalar(0, 0, 0)); + rgbBuffer_ = cv::Mat(cv::Size(cameraWidth_, cameraHeight_), irDepth_?CV_8UC1:CV_8UC3, irDepth_?cv::Scalar(0):cv::Scalar(0, 0, 0)); model_ = CameraModel(camera_name, intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy, this->getLocalTransform(), 0, cv::Size(intrinsic.width, intrinsic.height)); rgbStreamProfile = profile; *rgbIntrinsics_ = intrinsic; @@ -638,7 +641,17 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st if (!added) { UERROR("Given stream configuration is not supported by the device! " - "Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, 640, 480, 30); + "Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, cameraWidth_, cameraHeight_, cameraFps_); + UERROR("Available configurations:"); + for (auto& profile : profiles) + { + auto video_profile = profile.as(); + UERROR("%s %d %d %d", rs2_format_to_string( + video_profile.format()), + video_profile.width(), + video_profile.height(), + video_profile.fps()); + } return false; } } @@ -819,6 +832,15 @@ void CameraRealSense2::setIRDepthFormat(bool enabled) #endif } +void CameraRealSense2::setResolution(int width, int height, int fps) +{ +#ifdef RTABMAP_REALSENSE2 + cameraWidth_ = width; + cameraHeight_ = height; + cameraFps_ = fps; +#endif +} + void CameraRealSense2::setImagesRectified(bool enabled) { #ifdef RTABMAP_REALSENSE2 diff --git a/corelib/src/camera/CameraVideo.cpp b/corelib/src/camera/CameraVideo.cpp index 5da7ea09..adf022f4 100644 --- a/corelib/src/camera/CameraVideo.cpp +++ b/corelib/src/camera/CameraVideo.cpp @@ -43,7 +43,9 @@ CameraVideo::CameraVideo( Camera(imageRate, localTransform), _rectifyImages(rectifyImages), _src(kUsbDevice), - _usbDevice(usbDevice) + _usbDevice(usbDevice), + _width(0), + _height(0) { } @@ -57,7 +59,9 @@ CameraVideo::CameraVideo( _filePath(filePath), _rectifyImages(rectifyImages), _src(kVideoFile), - _usbDevice(0) + _usbDevice(0), + _width(0), + _height(0) { } @@ -123,6 +127,19 @@ bool CameraVideo::init(const std::string & calibrationFolder, const std::string } } _model.setLocalTransform(this->getLocalTransform()); + if(_src == kUsbDevice) + { + if(_model.isValidForProjection()) + { + _capture.set(cv::CAP_PROP_FRAME_WIDTH, _model.imageWidth()); + _capture.set(cv::CAP_PROP_FRAME_HEIGHT, _model.imageHeight()); + } + else if(_width > 0 && _height > 0) + { + _capture.set(cv::CAP_PROP_FRAME_WIDTH, _width); + _capture.set(cv::CAP_PROP_FRAME_HEIGHT, _height); + } + } if(_rectifyImages && !_model.isValidForRectification()) { UERROR("Parameter \"rectifyImages\" is set, but no camera model is loaded or valid."); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 69bc394d..5ce697e2 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -568,6 +568,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->source_images_spinBox_startPos, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->source_images_spinBox_maxFrames, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->comboBox_cameraImages_bayerMode, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + // usb group + connect(_ui->spinBox_usbcam_streamWidth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_usbcam_streamHeight, SIGNAL(valueChanged(int)), 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())); @@ -614,6 +617,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->comboBox_realsenseRGBSource, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkbox_rs2_emitter, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkbox_rs2_irDepth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_rs2_width, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_rs2_height, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_rs2_rate, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->toolButton_cameraImages_timestamps, SIGNAL(clicked()), this, SLOT(selectSourceImagesStamps())); connect(_ui->lineEdit_cameraImages_timestamps, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); @@ -1696,6 +1702,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->source_comboBox_image_type->setCurrentIndex(kSrcUsbDevice-kSrcUsbDevice); _ui->source_images_spinBox_startPos->setValue(0); _ui->source_images_spinBox_maxFrames->setValue(0); + _ui->spinBox_usbcam_streamWidth->setValue(0); + _ui->spinBox_usbcam_streamHeight->setValue(0); _ui->checkBox_rgb_rectify->setChecked(false); _ui->comboBox_cameraImages_bayerMode->setCurrentIndex(0); @@ -1763,6 +1771,9 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->comboBox_realsenseRGBSource->setCurrentIndex(0); _ui->checkbox_rs2_emitter->setChecked(true); _ui->checkbox_rs2_irDepth->setChecked(false); + _ui->spinBox_rs2_width->setValue(640); + _ui->spinBox_rs2_height->setValue(480); + _ui->spinBox_rs2_rate->setValue(30); _ui->lineEdit_openniOniPath->clear(); _ui->lineEdit_openni2OniPath->clear(); _ui->lineEdit_cameraRGBDImages_path_rgb->setText(""); @@ -2170,6 +2181,9 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) settings.beginGroup("RealSense2"); _ui->checkbox_rs2_emitter->setChecked(settings.value("emitter", _ui->checkbox_rs2_emitter->isChecked()).toBool()); _ui->checkbox_rs2_irDepth->setChecked(settings.value("irdepth", _ui->checkbox_rs2_irDepth->isChecked()).toBool()); + _ui->spinBox_rs2_width->setValue(settings.value("width", _ui->spinBox_rs2_width->value()).toInt()); + _ui->spinBox_rs2_height->setValue(settings.value("height", _ui->spinBox_rs2_height->value()).toInt()); + _ui->spinBox_rs2_rate->setValue(settings.value("rate", _ui->spinBox_rs2_rate->value()).toInt()); settings.endGroup(); // RealSense settings.beginGroup("RGBDImages"); @@ -2231,6 +2245,11 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->spinBox_cameraImages_max_imu_rate->setValue(settings.value("imu_rate", _ui->spinBox_cameraImages_max_imu_rate->value()).toInt()); settings.endGroup(); // images + 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()); + settings.endGroup(); // UsbCam + settings.beginGroup("Video"); _ui->source_video_lineEdit_path->setText(settings.value("path", _ui->source_video_lineEdit_path->text()).toString()); settings.endGroup(); // video @@ -2602,6 +2621,9 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.beginGroup("RealSense2"); settings.setValue("emitter", _ui->checkbox_rs2_emitter->isChecked()); settings.setValue("irdepth", _ui->checkbox_rs2_irDepth->isChecked()); + settings.setValue("width", _ui->spinBox_rs2_width->value()); + settings.setValue("height", _ui->spinBox_rs2_height->value()); + settings.setValue("rate", _ui->spinBox_rs2_rate->value()); settings.endGroup(); // RealSense2 settings.beginGroup("RGBDImages"); @@ -2661,6 +2683,11 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("imu_rate", _ui->spinBox_cameraImages_max_imu_rate->value()); settings.endGroup(); // images + settings.beginGroup("UsbCam"); + settings.setValue("width", _ui->spinBox_usbcam_streamWidth->value()); + settings.setValue("height", _ui->spinBox_usbcam_streamHeight->value()); + settings.endGroup(); // UsbCam + settings.beginGroup("Video"); settings.setValue("path", _ui->source_video_lineEdit_path->text()); settings.endGroup(); // video @@ -4523,9 +4550,13 @@ void PreferencesDialog::updateSourceGrpVisibility() _ui->groupBox_cameraStereoZed->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZed - kSrcStereo); _ui->groupBox_cameraStereoRealSense2->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoRealSense2 - kSrcStereo); - _ui->stackedWidget_image->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && (_ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB || _ui->source_comboBox_image_type->currentIndex() == kSrcVideo-kSrcRGB)); + _ui->stackedWidget_image->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && + (_ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB || + _ui->source_comboBox_image_type->currentIndex() == kSrcVideo-kSrcRGB || + _ui->source_comboBox_image_type->currentIndex() == kSrcUsbDevice-kSrcRGB)); _ui->source_groupBox_images->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB); _ui->source_groupBox_video->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcVideo-kSrcRGB); + _ui->source_groupBox_usbcam->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcUsbDevice-kSrcRGB); _ui->groupBox_sourceImages_optional->setVisible( (_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD) || @@ -5270,6 +5301,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor) { ((CameraRealSense2*)camera)->setEmitterEnabled(_ui->checkbox_rs2_emitter->isChecked()); ((CameraRealSense2*)camera)->setIRDepthFormat(_ui->checkbox_rs2_irDepth->isChecked()); + ((CameraRealSense2*)camera)->setResolution(_ui->spinBox_rs2_width->value(), _ui->spinBox_rs2_height->value(), _ui->spinBox_rs2_rate->value()); } } } @@ -5434,6 +5466,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor) _ui->checkBox_rgb_rectify->isChecked() && !useRawImages, this->getGeneralInputRate(), this->getSourceLocalTransform()); + ((CameraVideo*)camera)->setResolution(_ui->spinBox_usbcam_streamWidth->value(), _ui->spinBox_usbcam_streamHeight->value()); } else if(driver == kSrcVideo) { diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index c18a2f40..7bcdb2c2 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -1977 + 0 680 3082 @@ -2962,7 +2962,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 1 + 0 @@ -3091,7 +3091,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 6 + 9 @@ -4057,6 +4057,36 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki RealSense2 + + + + Stream width. + + + true + + + + + + + pix + + + 9999 + + + + + + + Hz + + + 999 + + + @@ -4098,6 +4128,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + pix + + + 9999 + + + + + + + Stream height. + + + true + + + + Qt::Vertical @@ -4110,6 +4160,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + Stream rate. + + + true + + + @@ -4289,7 +4349,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 5 + 7 @@ -4970,10 +5030,65 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 1 + 0 + + + + USB Camera + + + + + + 0 + + + 10000 + + + + + + + Stream height (0 for default resolution). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Stream width (0 for default resolution). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 10000 + + + + + + @@ -4981,7 +5096,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 0 + 20 0 @@ -7929,7 +8044,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - T_similarity : Similarity threshold. Values must be >=0.0 ans <=1.0. + T_similarity : Similarity threshold. Values must be >=0.0 and <=1.0. true