Skip to content

Commit

Permalink
OAK bug fixes and improvements (#1143)
Browse files Browse the repository at this point in the history 
* *fix distortion correction problem of small FOV devices
*remove deprecated IMU firmware update API
*add useSpecTranslation option

* reduce depth image noise and simplify calculation for compressed transport
  • Loading branch information
@borongyuan
borongyuan authored Oct 8, 2023
1 parent 2aa139b commit f06cc0b
Show file tree
Hide file tree
Showing 4 changed files with 37 additions and 33 deletions.
4 changes: 2 additions & 2 deletions corelib/include/rtabmap/core/camera/CameraDepthAI.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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_;
Expand Down
30 changes: 17 additions & 13 deletions corelib/src/camera/CameraDepthAI.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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_);
Expand All @@ -340,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);
}
Expand Down Expand Up @@ -374,8 +382,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)
Expand Down Expand Up @@ -431,7 +437,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
double fy = new_camera_matrix.at<double>(1, 1);
double cx = new_camera_matrix.at<double>(0, 2);
double cy = new_camera_matrix.at<double>(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_);

Expand Down Expand Up @@ -568,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>([&](uint16_t& pixel, const int * position) -> void {
pixel = stereoModel_.computeDepth(depthOrRight.at<uint8_t>(position))*1000;
});
depthOrRight = depth;
cv::Mat disp;
depthOrRight.convertTo(disp, CV_16UC1);
cv::divide(-stereoModel_.right().Tx() * 1000, disp, depthOrRight);
}
}
else
Expand Down
20 changes: 10 additions & 10 deletions guilib/src/PreferencesDialog.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()));
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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());
Expand Down
16 changes: 8 additions & 8 deletions guilib/src/ui/preferencesDialog.ui
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5882,7 +5882,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</item>
</widget>
</item>
<item row="3" column="0">
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_depthai_alpha_scaling">
<property name="minimum">
<double>-1.000000000000000</double>
Expand Down Expand Up @@ -5924,7 +5924,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="4" column="0">
<item row="5" column="0">
<widget class="QCheckBox" name="checkBox_depthai_imu_published">
<property name="text">
<string/>
Expand Down Expand Up @@ -5957,10 +5957,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="5" column="1">
<item row="3" column="1">
<widget class="QLabel" name="label_646">
<property name="text">
<string>IMU firmware update</string>
<string>Use the translation information from the board design data (not the calibration data).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
Expand Down Expand Up @@ -5990,7 +5990,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="3" column="1">
<item row="4" column="1">
<widget class="QLabel" name="label_678">
<property name="text">
<string>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 &lt; 0.0 helps removing invalid areas. If alpha is set to -1, old rectification policy is used.</string>
Expand All @@ -6016,7 +6016,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="4" column="1">
<item row="5" column="1">
<widget class="QLabel" name="label_650">
<property name="text">
<string>IMU published</string>
Expand All @@ -6029,8 +6029,8 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QCheckBox" name="checkBox_depthai_imu_firmware_update">
<item row="3" column="0">
<widget class="QCheckBox" name="checkBox_depthai_use_spec_translation">
<property name="text">
<string/>
</property>
Expand Down

0 comments on commit f06cc0b

Please sign in to comment.