Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

OAK Internal Sync & DepthAI HF-Net Support #1193

Merged
merged 8 commits into from
Mar 27, 2024
Merged

OAK Internal Sync & DepthAI HF-Net Support #1193

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
5db7467
Using Sync Node
borongyuan Jan 3, 2024
83e0bbb
Using normalized IR intensity
borongyuan Jan 3, 2024
c7ed468
Set IMU output frequency to 200 Hz
borongyuan Jan 20, 2024
c15b82c
set sync threshold properly
borongyuan Jan 21, 2024
df317dc
add depthai hfnet local head
borongyuan Mar 1, 2024
73d97ae
fix DB error
borongyuan Mar 2, 2024
306898f
add depthai hfnet global head
borongyuan Mar 2, 2024
b69c62d
fix data delay if image rate is set manually
borongyuan Mar 21, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 4 additions & 6 deletions corelib/include/rtabmap/core/camera/CameraDepthAI.h
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading