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

OAK bug fixes and improvements (#1143)

Browse Source 
* *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
This commit is contained in:
Borong Yuan
2023-10-08 11:40:30 +08:00
committed by GitHub
parent 2aa139b579
commit f06cc0b17f
4 changed files with 37 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/include/rtabmap/core/camera/CameraDepthAI.h
View File

@@ -56,8 +56,8 @@ public:
virtual ~CameraDepthAI(); virtual ~CameraDepthAI();
void setOutputDepth(bool enabled, int confidence = 200); void setOutputDepth(bool enabled, int confidence = 200);
void setUseSpecTranslation(bool useSpecTranslation);
void setAlphaScaling(float alphaScaling = 0.0f); void setAlphaScaling(float alphaScaling = 0.0f);
void setIMUFirmwareUpdate(bool enabled);
void setIMUPublished(bool published); void setIMUPublished(bool published);
void publishInterIMU(bool enabled); void publishInterIMU(bool enabled);
void setLaserDotBrightness(float dotProjectormA = 0.0f); void setLaserDotBrightness(float dotProjectormA = 0.0f);
@@ -83,8 +83,8 @@ private:
bool outputDepth_; bool outputDepth_;
int depthConfidence_; int depthConfidence_;
int resolution_; int resolution_;
bool useSpecTranslation_;
float alphaScaling_; float alphaScaling_;
bool imuFirmwareUpdate_;
bool imuPublished_; bool imuPublished_;
bool publishInterIMU_; bool publishInterIMU_;
float dotProjectormA_; float dotProjectormA_;

30
corelib/src/camera/CameraDepthAI.cpp
View File

@@ -57,8 +57,8 @@ CameraDepthAI::CameraDepthAI(
outputDepth_(false), outputDepth_(false),
depthConfidence_(200), depthConfidence_(200),
resolution_(resolution), resolution_(resolution),
useSpecTranslation_(false),
alphaScaling_(0.0), alphaScaling_(0.0),
imuFirmwareUpdate_(false),
imuPublished_(true), imuPublished_(true),
publishInterIMU_(false), publishInterIMU_(false),
dotProjectormA_(0.0), dotProjectormA_(0.0),
@@ -100,19 +100,19 @@ void CameraDepthAI::setOutputDepth(bool enabled, int confidence)
#endif #endif
} }
void CameraDepthAI::setAlphaScaling(float alphaScaling) void CameraDepthAI::setUseSpecTranslation(bool useSpecTranslation)
{ {
#ifdef RTABMAP_DEPTHAI #ifdef RTABMAP_DEPTHAI
alphaScaling_ = alphaScaling; useSpecTranslation_ = useSpecTranslation;
#else #else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif #endif
} }
void CameraDepthAI::setIMUFirmwareUpdate(bool enabled) void CameraDepthAI::setAlphaScaling(float alphaScaling)
{ {
#ifdef RTABMAP_DEPTHAI #ifdef RTABMAP_DEPTHAI
imuFirmwareUpdate_ = enabled; alphaScaling_ = alphaScaling;
#else #else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif #endif
@@ -315,6 +315,9 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT); stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT);
stereo->setExtendedDisparity(false); stereo->setExtendedDisparity(false);
stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
stereo->enableDistortionCorrection(true);
stereo->setDisparityToDepthUseSpecTranslation(useSpecTranslation_);
stereo->setDepthAlignmentUseSpecTranslation(useSpecTranslation_);
if(alphaScaling_>-1.0f) if(alphaScaling_>-1.0f)
stereo->setAlphaScaling(alphaScaling_); stereo->setAlphaScaling(alphaScaling_);
stereo->initialConfig.setConfidenceThreshold(depthConfidence_); stereo->initialConfig.setConfidenceThreshold(depthConfidence_);
@@ -340,9 +343,14 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
depthOrRightEnc->setDefaultProfilePreset(monoRight->getFps(), dai::VideoEncoderProperties::Profile::MJPEG); depthOrRightEnc->setDefaultProfilePreset(monoRight->getFps(), dai::VideoEncoderProperties::Profile::MJPEG);
stereo->rectifiedLeft.link(leftEnc->input); stereo->rectifiedLeft.link(leftEnc->input);
if(outputDepth_) if(outputDepth_)
{
depthOrRightEnc->setQuality(100);
stereo->disparity.link(depthOrRightEnc->input); stereo->disparity.link(depthOrRightEnc->input);
}
else else
{
stereo->rectifiedRight.link(depthOrRightEnc->input); stereo->rectifiedRight.link(depthOrRightEnc->input);
}
leftEnc->bitstream.link(xoutLeft->input); leftEnc->bitstream.link(xoutLeft->input);
depthOrRightEnc->bitstream.link(xoutDepthOrRight->input); depthOrRightEnc->bitstream.link(xoutDepthOrRight->input);
} }
@@ -374,8 +382,6 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
// Link plugins IMU -> XLINK // Link plugins IMU -> XLINK
imu->out.link(xoutIMU->input); imu->out.link(xoutIMU->input);
imu->enableFirmwareUpdate(imuFirmwareUpdate_);
} }
if(detectFeatures_ == 1) if(detectFeatures_ == 1)
@@ -431,7 +437,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
double fy = new_camera_matrix.at<double>(1, 1); double fy = new_camera_matrix.at<double>(1, 1);
double cx = new_camera_matrix.at<double>(0, 2); double cx = new_camera_matrix.at<double>(0, 2);
double cy = new_camera_matrix.at<double>(1, 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); 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_); stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize_);
@@ -568,11 +574,9 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
depthOrRight = cv::imdecode(rectifRightOrDepth->getData(), cv::IMREAD_GRAYSCALE); depthOrRight = cv::imdecode(rectifRightOrDepth->getData(), cv::IMREAD_GRAYSCALE);
if(outputDepth_) if(outputDepth_)
{ {
cv::Mat depth(targetSize_, CV_16UC1); cv::Mat disp;
depth.forEach<uint16_t>([&](uint16_t& pixel, const int * position) -> void { depthOrRight.convertTo(disp, CV_16UC1);
pixel = stereoModel_.computeDepth(depthOrRight.at<uint8_t>(position))*1000; cv::divide(-stereoModel_.right().Tx() * 1000, disp, depthOrRight);
});
depthOrRight = depth;
} }
} }
else else

