Skip to content

Commit

Permalink
add depthai gftt detector (#1047)
Browse files Browse the repository at this point in the history 
* add depthai feature detector draft

* update depthai gftt config

* optimize to detect more features
  • Loading branch information
@borongyuan
borongyuan authored Jun 11, 2023
1 parent 0c47693 commit d031d79
Show file tree
Hide file tree
Showing 4 changed files with 111 additions and 1 deletion.
7 changes: 7 additions & 0 deletions corelib/include/rtabmap/core/camera/CameraDepthAI.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,8 @@ class RTABMAP_CORE_EXPORT CameraDepthAI :
void publishInterIMU(bool enabled);
void setLaserDotBrightness(float dotProjectormA = 0.0f);
void setFloodLightBrightness(float floodLightmA = 200.0f);
void setDetectFeatures(int detectFeatures = 0);
void setGFTTDetector(bool useHarrisDetector, double minDistance = 7.0f, int numTargetFeatures = 1000);

virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
Expand All @@ -84,9 +86,14 @@ class RTABMAP_CORE_EXPORT CameraDepthAI :
bool publishInterIMU_;
float dotProjectormA_;
float floodLightmA_;
int detectFeatures_;
bool useHarrisDetector_;
double minDistance_;
int numTargetFeatures_;
std::shared_ptr<dai::Device> device_;
std::shared_ptr<dai::DataOutputQueue> leftQueue_;
std::shared_ptr<dai::DataOutputQueue> rightOrDepthQueue_;
std::shared_ptr<dai::DataOutputQueue> featuresQueue_;
std::map<double, cv::Vec3f> accBuffer_;
std::map<double, cv::Vec3f> gyroBuffer_;
UMutex imuMutex_;
Expand Down
63 changes: 62 additions & 1 deletion corelib/src/camera/CameraDepthAI.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,11 @@ CameraDepthAI::CameraDepthAI(
imuPublished_(true),
publishInterIMU_(false),
dotProjectormA_(0.0),
floodLightmA_(200.0)
floodLightmA_(200.0),
detectFeatures_(0),
useHarrisDetector_(false),
minDistance_(7.0),
numTargetFeatures_(1000)
#endif
{
#ifdef RTABMAP_DEPTHAI
Expand Down Expand Up @@ -146,6 +150,26 @@ void CameraDepthAI::setFloodLightBrightness(float floodLightmA)
#endif
}

void CameraDepthAI::setDetectFeatures(int detectFeatures)
{
#ifdef RTABMAP_DEPTHAI
detectFeatures_ = detectFeatures;
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
}

void CameraDepthAI::setGFTTDetector(bool useHarrisDetector, double minDistance, int numTargetFeatures)
{
#ifdef RTABMAP_DEPTHAI
useHarrisDetector_ = useHarrisDetector;
minDistance_ = minDistance;
numTargetFeatures_ = numTargetFeatures;
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
}

bool CameraDepthAI::init(const std::string & calibrationFolder, const std::string & cameraName)
{
UDEBUG("");
Expand Down Expand Up @@ -198,18 +222,26 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
std::shared_ptr<dai::node::IMU> imu;
if(imuPublished_)
imu = p.create<dai::node::IMU>();
std::shared_ptr<dai::node::FeatureTracker> gfttDetector;
if(detectFeatures_ == 1)
gfttDetector = p.create<dai::node::FeatureTracker>();

auto xoutLeft = p.create<dai::node::XLinkOut>();
auto xoutDepthOrRight = p.create<dai::node::XLinkOut>();
std::shared_ptr<dai::node::XLinkOut> xoutIMU;
if(imuPublished_)
xoutIMU = p.create<dai::node::XLinkOut>();
std::shared_ptr<dai::node::XLinkOut> xoutFeatures;
if(detectFeatures_)
xoutFeatures = p.create<dai::node::XLinkOut>();

// XLinkOut
xoutLeft->setStreamName("rectified_left");
xoutDepthOrRight->setStreamName(outputDepth_?"depth":"rectified_right");
if(imuPublished_)
xoutIMU->setStreamName("imu");
if(detectFeatures_)
xoutFeatures->setStreamName("features");

// MonoCamera
monoLeft->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
Expand Down Expand Up @@ -266,6 +298,20 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
imu->enableFirmwareUpdate(imuFirmwareUpdate_);
}

if(detectFeatures_ == 1)
{
gfttDetector->setHardwareResources(1, 2);
gfttDetector->initialConfig.setCornerDetector(
useHarrisDetector_?dai::FeatureTrackerConfig::CornerDetector::Type::HARRIS:dai::FeatureTrackerConfig::CornerDetector::Type::SHI_THOMASI);
gfttDetector->initialConfig.setNumTargetFeatures(numTargetFeatures_);
gfttDetector->initialConfig.setMotionEstimator(false);
auto cfg = gfttDetector->initialConfig.get();
cfg.featureMaintainer.minimumDistanceBetweenFeatures = minDistance_ * minDistance_;
gfttDetector->initialConfig.set(cfg);
stereo->rectifiedLeft.link(gfttDetector->inputImage);
gfttDetector->outputFeatures.link(xoutFeatures->input);
}

device_.reset(new dai::Device(p, deviceToUse));

UINFO("Loading eeprom calibration data");
Expand Down Expand Up @@ -370,6 +416,8 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
}
leftQueue_ = device_->getOutputQueue("rectified_left", 8, false);
rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 8, false);
if(detectFeatures_)
featuresQueue_ = device_->getOutputQueue("features", 8, false);

