mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-08 02:57:46 +08:00
OAK Internal Sync & DepthAI HF-Net Support (#1193)
* 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
This commit is contained in:
@@ -59,7 +59,7 @@ public:
|
|||||||
void setDepthProfile(int confThreshold = 200, int lrcThreshold = 5);
|
void setDepthProfile(int confThreshold = 200, int lrcThreshold = 5);
|
||||||
void setRectification(bool useSpecTranslation, float alphaScaling = 0.0f);
|
void setRectification(bool useSpecTranslation, float alphaScaling = 0.0f);
|
||||||
void setIMU(bool imuPublished, bool publishInterIMU);
|
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 setDetectFeatures(int detectFeatures = 0);
|
||||||
void setBlobPath(const std::string & blobPath);
|
void setBlobPath(const std::string & blobPath);
|
||||||
void setGFTTDetector(bool useHarrisDetector = false, float minDistance = 7.0f, int numTargetFeatures = 1000);
|
void setGFTTDetector(bool useHarrisDetector = false, float minDistance = 7.0f, int numTargetFeatures = 1000);
|
||||||
@@ -86,8 +86,8 @@ private:
|
|||||||
float alphaScaling_;
|
float alphaScaling_;
|
||||||
bool imuPublished_;
|
bool imuPublished_;
|
||||||
bool publishInterIMU_;
|
bool publishInterIMU_;
|
||||||
float dotProjectormA_;
|
float dotIntensity_;
|
||||||
float floodLightmA_;
|
float floodIntensity_;
|
||||||
int detectFeatures_;
|
int detectFeatures_;
|
||||||
bool useHarrisDetector_;
|
bool useHarrisDetector_;
|
||||||
float minDistance_;
|
float minDistance_;
|
||||||
@@ -97,9 +97,7 @@ private:
|
|||||||
int nmsRadius_;
|
int nmsRadius_;
|
||||||
std::string blobPath_;
|
std::string blobPath_;
|
||||||
std::shared_ptr<dai::Device> device_;
|
std::shared_ptr<dai::Device> device_;
|
||||||
std::shared_ptr<dai::DataOutputQueue> leftOrColorQueue_;
|
std::shared_ptr<dai::DataOutputQueue> cameraQueue_;
|
||||||
std::shared_ptr<dai::DataOutputQueue> rightOrDepthQueue_;
|
|
||||||
std::shared_ptr<dai::DataOutputQueue> featuresQueue_;
|
|
||||||
std::map<double, cv::Vec3f> accBuffer_;
|
std::map<double, cv::Vec3f> accBuffer_;
|
||||||
std::map<double, cv::Vec3f> gyroBuffer_;
|
std::map<double, cv::Vec3f> gyroBuffer_;
|
||||||
UMutex imuMutex_;
|
UMutex imuMutex_;
|
||||||
|
|||||||
@@ -6728,7 +6728,7 @@ void DBDriverSqlite3::stepGlobalDescriptor(sqlite3_stmt * ppStmt,
|
|||||||
|
|
||||||
//data
|
//data
|
||||||
std::vector<unsigned char> dataBytes = rtabmap::compressData(descriptor.data());
|
std::vector<unsigned char> dataBytes = rtabmap::compressData(descriptor.data());
|
||||||
if(infoBytes.empty())
|
if(dataBytes.empty())
|
||||||
{
|
{
|
||||||
rc = sqlite3_bind_null(ppStmt, index++);
|
rc = sqlite3_bind_null(ppStmt, index++);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -62,8 +62,8 @@ CameraDepthAI::CameraDepthAI(
|
|||||||
alphaScaling_(0.0),
|
alphaScaling_(0.0),
|
||||||
imuPublished_(true),
|
imuPublished_(true),
|
||||||
publishInterIMU_(false),
|
publishInterIMU_(false),
|
||||||
dotProjectormA_(0.0),
|
dotIntensity_(0.0),
|
||||||
floodLightmA_(200.0),
|
floodIntensity_(0.0),
|
||||||
detectFeatures_(0),
|
detectFeatures_(0),
|
||||||
useHarrisDetector_(false),
|
useHarrisDetector_(false),
|
||||||
minDistance_(7.0),
|
minDistance_(7.0),
|
||||||
@@ -127,11 +127,11 @@ void CameraDepthAI::setIMU(bool imuPublished, bool publishInterIMU)
|
|||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void CameraDepthAI::setIrBrightness(float dotProjectormA, float floodLightmA)
|
void CameraDepthAI::setIrIntensity(float dotIntensity, float floodIntensity)
|
||||||
{
|
{
|
||||||
#ifdef RTABMAP_DEPTHAI
|
#ifdef RTABMAP_DEPTHAI
|
||||||
dotProjectormA_ = dotProjectormA;
|
dotIntensity_ = dotIntensity;
|
||||||
floodLightmA_ = floodLightmA;
|
floodIntensity_ = floodIntensity;
|
||||||
#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
|
||||||
@@ -235,51 +235,45 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
|
|||||||
imu = p.create<dai::node::IMU>();
|
imu = p.create<dai::node::IMU>();
|
||||||
std::shared_ptr<dai::node::FeatureTracker> gfttDetector;
|
std::shared_ptr<dai::node::FeatureTracker> gfttDetector;
|
||||||
std::shared_ptr<dai::node::ImageManip> manip;
|
std::shared_ptr<dai::node::ImageManip> manip;
|
||||||
std::shared_ptr<dai::node::NeuralNetwork> superPointNetwork;
|
std::shared_ptr<dai::node::NeuralNetwork> neuralNetwork;
|
||||||
if(detectFeatures_ == 1)
|
if(detectFeatures_ == 1)
|
||||||
{
|
{
|
||||||
gfttDetector = p.create<dai::node::FeatureTracker>();
|
gfttDetector = p.create<dai::node::FeatureTracker>();
|
||||||
}
|
}
|
||||||
else if(detectFeatures_ == 2)
|
else if(detectFeatures_ >= 2)
|
||||||
{
|
{
|
||||||
if(!blobPath_.empty())
|
if(!blobPath_.empty())
|
||||||
{
|
{
|
||||||
manip = p.create<dai::node::ImageManip>();
|
manip = p.create<dai::node::ImageManip>();
|
||||||
superPointNetwork = p.create<dai::node::NeuralNetwork>();
|
neuralNetwork = p.create<dai::node::NeuralNetwork>();
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
UWARN("Missing SuperPoint blob file!");
|
UWARN("Missing MyriadX blob file!");
|
||||||
detectFeatures_ = 0;
|
detectFeatures_ = 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
auto xoutLeftOrColor = p.create<dai::node::XLinkOut>();
|
auto sync = p.create<dai::node::Sync>();
|
||||||
auto xoutDepthOrRight = p.create<dai::node::XLinkOut>();
|
auto xoutCamera = p.create<dai::node::XLinkOut>();
|
||||||
std::shared_ptr<dai::node::XLinkOut> xoutIMU;
|
std::shared_ptr<dai::node::XLinkOut> xoutIMU;
|
||||||
if(imuPublished_)
|
if(imuPublished_)
|
||||||
xoutIMU = p.create<dai::node::XLinkOut>();
|
xoutIMU = p.create<dai::node::XLinkOut>();
|
||||||
std::shared_ptr<dai::node::XLinkOut> xoutFeatures;
|
|
||||||
if(detectFeatures_)
|
|
||||||
xoutFeatures = p.create<dai::node::XLinkOut>();
|
|
||||||
|
|
||||||
// XLinkOut
|
// XLinkOut
|
||||||
xoutLeftOrColor->setStreamName(outputMode_<2?"rectified_left":"rectified_color");
|
xoutCamera->setStreamName("camera");
|
||||||
xoutDepthOrRight->setStreamName(outputMode_?"depth":"rectified_right");
|
|
||||||
if(imuPublished_)
|
if(imuPublished_)
|
||||||
xoutIMU->setStreamName("imu");
|
xoutIMU->setStreamName("imu");
|
||||||
if(detectFeatures_)
|
|
||||||
xoutFeatures->setStreamName("features");
|
|
||||||
|
|
||||||
monoLeft->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
|
monoLeft->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
|
||||||
monoRight->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
|
monoRight->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
|
||||||
monoLeft->setCamera("left");
|
monoLeft->setCamera("left");
|
||||||
monoRight->setCamera("right");
|
monoRight->setCamera("right");
|
||||||
if(detectFeatures_ == 2)
|
if(detectFeatures_ >= 2)
|
||||||
{
|
{
|
||||||
if(this->getImageRate() <= 0 || this->getImageRate() > 15)
|
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);
|
monoLeft->setFps(15);
|
||||||
monoRight->setFps(15);
|
monoRight->setFps(15);
|
||||||
}
|
}
|
||||||
@@ -325,6 +319,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
|
|||||||
if(alphaScaling_ > -1.0f)
|
if(alphaScaling_ > -1.0f)
|
||||||
colorCam->setCalibrationAlpha(alphaScaling_);
|
colorCam->setCalibrationAlpha(alphaScaling_);
|
||||||
}
|
}
|
||||||
|
this->setImageRate(0);
|
||||||
|
|
||||||
// Using VideoEncoder on PoE devices, Subpixel is not supported
|
// Using VideoEncoder on PoE devices, Subpixel is not supported
|
||||||
if(deviceToUse.protocol == X_LINK_TCP_IP || mxidOrName_.find(".") != std::string::npos)
|
if(deviceToUse.protocol == X_LINK_TCP_IP || mxidOrName_.find(".") != std::string::npos)
|
||||||
@@ -336,22 +331,24 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
|
|||||||
if(outputMode_ < 2)
|
if(outputMode_ < 2)
|
||||||
{
|
{
|
||||||
stereo->rectifiedLeft.link(leftOrColorEnc->input);
|
stereo->rectifiedLeft.link(leftOrColorEnc->input);
|
||||||
|
leftOrColorEnc->bitstream.link(sync->inputs["left"]);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
colorCam->video.link(leftOrColorEnc->input);
|
colorCam->video.link(leftOrColorEnc->input);
|
||||||
|
leftOrColorEnc->bitstream.link(sync->inputs["color"]);
|
||||||
}
|
}
|
||||||
if(outputMode_)
|
if(outputMode_)
|
||||||
{
|
{
|
||||||
depthOrRightEnc->setQuality(100);
|
depthOrRightEnc->setQuality(100);
|
||||||
stereo->disparity.link(depthOrRightEnc->input);
|
stereo->disparity.link(depthOrRightEnc->input);
|
||||||
|
depthOrRightEnc->bitstream.link(sync->inputs["depth"]);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
stereo->rectifiedRight.link(depthOrRightEnc->input);
|
stereo->rectifiedRight.link(depthOrRightEnc->input);
|
||||||
|
depthOrRightEnc->bitstream.link(sync->inputs["right"]);
|
||||||
}
|
}
|
||||||
leftOrColorEnc->bitstream.link(xoutLeftOrColor->input);
|
|
||||||
depthOrRightEnc->bitstream.link(xoutDepthOrRight->input);
|
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
@@ -363,24 +360,27 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
|
|||||||
stereo->initialConfig.set(config);
|
stereo->initialConfig.set(config);
|
||||||
if(outputMode_ < 2)
|
if(outputMode_ < 2)
|
||||||
{
|
{
|
||||||
stereo->rectifiedLeft.link(xoutLeftOrColor->input);
|
stereo->rectifiedLeft.link(sync->inputs["left"]);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
|
monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
|
||||||
monoRight->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_)
|
if(outputMode_)
|
||||||
stereo->depth.link(xoutDepthOrRight->input);
|
stereo->depth.link(sync->inputs["depth"]);
|
||||||
else
|
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_)
|
if(imuPublished_)
|
||||||
{
|
{
|
||||||
// enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 100 hz rate
|
// enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 200 hz rate
|
||||||
imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 100);
|
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
|
// 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);
|
imu->setBatchReportThreshold(1);
|
||||||
// maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
|
// maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
|
||||||
@@ -403,20 +403,20 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
|
|||||||
cfg.featureMaintainer.minimumDistanceBetweenFeatures = minDistance_ * minDistance_;
|
cfg.featureMaintainer.minimumDistanceBetweenFeatures = minDistance_ * minDistance_;
|
||||||
gfttDetector->initialConfig.set(cfg);
|
gfttDetector->initialConfig.set(cfg);
|
||||||
stereo->rectifiedLeft.link(gfttDetector->inputImage);
|
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->setKeepAspectRatio(false);
|
||||||
manip->setMaxOutputFrameSize(320 * 200);
|
manip->setMaxOutputFrameSize(320 * 200);
|
||||||
manip->initialConfig.setResize(320, 200);
|
manip->initialConfig.setResize(320, 200);
|
||||||
superPointNetwork->setBlobPath(blobPath_);
|
neuralNetwork->setBlobPath(blobPath_);
|
||||||
superPointNetwork->setNumInferenceThreads(2);
|
neuralNetwork->setNumInferenceThreads(2);
|
||||||
superPointNetwork->setNumNCEPerInferenceThread(1);
|
neuralNetwork->setNumNCEPerInferenceThread(1);
|
||||||
superPointNetwork->input.setBlocking(false);
|
neuralNetwork->input.setBlocking(false);
|
||||||
stereo->rectifiedLeft.link(manip->inputImage);
|
stereo->rectifiedLeft.link(manip->inputImage);
|
||||||
manip->out.link(superPointNetwork->input);
|
manip->out.link(neuralNetwork->input);
|
||||||
superPointNetwork->out.link(xoutFeatures->input);
|
neuralNetwork->out.link(sync->inputs["feat"]);
|
||||||
}
|
}
|
||||||
|
|
||||||
device_.reset(new dai::Device(p, deviceToUse));
|
device_.reset(new dai::Device(p, deviceToUse));
|
||||||
@@ -503,6 +503,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
|
|||||||
UINFO("IMU disabled");
|
UINFO("IMU disabled");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
cameraQueue_ = device_->getOutputQueue("camera", 8, false);
|
||||||
if(imuPublished_)
|
if(imuPublished_)
|
||||||
{
|
{
|
||||||
imuLocalTransform_ = this->getLocalTransform() * imuLocalTransform_;
|
imuLocalTransform_ = this->getLocalTransform() * imuLocalTransform_;
|
||||||
@@ -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(!device_->getIrDrivers().empty())
|
||||||
if(!irDrivers.empty())
|
|
||||||
{
|
{
|
||||||
device_->setIrLaserDotProjectorBrightness(dotProjectormA_);
|
device_->setIrLaserDotProjectorIntensity(dotIntensity_);
|
||||||
device_->setIrFloodLightBrightness(floodLightmA_);
|
device_->setIrFloodLightIntensity(floodIntensity_);
|
||||||
}
|
}
|
||||||
|
|
||||||
uSleep(2000); // avoid bad frames on start
|
uSleep(2000); // avoid bad frames on start
|
||||||
@@ -577,16 +573,11 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
|
|||||||
SensorData data;
|
SensorData data;
|
||||||
#ifdef RTABMAP_DEPTHAI
|
#ifdef RTABMAP_DEPTHAI
|
||||||
|
|
||||||
|
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");
|
||||||
|
|
||||||
cv::Mat leftOrColor, depthOrRight;
|
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>();
|
|
||||||
|
|
||||||
double stamp = std::chrono::duration<double>(rectifLeftOrColor->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count();
|
|
||||||
if(device_->getDeviceInfo().protocol == X_LINK_TCP_IP || mxidOrName_.find(".") != std::string::npos)
|
if(device_->getDeviceInfo().protocol == X_LINK_TCP_IP || mxidOrName_.find(".") != std::string::npos)
|
||||||
{
|
{
|
||||||
leftOrColor = cv::imdecode(rectifLeftOrColor->getData(), cv::IMREAD_ANYCOLOR);
|
leftOrColor = cv::imdecode(rectifLeftOrColor->getData(), cv::IMREAD_ANYCOLOR);
|
||||||
@@ -604,6 +595,7 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
|
|||||||
depthOrRight = rectifRightOrDepth->getCvFrame();
|
depthOrRight = rectifRightOrDepth->getCvFrame();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
double stamp = std::chrono::duration<double>(rectifLeftOrColor->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count();
|
||||||
if(outputMode_)
|
if(outputMode_)
|
||||||
data = SensorData(leftOrColor, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);
|
data = SensorData(leftOrColor, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);
|
||||||
else
|
else
|
||||||
@@ -660,28 +652,30 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
|
|||||||
|
|
||||||
if(detectFeatures_ == 1)
|
if(detectFeatures_ == 1)
|
||||||
{
|
{
|
||||||
auto features = featuresQueue_->get<dai::TrackedFeatures>();
|
auto features = messageGroup->get<dai::TrackedFeatures>("feat")->trackedFeatures;
|
||||||
while(features->getSequenceNum() < rectifLeftOrColor->getSequenceNum())
|
|
||||||
features = featuresQueue_->get<dai::TrackedFeatures>();
|
|
||||||
auto detectedFeatures = features->trackedFeatures;
|
|
||||||
|
|
||||||
std::vector<cv::KeyPoint> keypoints;
|
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));
|
keypoints.emplace_back(cv::KeyPoint(feature.position.x, feature.position.y, 3));
|
||||||
data.setFeatures(keypoints, std::vector<cv::Point3f>(), cv::Mat());
|
data.setFeatures(keypoints, std::vector<cv::Point3f>(), cv::Mat());
|
||||||
}
|
}
|
||||||
else if(detectFeatures_ == 2)
|
else if(detectFeatures_ >= 2)
|
||||||
{
|
{
|
||||||
auto features = featuresQueue_->get<dai::NNData>();
|
auto features = messageGroup->get<dai::NNData>("feat");
|
||||||
while(features->getSequenceNum() < rectifLeftOrColor->getSequenceNum())
|
std::vector<float> scores_dense, local_descriptor_map, global_descriptor;
|
||||||
features = featuresQueue_->get<dai::NNData>();
|
if(detectFeatures_ == 2)
|
||||||
|
{
|
||||||
auto heatmap = features->getLayerFp16("heatmap");
|
scores_dense = features->getLayerFp16("heatmap");
|
||||||
auto desc = features->getLayerFp16("desc");
|
local_descriptor_map = features->getLayerFp16("desc");
|
||||||
|
}
|
||||||
cv::Mat scores(200, 320, CV_32FC1, heatmap.data());
|
else if(detectFeatures_ == 3)
|
||||||
cv::resize(scores, scores, targetSize_, 0, 0, cv::INTER_CUBIC);
|
{
|
||||||
|
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_)
|
if(nms_)
|
||||||
{
|
{
|
||||||
cv::Mat dilated_scores(targetSize_, CV_32FC1);
|
cv::Mat dilated_scores(targetSize_, CV_32FC1);
|
||||||
@@ -714,10 +708,11 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
|
|||||||
keypoints.emplace_back(cv::KeyPoint(kpt, 8, -1, response));
|
keypoints.emplace_back(cv::KeyPoint(kpt, 8, -1, response));
|
||||||
}
|
}
|
||||||
|
|
||||||
cv::Mat coarse_desc(25, 40, CV_32FC(256), desc.data());
|
cv::Mat coarse_desc(25, 40, CV_32FC(256), local_descriptor_map.data());
|
||||||
coarse_desc.forEach<cv::Vec<float, 256>>([&](cv::Vec<float, 256>& descriptor, const int position[]) -> void {
|
if(detectFeatures_ == 2)
|
||||||
cv::normalize(descriptor, descriptor);
|
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 mapX(keypoints.size(), 1, CV_32FC1);
|
||||||
cv::Mat mapY(keypoints.size(), 1, CV_32FC1);
|
cv::Mat mapY(keypoints.size(), 1, CV_32FC1);
|
||||||
for(size_t i=0; i<keypoints.size(); ++i)
|
for(size_t i=0; i<keypoints.size(); ++i)
|
||||||
@@ -734,6 +729,8 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
|
|||||||
descriptors = descriptors.reshape(1);
|
descriptors = descriptors.reshape(1);
|
||||||
|
|
||||||
data.setFeatures(keypoints, std::vector<cv::Point3f>(), descriptors);
|
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
|
#else
|
||||||
|
|||||||
@@ -817,8 +817,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
|
|||||||
connect(_ui->checkBox_depthai_use_spec_translation, SIGNAL(stateChanged(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->doubleSpinBox_depthai_laser_dot_brightness, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
|
connect(_ui->doubleSpinBox_depthai_dot_intensity, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
|
||||||
connect(_ui->doubleSpinBox_depthai_floodlight_brightness, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
|
connect(_ui->doubleSpinBox_depthai_flood_intensity, 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()));
|
||||||
connect(_ui->lineEdit_depthai_blob_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
|
connect(_ui->lineEdit_depthai_blob_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
|
||||||
connect(_ui->toolButton_depthai_blob_path, SIGNAL(clicked()), this, SLOT(selectSourceDepthaiBlobPath()));
|
connect(_ui->toolButton_depthai_blob_path, SIGNAL(clicked()), this, SLOT(selectSourceDepthaiBlobPath()));
|
||||||
@@ -2158,8 +2158,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
|
|||||||
_ui->checkBox_depthai_use_spec_translation->setChecked(false);
|
_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->doubleSpinBox_depthai_laser_dot_brightness->setValue(0.0);
|
_ui->doubleSpinBox_depthai_dot_intensity->setValue(0.0);
|
||||||
_ui->doubleSpinBox_depthai_floodlight_brightness->setValue(200.0);
|
_ui->doubleSpinBox_depthai_flood_intensity->setValue(0.0);
|
||||||
_ui->comboBox_depthai_detect_features->setCurrentIndex(0);
|
_ui->comboBox_depthai_detect_features->setCurrentIndex(0);
|
||||||
_ui->lineEdit_depthai_blob_path->clear();
|
_ui->lineEdit_depthai_blob_path->clear();
|
||||||
|
|
||||||
@@ -2651,8 +2651,8 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
|
|||||||
_ui->checkBox_depthai_use_spec_translation->setChecked(settings.value("use_spec_translation", _ui->checkBox_depthai_use_spec_translation->isChecked()).toBool());
|
_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->doubleSpinBox_depthai_laser_dot_brightness->setValue(settings.value("laser_dot_brightness", _ui->doubleSpinBox_depthai_laser_dot_brightness->value()).toDouble());
|
_ui->doubleSpinBox_depthai_dot_intensity->setValue(settings.value("dot_intensity", _ui->doubleSpinBox_depthai_dot_intensity->value()).toDouble());
|
||||||
_ui->doubleSpinBox_depthai_floodlight_brightness->setValue(settings.value("floodlight_brightness", _ui->doubleSpinBox_depthai_floodlight_brightness->value()).toDouble());
|
_ui->doubleSpinBox_depthai_flood_intensity->setValue(settings.value("flood_intensity", _ui->doubleSpinBox_depthai_flood_intensity->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());
|
||||||
_ui->lineEdit_depthai_blob_path->setText(settings.value("blob_path", _ui->lineEdit_depthai_blob_path->text()).toString());
|
_ui->lineEdit_depthai_blob_path->setText(settings.value("blob_path", _ui->lineEdit_depthai_blob_path->text()).toString());
|
||||||
settings.endGroup(); // DepthAI
|
settings.endGroup(); // DepthAI
|
||||||
@@ -3188,8 +3188,8 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
|
|||||||
settings.setValue("use_spec_translation", _ui->checkBox_depthai_use_spec_translation->isChecked());
|
settings.setValue("use_spec_translation", _ui->checkBox_depthai_use_spec_translation->isChecked());
|
||||||
settings.setValue("alpha_scaling", _ui->doubleSpinBox_depthai_alpha_scaling->value());
|
settings.setValue("alpha_scaling", _ui->doubleSpinBox_depthai_alpha_scaling->value());
|
||||||
settings.setValue("imu_published", _ui->checkBox_depthai_imu_published->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("dot_intensity", _ui->doubleSpinBox_depthai_dot_intensity->value());
|
||||||
settings.setValue("floodlight_brightness", _ui->doubleSpinBox_depthai_floodlight_brightness->value());
|
settings.setValue("flood_intensity", _ui->doubleSpinBox_depthai_flood_intensity->value());
|
||||||
settings.setValue("detect_features", _ui->comboBox_depthai_detect_features->currentIndex());
|
settings.setValue("detect_features", _ui->comboBox_depthai_detect_features->currentIndex());
|
||||||
settings.setValue("blob_path", _ui->lineEdit_depthai_blob_path->text());
|
settings.setValue("blob_path", _ui->lineEdit_depthai_blob_path->text());
|
||||||
settings.endGroup(); // DepthAI
|
settings.endGroup(); // DepthAI
|
||||||
@@ -6510,14 +6510,14 @@ Camera * PreferencesDialog::createCamera(
|
|||||||
((CameraDepthAI*)camera)->setDepthProfile(_ui->spinBox_depthai_conf_threshold->value(), _ui->spinBox_depthai_lrc_threshold->value());
|
((CameraDepthAI*)camera)->setDepthProfile(_ui->spinBox_depthai_conf_threshold->value(), _ui->spinBox_depthai_lrc_threshold->value());
|
||||||
((CameraDepthAI*)camera)->setRectification(_ui->checkBox_depthai_use_spec_translation->isChecked(), _ui->doubleSpinBox_depthai_alpha_scaling->value());
|
((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)->setIMU(_ui->checkBox_depthai_imu_published->isChecked(), _ui->checkbox_publishInterIMU->isChecked());
|
||||||
((CameraDepthAI*)camera)->setIrBrightness(_ui->doubleSpinBox_depthai_laser_dot_brightness->value(), _ui->doubleSpinBox_depthai_floodlight_brightness->value());
|
((CameraDepthAI*)camera)->setIrIntensity(_ui->doubleSpinBox_depthai_dot_intensity->value(), _ui->doubleSpinBox_depthai_flood_intensity->value());
|
||||||
((CameraDepthAI*)camera)->setDetectFeatures(_ui->comboBox_depthai_detect_features->currentIndex());
|
((CameraDepthAI*)camera)->setDetectFeatures(_ui->comboBox_depthai_detect_features->currentIndex());
|
||||||
((CameraDepthAI*)camera)->setBlobPath(_ui->lineEdit_depthai_blob_path->text().toStdString());
|
((CameraDepthAI*)camera)->setBlobPath(_ui->lineEdit_depthai_blob_path->text().toStdString());
|
||||||
if(_ui->comboBox_depthai_detect_features->currentIndex() == 1)
|
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());
|
((CameraDepthAI*)camera)->setGFTTDetector(_ui->checkBox_GFTT_useHarrisDetector->isChecked(), _ui->doubleSpinBox_GFTT_minDistance->value(), _ui->reextract_maxFeatures->value());
|
||||||
}
|
}
|
||||||
else if(_ui->comboBox_depthai_detect_features->currentIndex() == 2)
|
else if(_ui->comboBox_depthai_detect_features->currentIndex() >= 2)
|
||||||
{
|
{
|
||||||
((CameraDepthAI*)camera)->setSuperPointDetector(_ui->doubleSpinBox_sptorch_threshold->value(), _ui->checkBox_sptorch_nms->isChecked(), _ui->spinBox_sptorch_minDistance->value());
|
((CameraDepthAI*)camera)->setSuperPointDetector(_ui->doubleSpinBox_sptorch_threshold->value(), _ui->checkBox_sptorch_nms->isChecked(), _ui->spinBox_sptorch_minDistance->value());
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -5974,12 +5974,12 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
|
|||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
<item row="7" column="0">
|
<item row="7" column="0">
|
||||||
<widget class="QDoubleSpinBox" name="doubleSpinBox_depthai_laser_dot_brightness">
|
<widget class="QDoubleSpinBox" name="doubleSpinBox_depthai_dot_intensity">
|
||||||
<property name="suffix">
|
|
||||||
<string> mA</string>
|
|
||||||
</property>
|
|
||||||
<property name="maximum">
|
<property name="maximum">
|
||||||
<double>1200.000000000000000</double>
|
<double>1.000000000000000</double>
|
||||||
|
</property>
|
||||||
|
<property name="singleStep">
|
||||||
|
<double>0.050000000000000</double>
|
||||||
</property>
|
</property>
|
||||||
<property name="value">
|
<property name="value">
|
||||||
<double>0.000000000000000</double>
|
<double>0.000000000000000</double>
|
||||||
@@ -5996,7 +5996,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
|
|||||||
<item row="8" column="1">
|
<item row="8" column="1">
|
||||||
<widget class="QLabel" name="label_677">
|
<widget class="QLabel" name="label_677">
|
||||||
<property name="text">
|
<property name="text">
|
||||||
<string>Floodlight brightness.</string>
|
<string>IR flood light intensity.</string>
|
||||||
</property>
|
</property>
|
||||||
<property name="wordWrap">
|
<property name="wordWrap">
|
||||||
<bool>true</bool>
|
<bool>true</bool>
|
||||||
@@ -6009,7 +6009,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
|
|||||||
<item row="7" column="1">
|
<item row="7" column="1">
|
||||||
<widget class="QLabel" name="label_676">
|
<widget class="QLabel" name="label_676">
|
||||||
<property name="text">
|
<property name="text">
|
||||||
<string>Laser dot brightness.</string>
|
<string>IR laser dot projector intensity.</string>
|
||||||
</property>
|
</property>
|
||||||
<property name="wordWrap">
|
<property name="wordWrap">
|
||||||
<bool>true</bool>
|
<bool>true</bool>
|
||||||
@@ -6143,15 +6143,15 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
|
|||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
<item row="8" column="0">
|
<item row="8" column="0">
|
||||||
<widget class="QDoubleSpinBox" name="doubleSpinBox_depthai_floodlight_brightness">
|
<widget class="QDoubleSpinBox" name="doubleSpinBox_depthai_flood_intensity">
|
||||||
<property name="suffix">
|
|
||||||
<string> mA</string>
|
|
||||||
</property>
|
|
||||||
<property name="maximum">
|
<property name="maximum">
|
||||||
<double>1500.000000000000000</double>
|
<double>1.000000000000000</double>
|
||||||
|
</property>
|
||||||
|
<property name="singleStep">
|
||||||
|
<double>0.050000000000000</double>
|
||||||
</property>
|
</property>
|
||||||
<property name="value">
|
<property name="value">
|
||||||
<double>200.000000000000000</double>
|
<double>0.000000000000000</double>
|
||||||
</property>
|
</property>
|
||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
@@ -6178,6 +6178,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
|
|||||||
<string>SuperPoint</string>
|
<string>SuperPoint</string>
|
||||||
</property>
|
</property>
|
||||||
</item>
|
</item>
|
||||||
|
<item>
|
||||||
|
<property name="text">
|
||||||
|
<string>HF-Net</string>
|
||||||
|
</property>
|
||||||
|
</item>
|
||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
<item row="9" column="1">
|
<item row="9" column="1">
|
||||||
@@ -6220,7 +6225,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
|
|||||||
<item row="10" column="1">
|
<item row="10" column="1">
|
||||||
<widget class="QLabel" name="label_680">
|
<widget class="QLabel" name="label_680">
|
||||||
<property name="text">
|
<property name="text">
|
||||||
<string>Path to SuperPoint blob file.</string>
|
<string>Path to MyriadX blob file.</string>
|
||||||
</property>
|
</property>
|
||||||
<property name="wordWrap">
|
<property name="wordWrap">
|
||||||
<bool>true</bool>
|
<bool>true</bool>
|
||||||
|
|||||||
Reference in New Issue
Block a user