UI: Exposed more depthai options, setting default rectification alpha…

… to -1 to avoid OAK-D Pro crash

corelib/include/rtabmap/core/camera/CameraDepthAI.h

@@ -57,6 +57,9 @@ class RTABMAP_CORE_EXPORT CameraDepthAI :
 
  void setOutputMode(int outputMode = 0);
  void setDepthProfile(int confThreshold = 200, int lrcThreshold = 5);
+ void setExtendedDisparity(bool extendedDisparity);
+ void setSubpixelMode(bool enabled, int fractionalBits = 3);
+ void setCompanding(bool enabled, int width=96);
  void setRectification(bool useSpecTranslation, float alphaScaling = 0.0f);
  void setIMU(bool imuPublished, bool publishInterIMU);
  void setIrIntensity(float dotIntensity = 0.0f, float floodIntensity = 0.0f);
@@ -82,6 +85,9 @@ class RTABMAP_CORE_EXPORT CameraDepthAI :
  int confThreshold_;
  int lrcThreshold_;
  int resolution_;
+ bool extendedDisparity_;
+ int subpixelFractionalBits_;
+ int compandingWidth_;
  bool useSpecTranslation_;
  float alphaScaling_;
  bool imuPublished_;

corelib/src/camera/CameraDepthAI.cpp

@@ -58,6 +58,9 @@ CameraDepthAI::CameraDepthAI(
  confThreshold_(200),
  lrcThreshold_(5),
  resolution_(resolution),
+ extendedDisparity_(false),
+ subpixelFractionalBits_(0),
+ compandingWidth_(0),
  useSpecTranslation_(false),
  alphaScaling_(0.0),
  imuPublished_(true),
@@ -107,6 +110,58 @@ void CameraDepthAI::setDepthProfile(int confThreshold, int lrcThreshold)
 #endif
 }
 
+void CameraDepthAI::setExtendedDisparity(bool extendedDisparity)
+{
+#ifdef RTABMAP_DEPTHAI
+ extendedDisparity_ = extendedDisparity;
+ if(extendedDisparity_)
+ {
+ if(subpixelFractionalBits_>0)
+ {
+ UWARN("Extended disparity has been enabled while subpixel being also enabled, disabling subpixel...");
+ subpixelFractionalBits_ = 0;
+ }
+ if(compandingWidth_>0)
+ {
+ UWARN("Extended disparity has been enabled while companding being also enabled, disabling companding...");
+ compandingWidth_ = 0;
+ }
+ }
+#else
+ UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
+#endif
+}
+
+void CameraDepthAI::setSubpixelMode(bool enabled, int fractionalBits)
+{
+#ifdef RTABMAP_DEPTHAI
+ UASSERT(fractionalBits>=3 && fractionalBits<=5);
+ subpixelFractionalBits_ = enabled?fractionalBits:0;
+ if(subpixelFractionalBits_ != 0 && extendedDisparity_)
+ {
+ UWARN("Subpixel has been enabled while extended disparity being also enabled, disabling extended disparity...");
+ extendedDisparity_ = false;
+ }
+#else
+ UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
+#endif
+}
+
+void CameraDepthAI::setCompanding(bool enabled, int width)
+{
+#ifdef RTABMAP_DEPTHAI
+ UASSERT(width == 64 || width == 96);
+ compandingWidth_ = enabled?width:0;
+ if(compandingWidth_ != 0 && extendedDisparity_)
+ {
+ UWARN("Companding has been enabled while extended disparity being also enabled, disabling extended disparity...");
+ extendedDisparity_ = false;
+ }
+#else
+ UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
+#endif
+}
+
 void CameraDepthAI::setRectification(bool useSpecTranslation, float alphaScaling)
 {
 #ifdef RTABMAP_DEPTHAI
@@ -289,16 +344,17 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
  stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A);
  else
  stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT);
- stereo->setExtendedDisparity(false);
+ stereo->setExtendedDisparity(extendedDisparity_);
  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(confThreshold_);
- stereo->initialConfig.setLeftRightCheck(true);
- stereo->initialConfig.setLeftRightCheckThreshold(lrcThreshold_);
+ stereo->initialConfig.setLeftRightCheck(lrcThreshold_>=0);
+ if(lrcThreshold_>=0)
+ stereo->initialConfig.setLeftRightCheckThreshold(lrcThreshold_);
  stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_7x7);
  auto config = stereo->initialConfig.get();
  config.censusTransform.kernelSize = dai::StereoDepthConfig::CensusTransform::KernelSize::KERNEL_7x9;