20
guilib/src/PreferencesDialog.cpp
View File

@@ -805,9 +805,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->comboBox_depthai_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->comboBox_depthai_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkBox_depthai_depth, SIGNAL(stateChanged(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->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->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_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_laser_dot_brightness, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->doubleSpinBox_depthai_floodlight_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())); 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->comboBox_depthai_resolution->setCurrentIndex(1);
_ui->checkBox_depthai_depth->setChecked(false); _ui->checkBox_depthai_depth->setChecked(false);
_ui->spinBox_depthai_confidence->setValue(200); _ui->spinBox_depthai_confidence->setValue(200);
_ui->checkBox_depthai_use_spec_translation->setChecked(false);
_ui->doubleSpinBox_depthai_alpha_scaling->setValue(0.0); _ui->doubleSpinBox_depthai_alpha_scaling->setValue(0.0);
_ui->checkBox_depthai_imu_published->setChecked(true); _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_laser_dot_brightness->setValue(0.0);
_ui->doubleSpinBox_depthai_floodlight_brightness->setValue(200.0); _ui->doubleSpinBox_depthai_floodlight_brightness->setValue(200.0);
_ui->comboBox_depthai_detect_features->setCurrentIndex(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->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->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->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->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_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_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->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()); _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.endGroup(); // MyntEye
settings.beginGroup("DepthAI"); settings.beginGroup("DepthAI");
settings.setValue("resolution", _ui->comboBox_depthai_resolution->currentIndex()); settings.setValue("resolution", _ui->comboBox_depthai_resolution->currentIndex());
settings.setValue("depth", _ui->checkBox_depthai_depth->isChecked()); settings.setValue("depth", _ui->checkBox_depthai_depth->isChecked());
settings.setValue("confidence", _ui->spinBox_depthai_confidence->value()); settings.setValue("confidence", _ui->spinBox_depthai_confidence->value());
settings.setValue("alpha_scaling", _ui->doubleSpinBox_depthai_alpha_scaling->value()); settings.setValue("use_spec_translation", _ui->checkBox_depthai_use_spec_translation->isChecked());
settings.setValue("imu_published", _ui->checkBox_depthai_imu_published->isChecked()); settings.setValue("alpha_scaling", _ui->doubleSpinBox_depthai_alpha_scaling->value());
settings.setValue("imu_firmware_update", _ui->checkBox_depthai_imu_firmware_update->isChecked()); settings.setValue("imu_published", _ui->checkBox_depthai_imu_published->isChecked());
settings.setValue("laser_dot_brightness", _ui->doubleSpinBox_depthai_laser_dot_brightness->value()); settings.setValue("laser_dot_brightness", _ui->doubleSpinBox_depthai_laser_dot_brightness->value());
settings.setValue("floodlight_brightness", _ui->doubleSpinBox_depthai_floodlight_brightness->value()); settings.setValue("floodlight_brightness", _ui->doubleSpinBox_depthai_floodlight_brightness->value());
settings.setValue("detect_features", _ui->comboBox_depthai_detect_features->currentIndex()); settings.setValue("detect_features", _ui->comboBox_depthai_detect_features->currentIndex());
@@ -6404,8 +6404,8 @@ Camera * PreferencesDialog::createCamera(
this->getGeneralInputRate(), this->getGeneralInputRate(),
this->getSourceLocalTransform()); this->getSourceLocalTransform());
((CameraDepthAI*)camera)->setOutputDepth(_ui->checkBox_depthai_depth->isChecked(), _ui->spinBox_depthai_confidence->value()); ((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)->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)->setIMUPublished(_ui->checkBox_depthai_imu_published->isChecked());
((CameraDepthAI*)camera)->publishInterIMU(_ui->checkbox_publishInterIMU->isChecked()); ((CameraDepthAI*)camera)->publishInterIMU(_ui->checkbox_publishInterIMU->isChecked());
((CameraDepthAI*)camera)->setLaserDotBrightness(_ui->doubleSpinBox_depthai_laser_dot_brightness->value()); ((CameraDepthAI*)camera)->setLaserDotBrightness(_ui->doubleSpinBox_depthai_laser_dot_brightness->value());

