From 93637f81f82f34186ee05d694535048fd6843378 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Sat, 7 Oct 2023 15:24:25 +0800 Subject: [PATCH 1/2] *fix distortion correction problem of small FOV devices *remove deprecated IMU firmware update API *add useSpecTranslation option --- .../rtabmap/core/camera/CameraDepthAI.h | 4 ++-- corelib/src/camera/CameraDepthAI.cpp | 17 ++++++++-------- guilib/src/PreferencesDialog.cpp | 20 +++++++++---------- guilib/src/ui/preferencesDialog.ui | 16 +++++++-------- 4 files changed, 29 insertions(+), 28 deletions(-) diff --git a/corelib/include/rtabmap/core/camera/CameraDepthAI.h b/corelib/include/rtabmap/core/camera/CameraDepthAI.h index 588f3a798e..d556e7bcb1 100644 --- a/corelib/include/rtabmap/core/camera/CameraDepthAI.h +++ b/corelib/include/rtabmap/core/camera/CameraDepthAI.h @@ -56,8 +56,8 @@ class RTABMAP_CORE_EXPORT CameraDepthAI : virtual ~CameraDepthAI(); void setOutputDepth(bool enabled, int confidence = 200); + void setUseSpecTranslation(bool useSpecTranslation); void setAlphaScaling(float alphaScaling = 0.0f); - void setIMUFirmwareUpdate(bool enabled); void setIMUPublished(bool published); void publishInterIMU(bool enabled); void setLaserDotBrightness(float dotProjectormA = 0.0f); @@ -83,8 +83,8 @@ class RTABMAP_CORE_EXPORT CameraDepthAI : bool outputDepth_; int depthConfidence_; int resolution_; + bool useSpecTranslation_; float alphaScaling_; - bool imuFirmwareUpdate_; bool imuPublished_; bool publishInterIMU_; float dotProjectormA_; diff --git a/corelib/src/camera/CameraDepthAI.cpp b/corelib/src/camera/CameraDepthAI.cpp index e4779c77e3..ef4bd48cd9 100644 --- a/corelib/src/camera/CameraDepthAI.cpp +++ b/corelib/src/camera/CameraDepthAI.cpp @@ -57,8 +57,8 @@ CameraDepthAI::CameraDepthAI( outputDepth_(false), depthConfidence_(200), resolution_(resolution), + useSpecTranslation_(false), alphaScaling_(0.0), - imuFirmwareUpdate_(false), imuPublished_(true), publishInterIMU_(false), dotProjectormA_(0.0), @@ -100,19 +100,19 @@ void CameraDepthAI::setOutputDepth(bool enabled, int confidence) #endif } -void CameraDepthAI::setAlphaScaling(float alphaScaling) +void CameraDepthAI::setUseSpecTranslation(bool useSpecTranslation) { #ifdef RTABMAP_DEPTHAI - alphaScaling_ = alphaScaling; + useSpecTranslation_ = useSpecTranslation; #else UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); #endif } -void CameraDepthAI::setIMUFirmwareUpdate(bool enabled) +void CameraDepthAI::setAlphaScaling(float alphaScaling) { #ifdef RTABMAP_DEPTHAI - imuFirmwareUpdate_ = enabled; + alphaScaling_ = alphaScaling; #else UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); #endif @@ -315,6 +315,9 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT); stereo->setExtendedDisparity(false); stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout + stereo->enableDistortionCorrection(true); + stereo->setDisparityToDepthUseSpecTranslation(useSpecTranslation_); + stereo->setDepthAlignmentUseSpecTranslation(useSpecTranslation_); if(alphaScaling_>-1.0f) stereo->setAlphaScaling(alphaScaling_); stereo->initialConfig.setConfidenceThreshold(depthConfidence_); @@ -374,8 +377,6 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin // Link plugins IMU -> XLINK imu->out.link(xoutIMU->input); - - imu->enableFirmwareUpdate(imuFirmwareUpdate_); } if(detectFeatures_ == 1) @@ -431,7 +432,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin double fy = new_camera_matrix.at(1, 1); double cx = new_camera_matrix.at(0, 2); double cy = new_camera_matrix.at(1, 2); - double baseline = calibHandler.getBaselineDistance()/100.0; + double baseline = calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_C, dai::CameraBoardSocket::CAM_B, useSpecTranslation_)/100.0; UINFO("left: fx=%f fy=%f cx=%f cy=%f baseline=%f", fx, fy, cx, cy, baseline); stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize_); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 8bf07eedd8..e6ce497e1d 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -805,9 +805,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->comboBox_depthai_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_depthai_depth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->spinBox_depthai_confidence, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkBox_depthai_use_spec_translation, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->doubleSpinBox_depthai_alpha_scaling, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_depthai_imu_published, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); - connect(_ui->checkBox_depthai_imu_firmware_update, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->doubleSpinBox_depthai_laser_dot_brightness, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->doubleSpinBox_depthai_floodlight_brightness, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->comboBox_depthai_detect_features, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); @@ -2111,9 +2111,9 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->comboBox_depthai_resolution->setCurrentIndex(1); _ui->checkBox_depthai_depth->setChecked(false); _ui->spinBox_depthai_confidence->setValue(200); + _ui->checkBox_depthai_use_spec_translation->setChecked(false); _ui->doubleSpinBox_depthai_alpha_scaling->setValue(0.0); _ui->checkBox_depthai_imu_published->setChecked(true); - _ui->checkBox_depthai_imu_firmware_update->setChecked(false); _ui->doubleSpinBox_depthai_laser_dot_brightness->setValue(0.0); _ui->doubleSpinBox_depthai_floodlight_brightness->setValue(200.0); _ui->comboBox_depthai_detect_features->setCurrentIndex(0); @@ -2601,9 +2601,9 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->comboBox_depthai_resolution->setCurrentIndex(settings.value("resolution", _ui->comboBox_depthai_resolution->currentIndex()).toInt()); _ui->checkBox_depthai_depth->setChecked(settings.value("depth", _ui->checkBox_depthai_depth->isChecked()).toBool()); _ui->spinBox_depthai_confidence->setValue(settings.value("confidence", _ui->spinBox_depthai_confidence->value()).toInt()); + _ui->checkBox_depthai_use_spec_translation->setChecked(settings.value("use_spec_translation", _ui->checkBox_depthai_use_spec_translation->isChecked()).toBool()); _ui->doubleSpinBox_depthai_alpha_scaling->setValue(settings.value("alpha_scaling", _ui->doubleSpinBox_depthai_alpha_scaling->value()).toDouble()); _ui->checkBox_depthai_imu_published->setChecked(settings.value("imu_published", _ui->checkBox_depthai_imu_published->isChecked()).toBool()); - _ui->checkBox_depthai_imu_firmware_update->setChecked(settings.value("imu_firmware_update", _ui->checkBox_depthai_imu_firmware_update->isChecked()).toBool()); _ui->doubleSpinBox_depthai_laser_dot_brightness->setValue(settings.value("laser_dot_brightness", _ui->doubleSpinBox_depthai_laser_dot_brightness->value()).toDouble()); _ui->doubleSpinBox_depthai_floodlight_brightness->setValue(settings.value("floodlight_brightness", _ui->doubleSpinBox_depthai_floodlight_brightness->value()).toDouble()); _ui->comboBox_depthai_detect_features->setCurrentIndex(settings.value("detect_features", _ui->comboBox_depthai_detect_features->currentIndex()).toInt()); @@ -3131,12 +3131,12 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.endGroup(); // MyntEye settings.beginGroup("DepthAI"); - settings.setValue("resolution", _ui->comboBox_depthai_resolution->currentIndex()); - settings.setValue("depth", _ui->checkBox_depthai_depth->isChecked()); - settings.setValue("confidence", _ui->spinBox_depthai_confidence->value()); - settings.setValue("alpha_scaling", _ui->doubleSpinBox_depthai_alpha_scaling->value()); - settings.setValue("imu_published", _ui->checkBox_depthai_imu_published->isChecked()); - settings.setValue("imu_firmware_update", _ui->checkBox_depthai_imu_firmware_update->isChecked()); + settings.setValue("resolution", _ui->comboBox_depthai_resolution->currentIndex()); + settings.setValue("depth", _ui->checkBox_depthai_depth->isChecked()); + settings.setValue("confidence", _ui->spinBox_depthai_confidence->value()); + settings.setValue("use_spec_translation", _ui->checkBox_depthai_use_spec_translation->isChecked()); + settings.setValue("alpha_scaling", _ui->doubleSpinBox_depthai_alpha_scaling->value()); + settings.setValue("imu_published", _ui->checkBox_depthai_imu_published->isChecked()); settings.setValue("laser_dot_brightness", _ui->doubleSpinBox_depthai_laser_dot_brightness->value()); settings.setValue("floodlight_brightness", _ui->doubleSpinBox_depthai_floodlight_brightness->value()); settings.setValue("detect_features", _ui->comboBox_depthai_detect_features->currentIndex()); @@ -6404,8 +6404,8 @@ Camera * PreferencesDialog::createCamera( this->getGeneralInputRate(), this->getSourceLocalTransform()); ((CameraDepthAI*)camera)->setOutputDepth(_ui->checkBox_depthai_depth->isChecked(), _ui->spinBox_depthai_confidence->value()); + ((CameraDepthAI*)camera)->setUseSpecTranslation(_ui->checkBox_depthai_use_spec_translation->isChecked()); ((CameraDepthAI*)camera)->setAlphaScaling(_ui->doubleSpinBox_depthai_alpha_scaling->value()); - ((CameraDepthAI*)camera)->setIMUFirmwareUpdate(_ui->checkBox_depthai_imu_firmware_update->isChecked()); ((CameraDepthAI*)camera)->setIMUPublished(_ui->checkBox_depthai_imu_published->isChecked()); ((CameraDepthAI*)camera)->publishInterIMU(_ui->checkbox_publishInterIMU->isChecked()); ((CameraDepthAI*)camera)->setLaserDotBrightness(_ui->doubleSpinBox_depthai_laser_dot_brightness->value()); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index b5a1243f61..624b8b31d7 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -5882,7 +5882,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + -1.000000000000000 @@ -5924,7 +5924,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + @@ -5957,10 +5957,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + - IMU firmware update + Use the translation information from the board design data (not the calibration data). true @@ -5990,7 +5990,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + Free scaling parameter between 0 (when all the pixels in the undistorted image are valid) and 1 (when all the source image pixels are retained in the undistorted image). On some high distortion lenses, and/or due to rectification (image rotated) invalid areas may appear even with alpha=0, in these cases alpha < 0.0 helps removing invalid areas. If alpha is set to -1, old rectification policy is used. @@ -6016,7 +6016,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + IMU published @@ -6029,8 +6029,8 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + From 46042ceac46a28afd60d94612742e1792105cec6 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Sat, 7 Oct 2023 18:07:57 +0800 Subject: [PATCH 2/2] reduce depth image noise and simplify calculation for compressed transport --- corelib/src/camera/CameraDepthAI.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/corelib/src/camera/CameraDepthAI.cpp b/corelib/src/camera/CameraDepthAI.cpp index ef4bd48cd9..114a19b1a6 100644 --- a/corelib/src/camera/CameraDepthAI.cpp +++ b/corelib/src/camera/CameraDepthAI.cpp @@ -343,9 +343,14 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin depthOrRightEnc->setDefaultProfilePreset(monoRight->getFps(), dai::VideoEncoderProperties::Profile::MJPEG); stereo->rectifiedLeft.link(leftEnc->input); if(outputDepth_) + { + depthOrRightEnc->setQuality(100); stereo->disparity.link(depthOrRightEnc->input); + } else + { stereo->rectifiedRight.link(depthOrRightEnc->input); + } leftEnc->bitstream.link(xoutLeft->input); depthOrRightEnc->bitstream.link(xoutDepthOrRight->input); } @@ -569,11 +574,9 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info) depthOrRight = cv::imdecode(rectifRightOrDepth->getData(), cv::IMREAD_GRAYSCALE); if(outputDepth_) { - cv::Mat depth(targetSize_, CV_16UC1); - depth.forEach([&](uint16_t& pixel, const int * position) -> void { - pixel = stereoModel_.computeDepth(depthOrRight.at(position))*1000; - }); - depthOrRight = depth; + cv::Mat disp; + depthOrRight.convertTo(disp, CV_16UC1); + cv::divide(-stereoModel_.right().Tx() * 1000, disp, depthOrRight); } } else