@@ -352,11 +408,13 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
  }
  else
  {
- stereo->setSubpixel(true);
- stereo->setSubpixelFractionalBits(4);
+ stereo->setSubpixel(subpixelFractionalBits_>=3 && subpixelFractionalBits_<=5);
+ if(subpixelFractionalBits_>=3 && subpixelFractionalBits_<=5)
+ stereo->setSubpixelFractionalBits(subpixelFractionalBits_);
  config = stereo->initialConfig.get();
- config.costMatching.disparityWidth = dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64;
- config.costMatching.enableCompanding = true;
+ config.costMatching.enableCompanding = compandingWidth_>0;
+ if(compandingWidth_>0)
+ config.costMatching.disparityWidth = compandingWidth_==64?dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64:dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_96;
  stereo->initialConfig.set(config);
  if(outputMode_ < 2)
  {
@@ -421,9 +479,17 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 
  device_.reset(new dai::Device(p, deviceToUse));
 
+ UINFO("Available camera sensors: ");
+ for(auto& sensor : device_->getCameraSensorNames()) {
+ UINFO("Socket: CAM_%c - %s", 'A'+(unsigned char)sensor.first, sensor.second.c_str());
+ }
+
  UINFO("Loading eeprom calibration data");
  dai::CalibrationHandler calibHandler = device_->readCalibration();
 
+ auto eeprom = calibHandler.getEepromData();
+ UINFO("Product name: %s, board name: %s", eeprom.productName.c_str(), eeprom.boardName.c_str());
+
  auto cameraId = outputMode_<2?dai::CameraBoardSocket::CAM_B:dai::CameraBoardSocket::CAM_A;
  cv::Mat cameraMatrix, distCoeffs, newCameraMatrix;
 
@@ -462,7 +528,6 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
  // matrix[0][0], matrix[0][1], matrix[0][2], matrix[0][3],
  // matrix[1][0], matrix[1][1], matrix[1][2], matrix[1][3],
  // matrix[2][0], matrix[2][1], matrix[2][2], matrix[2][3]);
- auto eeprom = calibHandler.getEepromData();
  if(eeprom.boardName == "OAK-D" ||
  eeprom.boardName == "BW1098OBC")
  {
@@ -538,9 +603,14 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 
  if(!device_->getIrDrivers().empty())
  {
+ UINFO("Setting IR intensity");
  device_->setIrLaserDotProjectorIntensity(dotIntensity_);
  device_->setIrFloodLightIntensity(floodIntensity_);
  }
+ else if(dotIntensity_ > 0 || floodIntensity_ > 0)
+ {
+ UWARN("No IR drivers were detected! IR intensity cannot be set.");
+ }
 
  uSleep(2000); // avoid bad frames on start
 

guilib/include/rtabmap/gui/MainWindow.h

@@ -191,6 +191,7 @@ protected Q_SLOTS:
  void selectMyntEyeS();
  void selectDepthAIOAKD();
  void selectDepthAIOAKDLite();
+ void selectDepthAIOAKDPro();
  void dumpTheMemory();
  void dumpThePrediction();
  void sendGoal();

guilib/src/GuiLib.qrc

@@ -44,5 +44,6 @@
  <file>images/oakd.png</file>
  <file>images/oakd_lite.png</file>
  <file>images/astra.png</file>
+ <file>images/oakdpro.png</file>
  </qresource>
 </RCC>

guilib/src/MainWindow.cpp

@@ -462,6 +462,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
  connect(_ui->actionMYNT_EYE_S_SDK, SIGNAL(triggered()), this, SLOT(selectMyntEyeS()));
  connect(_ui->actionDepthAI_oakd, SIGNAL(triggered()), this, SLOT(selectDepthAIOAKD()));
  connect(_ui->actionDepthAI_oakdlite, SIGNAL(triggered()), this, SLOT(selectDepthAIOAKDLite()));
+ connect(_ui->actionDepthAI_oakdpro, SIGNAL(triggered()), this, SLOT(selectDepthAIOAKDPro()));
  _ui->actionFreenect->setEnabled(CameraFreenect::available());
  _ui->actionOpenNI_CV->setEnabled(CameraOpenNICV::available());
  _ui->actionOpenNI_CV_ASUS->setEnabled(CameraOpenNICV::available());
@@ -486,6 +487,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
  _ui->actionMYNT_EYE_S_SDK->setEnabled(CameraMyntEye::available());
  _ui->actionDepthAI_oakd->setEnabled(CameraDepthAI::available());
  _ui->actionDepthAI_oakdlite->setEnabled(CameraDepthAI::available());
+ _ui->actionDepthAI_oakdpro->setEnabled(CameraDepthAI::available());
  this->updateSelectSourceMenu();
 
  connect(_ui->actionPreferences, SIGNAL(triggered()), this, SLOT(openPreferences()));
@@ -5264,6 +5266,7 @@ void MainWindow::updateSelectSourceMenu()
  _ui->actionMYNT_EYE_S_SDK->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoMyntEye);
  _ui->actionDepthAI_oakd->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoDepthAI);
  _ui->actionDepthAI_oakdlite->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoDepthAI);