16
guilib/src/ui/preferencesDialog.ui
View File

@@ -5882,7 +5882,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</item> </item>
</widget> </widget>
</item> </item>
<item row="3" column="0"> <item row="4" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_depthai_alpha_scaling"> <widget class="QDoubleSpinBox" name="doubleSpinBox_depthai_alpha_scaling">
<property name="minimum"> <property name="minimum">
<double>-1.000000000000000</double> <double>-1.000000000000000</double>
@@ -5924,7 +5924,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property> </property>
</widget> </widget>
</item> </item>
<item row="4" column="0"> <item row="5" column="0">
<widget class="QCheckBox" name="checkBox_depthai_imu_published"> <widget class="QCheckBox" name="checkBox_depthai_imu_published">
<property name="text"> <property name="text">
<string/> <string/>
@@ -5957,10 +5957,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property> </property>
</widget> </widget>
</item> </item>
<item row="5" column="1"> <item row="3" column="1">
<widget class="QLabel" name="label_646"> <widget class="QLabel" name="label_646">
<property name="text"> <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>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>
@@ -5990,7 +5990,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property> </property>
</widget> </widget>
</item> </item>
<item row="3" column="1"> <item row="4" column="1">
<widget class="QLabel" name="label_678"> <widget class="QLabel" name="label_678">
<property name="text"> <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> <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>
@@ -6016,7 +6016,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property> </property>
</widget> </widget>
</item> </item>
<item row="4" column="1"> <item row="5" column="1">
<widget class="QLabel" name="label_650"> <widget class="QLabel" name="label_650">
<property name="text"> <property name="text">
<string>IMU published</string> <string>IMU published</string>
@@ -6029,8 +6029,8 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property> </property>
</widget> </widget>
</item> </item>
<item row="5" column="0"> <item row="3" column="0">
<widget class="QCheckBox" name="checkBox_depthai_imu_firmware_update"> <widget class="QCheckBox" name="checkBox_depthai_use_spec_translation">
<property name="text"> <property name="text">
<string/> <string/>
</property> </property>