i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

add depthai superpoint detector

Browse Source
This commit is contained in:
Borong Yuan
2023-06-17 16:53:25 +08:00
parent 2dd64a6283
commit 55a84cfdaa
5 changed files with 178 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

109
corelib/src/camera/CameraDepthAI.cpp
View File

@@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/camera/CameraDepthAI.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UThread.h>
#include <rtabmap/utilite/UEventsManager.h>
@@ -65,7 +66,10 @@ CameraDepthAI::CameraDepthAI(
detectFeatures_(0),
useHarrisDetector_(false),
minDistance_(7.0),
numTargetFeatures_(1000)
numTargetFeatures_(1000),
threshold_(0.01),
nms_(true),
nmsRadius_(4)
#endif
{
#ifdef RTABMAP_DEPTHAI
@@ -159,7 +163,16 @@ void CameraDepthAI::setDetectFeatures(int detectFeatures)
#endif
}
void CameraDepthAI::setGFTTDetector(bool useHarrisDetector, double minDistance, int numTargetFeatures)
void CameraDepthAI::setBlobPath(const std::string & blobPath)
{
#ifdef RTABMAP_DEPTHAI
blobPath_ = blobPath;
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
}
void CameraDepthAI::setGFTTDetector(bool useHarrisDetector, float minDistance, int numTargetFeatures)
{
#ifdef RTABMAP_DEPTHAI
useHarrisDetector_ = useHarrisDetector;
@@ -170,6 +183,17 @@ void CameraDepthAI::setGFTTDetector(bool useHarrisDetector, double minDistance,
#endif
}
void CameraDepthAI::setSuperPointDetector(float threshold, bool nms, int nmsRadius)
{
#ifdef RTABMAP_DEPTHAI
threshold_ = threshold;
nms_ = nms;
nmsRadius_ = nmsRadius;
#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("");
@@ -213,7 +237,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
// look for calibration files
stereoModel_ = StereoCameraModel();
cv::Size targetSize(resolution_<2?1280:resolution_==4?1920:640, resolution_==0?720:resolution_==1?800:resolution_==2?400:resolution_==3?480:1200);
targetSize_ = cv::Size(resolution_<2?1280:resolution_==4?1920:640, resolution_==0?720:resolution_==1?800:resolution_==2?400:resolution_==3?480:1200);
dai::Pipeline p;
auto monoLeft = p.create<dai::node::MonoCamera>();
@@ -223,8 +247,25 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
if(imuPublished_)
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;
if(detectFeatures_ == 1)
{
gfttDetector = p.create<dai::node::FeatureTracker>();
}
else if(detectFeatures_ == 2)
{
if(!blobPath_.empty())
{
manip = p.create<dai::node::ImageManip>();
superPointNetwork = p.create<dai::node::NeuralNetwork>();
}
else
{
UWARN("Missing SuperPoint blob file!");
detectFeatures_ = 0;
}
}
auto xoutLeft = p.create<dai::node::XLinkOut>();
auto xoutDepthOrRight = p.create<dai::node::XLinkOut>();
@@ -248,7 +289,16 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
monoLeft->setCamera("left");
monoRight->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
monoRight->setCamera("right");
if(this->getImageRate()>0)
if(detectFeatures_ == 2)
{
if(this->getImageRate() <= 0 || this->getImageRate() > 15)
{
UWARN("On-device SuperPoint enabled, image rate is limited to 15 FPS!");
monoLeft->setFps(15);
monoRight->setFps(15);
}
}
else if(this->getImageRate() > 0)
{
monoLeft->setFps(this->getImageRate());
monoRight->setFps(this->getImageRate());
@@ -311,6 +361,19 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
stereo->rectifiedLeft.link(gfttDetector->inputImage);
gfttDetector->outputFeatures.link(xoutFeatures->input);
}
else if(detectFeatures_ == 2)
{
manip->setKeepAspectRatio(false);
manip->setMaxOutputFrameSize(320 * 200);
manip->initialConfig.setResize(320, 200);
superPointNetwork->setBlobPath(blobPath_);
superPointNetwork->setNumInferenceThreads(1);
superPointNetwork->setNumNCEPerInferenceThread(2);
superPointNetwork->input.setBlocking(false);
stereo->rectifiedLeft.link(manip->inputImage);
manip->out.link(superPointNetwork->input);
superPointNetwork->out.link(xoutFeatures->input);
}
device_.reset(new dai::Device(p, deviceToUse));
@@ -319,7 +382,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
cv::Mat cameraMatrix, distCoeffs, new_camera_matrix;
std::vector<std::vector<float> > matrix = calibHandler.getCameraIntrinsics(dai::CameraBoardSocket::CAM_B, dai::Size2f(targetSize.width, targetSize.height));
std::vector<std::vector<float> > matrix = calibHandler.getCameraIntrinsics(dai::CameraBoardSocket::CAM_B, dai::Size2f(targetSize_.width, targetSize_.height));
cameraMatrix = (cv::Mat_<double>(3,3) <<
matrix[0][0], matrix[0][1], matrix[0][2],
matrix[1][0], matrix[1][1], matrix[1][2],
@@ -330,7 +393,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
distCoeffs = (cv::Mat_<double>(1,8) << coeffs[0], coeffs[1], coeffs[2], coeffs[3], coeffs[4], coeffs[5], coeffs[6], coeffs[7]);
if(alphaScaling_>-1.0f) {
new_camera_matrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, targetSize, alphaScaling_);
new_camera_matrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, targetSize_, alphaScaling_);
}
else {
new_camera_matrix = cameraMatrix;
@@ -342,7 +405,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
double cy = new_camera_matrix.at<double>(1, 2);
double baseline = calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_C, dai::CameraBoardSocket::CAM_B, false)/100.0;
UINFO("left: fx=%f fy=%f cx=%f cy=%f baseline=%f", fx, fy, cx, cy, baseline);
stereoModel_ = StereoCameraModel(device_->getMxId(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize);
stereoModel_ = StereoCameraModel(device_->getMxId(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize_);
if(imuPublished_)
{
@@ -536,6 +599,38 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
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)
{
auto features = featuresQueue_->get<dai::NNData>();
while(features->getSequenceNum() < rectifL->getSequenceNum())
features = featuresQueue_->get<dai::NNData>();
auto heatmap = features->getLayerFp16("heatmap");
auto desc = features->getLayerFp16("desc");
cv::Mat prob(200, 320, CV_32FC1, heatmap.data());
cv::resize(prob, prob, targetSize_, 0, 0, cv::INTER_CUBIC);
std::vector<cv::Point> kpts;
cv::findNonZero(prob > threshold_, kpts);
std::vector<cv::KeyPoint> keypoints_no_nms, keypoints;
for(auto& kpt : kpts)
{
float response = prob.at<float>(kpt);
keypoints_no_nms.emplace_back(cv::KeyPoint(kpt, 8, -1, response));
}
if(nms_ && !keypoints_no_nms.empty())
{
cv::Mat descEmpty;
util2d::NMS(keypoints_no_nms, descEmpty, keypoints, descEmpty, 0, nmsRadius_, targetSize_.width, targetSize_.height);
}
else if(!keypoints_no_nms.empty())
{
keypoints = keypoints_no_nms;
}
data.setFeatures(keypoints, std::vector<cv::Point3f>(), cv::Mat());
}
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");