+ _ui->actionDepthAI_oakdpro->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoDepthAI);
 }
 
 void MainWindow::changeImgRateSetting()
@@ -7161,6 +7164,11 @@ void MainWindow::selectDepthAIOAKDLite()
  _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoDepthAI, 0); // variant 0=no IMU
 }
 
+void MainWindow::selectDepthAIOAKDPro()
+{
+ _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoDepthAI, 2); // variant 2=IMU+color
+}
+
 void MainWindow::dumpTheMemory()
 {
  this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpMemory));

guilib/src/PreferencesDialog.cpp

@@ -814,6 +814,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
  connect(_ui->comboBox_depthai_output_mode, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
  connect(_ui->spinBox_depthai_conf_threshold, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
  connect(_ui->spinBox_depthai_lrc_threshold, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+ connect(_ui->checkBox_depthai_extended_disparity, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+ connect(_ui->comboBox_depthai_subpixel_fractional_bits, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+ connect(_ui->comboBox_depthai_disparity_companding, SIGNAL(currentIndexChanged(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()));
@@ -2157,9 +2160,12 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
  _ui->comboBox_depthai_resolution->setCurrentIndex(1);
  _ui->comboBox_depthai_output_mode->setCurrentIndex(0);
  _ui->spinBox_depthai_conf_threshold->setValue(200);
+ _ui->checkBox_depthai_extended_disparity->setChecked(false);
+ _ui->comboBox_depthai_subpixel_fractional_bits->setCurrentIndex(2);
+ _ui->comboBox_depthai_disparity_companding->setCurrentIndex(1);
  _ui->spinBox_depthai_lrc_threshold->setValue(5);
  _ui->checkBox_depthai_use_spec_translation->setChecked(false);
- _ui->doubleSpinBox_depthai_alpha_scaling->setValue(0.0);
+ _ui->doubleSpinBox_depthai_alpha_scaling->setValue(-1);
  _ui->checkBox_depthai_imu_published->setChecked(true);
  _ui->doubleSpinBox_depthai_dot_intensity->setValue(0.0);
  _ui->doubleSpinBox_depthai_flood_intensity->setValue(0.0);
@@ -2651,6 +2657,9 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
  _ui->comboBox_depthai_output_mode->setCurrentIndex(settings.value("output_mode", _ui->comboBox_depthai_output_mode->currentIndex()).toInt());
  _ui->spinBox_depthai_conf_threshold->setValue(settings.value("conf_threshold", _ui->spinBox_depthai_conf_threshold->value()).toInt());
  _ui->spinBox_depthai_lrc_threshold->setValue(settings.value("lrc_threshold", _ui->spinBox_depthai_lrc_threshold->value()).toInt());
+ _ui->checkBox_depthai_extended_disparity->setChecked(settings.value("extended_disparity", _ui->checkBox_depthai_extended_disparity->isChecked()).toBool());
+ _ui->comboBox_depthai_subpixel_fractional_bits->setCurrentIndex(settings.value("subpixel_fractional_bits", _ui->comboBox_depthai_subpixel_fractional_bits->currentIndex()).toInt());
+ _ui->comboBox_depthai_disparity_companding->setCurrentIndex(settings.value("companding", _ui->comboBox_depthai_disparity_companding->currentIndex()).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());
@@ -3188,6 +3197,9 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
  settings.setValue("output_mode", _ui->comboBox_depthai_output_mode->currentIndex());
  settings.setValue("conf_threshold", _ui->spinBox_depthai_conf_threshold->value());
  settings.setValue("lrc_threshold", _ui->spinBox_depthai_lrc_threshold->value());
+ settings.setValue("extended_disparity", _ui->checkBox_depthai_extended_disparity->isChecked());
+ settings.setValue("subpixel_fractional_bits", _ui->comboBox_depthai_subpixel_fractional_bits->currentIndex());
+ settings.setValue("companding", _ui->comboBox_depthai_disparity_companding->currentIndex());
  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());
@@ -4076,10 +4088,12 @@ void PreferencesDialog::selectSourceDriver(Src src, int variant)
  // disable IMU filtering (zed sends already quaternion)
  _ui->comboBox_imuFilter_strategy->setCurrentIndex(0);
  }
- else if(src == kSrcStereoDepthAI) // OAK-D (variant==1), OAK-D Lite (variant==0)
+ else if(src == kSrcStereoDepthAI) // OAK-D-Pro (variant==2), OAK-D (variant==1), OAK-D Lite (variant==0)
  {
- _ui->checkBox_depthai_imu_published->setChecked(variant == 1);
- _ui->comboBox_depthai_resolution->setCurrentIndex(variant == 1?1:3);
+ _ui->checkBox_depthai_imu_published->setChecked(variant >= 1);
+ _ui->comboBox_depthai_resolution->setCurrentIndex(variant >= 1?1:3);
+ _ui->comboBox_depthai_output_mode->setCurrentIndex(variant==2?2:0);
+ _ui->doubleSpinBox_depthai_dot_intensity->setValue(variant==2?1:0);
  }
  }
  else if(src >= kSrcRGB && src<kSrcDatabase)
@@ -6511,6 +6525,9 @@ Camera * PreferencesDialog::createCamera(
  this->getSourceLocalTransform());
  ((CameraDepthAI*)camera)->setOutputMode(_ui->comboBox_depthai_output_mode->currentIndex());
  ((CameraDepthAI*)camera)->setDepthProfile(_ui->spinBox_depthai_conf_threshold->value(), _ui->spinBox_depthai_lrc_threshold->value());
+ ((CameraDepthAI*)camera)->setExtendedDisparity(_ui->checkBox_depthai_extended_disparity->isChecked());
+ ((CameraDepthAI*)camera)->setSubpixelMode(_ui->comboBox_depthai_subpixel_fractional_bits->currentIndex()!=0, _ui->comboBox_depthai_subpixel_fractional_bits->currentIndex()==2?4:_ui->comboBox_depthai_subpixel_fractional_bits->currentIndex()==3?5:3);
+ ((CameraDepthAI*)camera)->setCompanding(_ui->comboBox_depthai_disparity_companding->currentIndex()!=0, _ui->comboBox_depthai_disparity_companding->currentIndex()==1?64:96);
  ((CameraDepthAI*)camera)->setRectification(_ui->checkBox_depthai_use_spec_translation->isChecked(), _ui->doubleSpinBox_depthai_alpha_scaling->value());
  ((CameraDepthAI*)camera)->setIMU(_ui->checkBox_depthai_imu_published->isChecked(), _ui->checkbox_publishInterIMU->isChecked());
  ((CameraDepthAI*)camera)->setIrIntensity(_ui->doubleSpinBox_depthai_dot_intensity->value(), _ui->doubleSpinBox_depthai_flood_intensity->value());

guilib/src/ui/mainWindow.ui

@@ -322,13 +322,24 @@
  </property>
  <addaction name="actionDepthAI_oakdlite"/>
  </widget>
+ <widget class="QMenu" name="menuOAK_D_Pro">
+ <property name="title">
+ <string>OAK-D Pro</string>
+ </property>
+ <property name="icon">
+ <iconset resource="../GuiLib.qrc">
+ <normaloff>:/images/oakdpro.png</normaloff>:/images/oakdpro.png</iconset>
+ </property>
+ <addaction name="actionDepthAI_oakdpro"/>
+ </widget>
  <addaction name="menuBumblebee2_2"/>
  <addaction name="menuZed_camera"/>
  <addaction name="menuTara_Camera"/>
  <addaction name="menuRealSense_T265"/>
  <addaction name="menuMynt_Eye_S"/>
  <addaction name="menuOAK_D"/>
  <addaction name="menuOAK_D_Lite"/>
+ <addaction name="menuOAK_D_Pro"/>
  </widget>
  <addaction name="menuRGB_D_camera"/>
  <addaction name="menuStereo_camera"/>
@@ -1699,6 +1710,14 @@
  <string>Depth AI</string>
  </property>
  </action>
+ <action name="actionDepthAI_oakdpro">
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ <property name="text">
+ <string>Depth AI</string>
+ </property>
+ </action>
  </widget>
  <customwidgets>
  <customwidget>