Skip to content

Commit

Permalink
OAK Internal Sync & DepthAI HF-Net Support (#1193)
Browse files Browse the repository at this point in the history 
* Using Sync Node

* Using normalized IR intensity

* Set IMU output frequency to 200 Hz

* set sync threshold properly

* add depthai hfnet local head

* fix DB error

* add depthai hfnet global head

* fix data delay if image rate is set manually
  • Loading branch information
@borongyuan
borongyuan authored Mar 27, 2024
1 parent bc6d390 commit bfeb487
Show file tree
Hide file tree
Showing 5 changed files with 104 additions and 104 deletions.
10 changes: 4 additions & 6 deletions corelib/include/rtabmap/core/camera/CameraDepthAI.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,7 @@ class RTABMAP_CORE_EXPORT CameraDepthAI :
void setDepthProfile(int confThreshold = 200, int lrcThreshold = 5);
void setRectification(bool useSpecTranslation, float alphaScaling = 0.0f);
void setIMU(bool imuPublished, bool publishInterIMU);
void setIrBrightness(float dotProjectormA = 0.0f, float floodLightmA = 200.0f);
void setIrIntensity(float dotIntensity = 0.0f, float floodIntensity = 0.0f);
void setDetectFeatures(int detectFeatures = 0);
void setBlobPath(const std::string & blobPath);
void setGFTTDetector(bool useHarrisDetector = false, float minDistance = 7.0f, int numTargetFeatures = 1000);
Expand All @@ -86,8 +86,8 @@ class RTABMAP_CORE_EXPORT CameraDepthAI :
float alphaScaling_;
bool imuPublished_;
bool publishInterIMU_;
float dotProjectormA_;
float floodLightmA_;
float dotIntensity_;
float floodIntensity_;
int detectFeatures_;
bool useHarrisDetector_;
float minDistance_;
Expand All @@ -97,9 +97,7 @@ class RTABMAP_CORE_EXPORT CameraDepthAI :
int nmsRadius_;
std::string blobPath_;
std::shared_ptr<dai::Device> device_;
std::shared_ptr<dai::DataOutputQueue> leftOrColorQueue_;
std::shared_ptr<dai::DataOutputQueue> rightOrDepthQueue_;
std::shared_ptr<dai::DataOutputQueue> featuresQueue_;
std::shared_ptr<dai::DataOutputQueue> cameraQueue_;
std::map<double, cv::Vec3f> accBuffer_;
std::map<double, cv::Vec3f> gyroBuffer_;
UMutex imuMutex_;
Expand Down
2 changes: 1 addition & 1 deletion corelib/src/DBDriverSqlite3.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6728,7 +6728,7 @@ void DBDriverSqlite3::stepGlobalDescriptor(sqlite3_stmt * ppStmt,

//data
std::vector<unsigned char> dataBytes = rtabmap::compressData(descriptor.data());
if(infoBytes.empty())
if(dataBytes.empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
}
Expand Down
143 changes: 70 additions & 73 deletions corelib/src/camera/CameraDepthAI.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,8 +62,8 @@ CameraDepthAI::CameraDepthAI(
alphaScaling_(0.0),
imuPublished_(true),
publishInterIMU_(false),
dotProjectormA_(0.0),
floodLightmA_(200.0),
dotIntensity_(0.0),
floodIntensity_(0.0),
detectFeatures_(0),
useHarrisDetector_(false),
minDistance_(7.0),
Expand Down Expand Up @@ -127,11 +127,11 @@ void CameraDepthAI::setIMU(bool imuPublished, bool publishInterIMU)
#endif
}

void CameraDepthAI::setIrBrightness(float dotProjectormA, float floodLightmA)
void CameraDepthAI::setIrIntensity(float dotIntensity, float floodIntensity)
{
#ifdef RTABMAP_DEPTHAI
dotProjectormA_ = dotProjectormA;
floodLightmA_ = floodLightmA;
dotIntensity_ = dotIntensity;
floodIntensity_ = floodIntensity;
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
Expand Down Expand Up @@ -235,51 +235,45 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
imu = p.create<dai::node::IMU>();
std::shared_ptr<dai::node::FeatureTracker> gfttDetector;
std::shared_ptr<dai::node::ImageManip> manip;
std::shared_ptr<dai::node::NeuralNetwork> superPointNetwork;
std::shared_ptr<dai::node::NeuralNetwork> neuralNetwork;
if(detectFeatures_ == 1)
{
gfttDetector = p.create<dai::node::FeatureTracker>();
}
else if(detectFeatures_ == 2)
else if(detectFeatures_ >= 2)
{
if(!blobPath_.empty())
{
manip = p.create<dai::node::ImageManip>();
superPointNetwork = p.create<dai::node::NeuralNetwork>();
neuralNetwork = p.create<dai::node::NeuralNetwork>();
}
else
{
UWARN("Missing SuperPoint blob file!");
UWARN("Missing MyriadX blob file!");
detectFeatures_ = 0;
}
}

auto xoutLeftOrColor = p.create<dai::node::XLinkOut>();
auto xoutDepthOrRight = p.create<dai::node::XLinkOut>();
auto sync = p.create<dai::node::Sync>();
auto xoutCamera = 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
xoutLeftOrColor->setStreamName(outputMode_<2?"rectified_left":"rectified_color");
xoutDepthOrRight->setStreamName(outputMode_?"depth":"rectified_right");
xoutCamera->setStreamName("camera");
if(imuPublished_)
xoutIMU->setStreamName("imu");
if(detectFeatures_)
xoutFeatures->setStreamName("features");

monoLeft->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
monoRight->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
monoLeft->setCamera("left");
monoRight->setCamera("right");
if(detectFeatures_ == 2)
if(detectFeatures_ >= 2)
{
if(this->getImageRate() <= 0 || this->getImageRate() > 15)
{
UWARN("On-device SuperPoint enabled, image rate is limited to 15 FPS!");
UWARN("On-device SuperPoint or HF-Net enabled, image rate is limited to 15 FPS!");
monoLeft->setFps(15);
monoRight->setFps(15);
}
Expand Down Expand Up @@ -325,6 +319,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
if(alphaScaling_ > -1.0f)
colorCam->setCalibrationAlpha(alphaScaling_);
}
this->setImageRate(0);

// Using VideoEncoder on PoE devices, Subpixel is not supported
if(deviceToUse.protocol == X_LINK_TCP_IP || mxidOrName_.find(".") != std::string::npos)
Expand All @@ -336,22 +331,24 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
if(outputMode_ < 2)
{
stereo->rectifiedLeft.link(leftOrColorEnc->input);
leftOrColorEnc->bitstream.link(sync->inputs["left"]);
}
else
{
colorCam->video.link(leftOrColorEnc->input);
leftOrColorEnc->bitstream.link(sync->inputs["color"]);
}
if(outputMode_)
{
depthOrRightEnc->setQuality(100);
stereo->disparity.link(depthOrRightEnc->input);
depthOrRightEnc->bitstream.link(sync->inputs["depth"]);
}
else
{
stereo->rectifiedRight.link(depthOrRightEnc->input);
depthOrRightEnc->bitstream.link(sync->inputs["right"]);
}
leftOrColorEnc->bitstream.link(xoutLeftOrColor->input);
depthOrRightEnc->bitstream.link(xoutDepthOrRight->input);
}
else
{
Expand All @@ -363,24 +360,27 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
stereo->initialConfig.set(config);
if(outputMode_ < 2)
{
stereo->rectifiedLeft.link(xoutLeftOrColor->input);
stereo->rectifiedLeft.link(sync->inputs["left"]);
}
else
{
monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
colorCam->video.link(xoutLeftOrColor->input);
colorCam->video.link(sync->inputs["color"]);
}
if(outputMode_)
stereo->depth.link(xoutDepthOrRight->input);
stereo->depth.link(sync->inputs["depth"]);
else
stereo->rectifiedRight.link(xoutDepthOrRight->input);
stereo->rectifiedRight.link(sync->inputs["right"]);
}

sync->setSyncThreshold(std::chrono::milliseconds(int(500 / monoLeft->getFps())));
sync->out.link(xoutCamera->input);

if(imuPublished_)
{
// enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 100 hz rate
imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 100);
// enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 200 hz rate
imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 200);
// above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
imu->setBatchReportThreshold(1);
// maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
Expand All @@ -403,20 +403,20 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
cfg.featureMaintainer.minimumDistanceBetweenFeatures = minDistance_ * minDistance_;
gfttDetector->initialConfig.set(cfg);
stereo->rectifiedLeft.link(gfttDetector->inputImage);
gfttDetector->outputFeatures.link(xoutFeatures->input);
gfttDetector->outputFeatures.link(sync->inputs["feat"]);
}
else if(detectFeatures_ == 2)
else if(detectFeatures_ >= 2)
{
manip->setKeepAspectRatio(false);
manip->setMaxOutputFrameSize(320 * 200);
manip->initialConfig.setResize(320, 200);
superPointNetwork->setBlobPath(blobPath_);
superPointNetwork->setNumInferenceThreads(2);
superPointNetwork->setNumNCEPerInferenceThread(1);
superPointNetwork->input.setBlocking(false);
neuralNetwork->setBlobPath(blobPath_);
neuralNetwork->setNumInferenceThreads(2);
neuralNetwork->setNumNCEPerInferenceThread(1);
neuralNetwork->input.setBlocking(false);
stereo->rectifiedLeft.link(manip->inputImage);
manip->out.link(superPointNetwork->input);
superPointNetwork->out.link(xoutFeatures->input);
manip->out.link(neuralNetwork->input);
neuralNetwork->out.link(sync->inputs["feat"]);
}