std::vector<std::tuple<std::string, int, int>> irDrivers = device_->getIrDrivers();
if(!irDrivers.empty())
Expand Down Expand Up @@ -476,6 +524,19 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
data.setIMU(IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_));
}

if(detectFeatures_ == 1)
{
auto features = featuresQueue_->get<dai::TrackedFeatures>();
while(features->getSequenceNum() < rectifL->getSequenceNum())
features = featuresQueue_->get<dai::TrackedFeatures>();
auto detectedFeatures = features->trackedFeatures;

std::vector<cv::KeyPoint> keypoints;
for(auto& feature : detectedFeatures)
keypoints.emplace_back(cv::KeyPoint(feature.position.x, feature.position.y, 3));
data.setFeatures(keypoints, std::vector<cv::Point3f>(), cv::Mat());
}

#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
Expand Down
9 changes: 9 additions & 0 deletions guilib/src/PreferencesDialog.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -810,6 +810,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
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()));

connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_source_imageDecimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
Expand Down Expand Up @@ -2067,6 +2068,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_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);

_ui->checkBox_cameraImages_configForEachFrame->setChecked(false);
_ui->checkBox_cameraImages_timestamps->setChecked(false);
Expand Down Expand Up @@ -2554,6 +2556,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
_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());
settings.endGroup(); // DepthAI

settings.beginGroup("Images");
Expand Down Expand Up @@ -3084,6 +3087,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.setValue("imu_firmware_update", _ui->checkBox_depthai_imu_firmware_update->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());
settings.endGroup(); // DepthAI

settings.beginGroup("Images");
Expand Down Expand Up @@ -6335,6 +6339,11 @@ Camera * PreferencesDialog::createCamera(
((CameraDepthAI*)camera)->publishInterIMU(_ui->checkbox_publishInterIMU->isChecked());
((CameraDepthAI*)camera)->setLaserDotBrightness(_ui->doubleSpinBox_depthai_laser_dot_brightness->value());
((CameraDepthAI*)camera)->setFloodLightBrightness(_ui->doubleSpinBox_depthai_floodlight_brightness->value());
((CameraDepthAI*)camera)->setDetectFeatures(_ui->comboBox_depthai_detect_features->currentIndex());
if(_ui->comboBox_depthai_detect_features->currentIndex() == 1)
{
((CameraDepthAI*)camera)->setGFTTDetector(_ui->checkBox_GFTT_useHarrisDetector->isChecked(), _ui->doubleSpinBox_GFTT_minDistance->value(), _ui->reextract_maxFeatures->value());
}
}
else if(driver == kSrcUsbDevice)
{
Expand Down
33 changes: 33 additions & 0 deletions guilib/src/ui/preferencesDialog.ui
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6024,6 +6024,39 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QComboBox" name="comboBox_depthai_detect_features">
<property name="currentIndex">
<number>0</number>
</property>
<property name="sizeAdjustPolicy">
<enum>QComboBox::AdjustToContents</enum>
</property>
<item>
<property name="text">
<string>None</string>
</property>
</item>
<item>
<property name="text">
<string>GFTT</string>
</property>
</item>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_679">
<property name="text">
<string>On-device feature detector.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
</layout>
</widget>
</item>
Expand Down

0 comments on commit d031d79

Please sign in to comment.