device_.reset(new dai::Device(p, deviceToUse));
Expand Down Expand Up @@ -503,6 +503,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
UINFO("IMU disabled");
}

cameraQueue_ = device_->getOutputQueue("camera", 8, false);
if(imuPublished_)
{
imuLocalTransform_ = this->getLocalTransform() * imuLocalTransform_;
Expand Down Expand Up @@ -534,16 +535,11 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
}
});
}
leftOrColorQueue_ = device_->getOutputQueue(outputMode_<2?"rectified_left":"rectified_color", 8, false);
rightOrDepthQueue_ = device_->getOutputQueue(outputMode_?"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())
if(!device_->getIrDrivers().empty())
{
device_->setIrLaserDotProjectorBrightness(dotProjectormA_);
device_->setIrFloodLightBrightness(floodLightmA_);
device_->setIrLaserDotProjectorIntensity(dotIntensity_);
device_->setIrFloodLightIntensity(floodIntensity_);
}

uSleep(2000); // avoid bad frames on start
Expand Down Expand Up @@ -577,16 +573,11 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
SensorData data;
#ifdef RTABMAP_DEPTHAI

cv::Mat leftOrColor, depthOrRight;
auto rectifLeftOrColor = leftOrColorQueue_->get<dai::ImgFrame>();
auto rectifRightOrDepth = rightOrDepthQueue_->get<dai::ImgFrame>();

while(rectifLeftOrColor->getSequenceNum() < rectifRightOrDepth->getSequenceNum())
rectifLeftOrColor = leftOrColorQueue_->get<dai::ImgFrame>();
while(rectifLeftOrColor->getSequenceNum() > rectifRightOrDepth->getSequenceNum())
rectifRightOrDepth = rightOrDepthQueue_->get<dai::ImgFrame>();
auto messageGroup = cameraQueue_->get<dai::MessageGroup>();
auto rectifLeftOrColor = messageGroup->get<dai::ImgFrame>(outputMode_<2?"left":"color");
auto rectifRightOrDepth = messageGroup->get<dai::ImgFrame>(outputMode_?"depth":"right");

double stamp = std::chrono::duration<double>(rectifLeftOrColor->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count();
cv::Mat leftOrColor, depthOrRight;
if(device_->getDeviceInfo().protocol == X_LINK_TCP_IP || mxidOrName_.find(".") != std::string::npos)
{
leftOrColor = cv::imdecode(rectifLeftOrColor->getData(), cv::IMREAD_ANYCOLOR);
Expand All @@ -604,6 +595,7 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
depthOrRight = rectifRightOrDepth->getCvFrame();
}

double stamp = std::chrono::duration<double>(rectifLeftOrColor->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count();
if(outputMode_)
data = SensorData(leftOrColor, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);
else
Expand Down Expand Up @@ -660,28 +652,30 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)

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

auto features = messageGroup->get<dai::TrackedFeatures>("feat")->trackedFeatures;
std::vector<cv::KeyPoint> keypoints;
for(auto& feature : detectedFeatures)
for(auto& feature : features)
keypoints.emplace_back(cv::KeyPoint(feature.position.x, feature.position.y, 3));
data.setFeatures(keypoints, std::vector<cv::Point3f>(), cv::Mat());
}
else if(detectFeatures_ == 2)
else if(detectFeatures_ >= 2)
{
auto features = featuresQueue_->get<dai::NNData>();
while(features->getSequenceNum() < rectifLeftOrColor->getSequenceNum())
features = featuresQueue_->get<dai::NNData>();

auto heatmap = features->getLayerFp16("heatmap");
auto desc = features->getLayerFp16("desc");

cv::Mat scores(200, 320, CV_32FC1, heatmap.data());
cv::resize(scores, scores, targetSize_, 0, 0, cv::INTER_CUBIC);
auto features = messageGroup->get<dai::NNData>("feat");
std::vector<float> scores_dense, local_descriptor_map, global_descriptor;
if(detectFeatures_ == 2)
{
scores_dense = features->getLayerFp16("heatmap");
local_descriptor_map = features->getLayerFp16("desc");
}
else if(detectFeatures_ == 3)
{
scores_dense = features->getLayerFp16("pred/local_head/detector/Squeeze");
local_descriptor_map = features->getLayerFp16("pred/local_head/descriptor/transpose");
global_descriptor = features->getLayerFp16("pred/global_head/l2_normalize_1");
}

cv::Mat scores(200, 320, CV_32FC1, scores_dense.data());
cv::resize(scores, scores, targetSize_, 0, 0, cv::INTER_CUBIC);
if(nms_)
{
cv::Mat dilated_scores(targetSize_, CV_32FC1);
Expand Down Expand Up @@ -714,10 +708,11 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
keypoints.emplace_back(cv::KeyPoint(kpt, 8, -1, response));
}

cv::Mat coarse_desc(25, 40, CV_32FC(256), desc.data());
coarse_desc.forEach<cv::Vec<float, 256>>([&](cv::Vec<float, 256>& descriptor, const int position[]) -> void {
cv::normalize(descriptor, descriptor);
});
cv::Mat coarse_desc(25, 40, CV_32FC(256), local_descriptor_map.data());
if(detectFeatures_ == 2)
coarse_desc.forEach<cv::Vec<float, 256>>([&](cv::Vec<float, 256>& descriptor, const int position[]) -> void {
cv::normalize(descriptor, descriptor);
});
cv::Mat mapX(keypoints.size(), 1, CV_32FC1);
cv::Mat mapY(keypoints.size(), 1, CV_32FC1);
for(size_t i=0; i<keypoints.size(); ++i)
Expand All @@ -734,6 +729,8 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
descriptors = descriptors.reshape(1);

data.setFeatures(keypoints, std::vector<cv::Point3f>(), descriptors);
if(detectFeatures_ == 3)
data.addGlobalDescriptor(GlobalDescriptor(1, cv::Mat(1, global_descriptor.size(), CV_32FC1, global_descriptor.data())));
}

#else
Expand Down
Loading

0 comments on commit bfeb487

Please sign in to comment.