i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-11 14:00:20 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
8fa82faf9474e96352dddb85b50663ec3a6acada
rtabmap/corelib/src/camera/CameraDepthAI.cpp
T
Borong Yuan d88353dc1b Further improvements to DepthAI integration (#1458) 
* Reorganize DepthAI params

* Advanced Tuning of DepthAI’s SGBM Pipeline

* Update IMU Local Transform for OAK-D LR, OAK-D SR, and PoE Devices

* Reorganiz the usage of DepthAI. Get params before creating the pipeline, and automatically determine the resolution of different models.

* Identify the IMU type to specifically handle BMI270 and BNO085/086
2025-03-08 11:30:11 -08:00

884 lines
32 KiB
C++
Raw Blame History

/*
Copyright (c) 2010-2021, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
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>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UFile.h>
namespace rtabmap {
bool CameraDepthAI::available()
{
#ifdef RTABMAP_DEPTHAI
return true;
#else
return false;
#endif
}
CameraDepthAI::CameraDepthAI(
const std::string & mxidOrName,
int imageWidth,
float imageRate,
const Transform & localTransform) :
Camera(imageRate, localTransform)
#ifdef RTABMAP_DEPTHAI
,
mxidOrName_(mxidOrName),
outputMode_(0),
confThreshold_(200),
lrcThreshold_(5),
imageWidth_(imageWidth),
extendedDisparity_(false),
enableCompanding_(false),
subpixelFractionalBits_(3),
disparityWidth_(1),
medianFilter_(5),
useSpecTranslation_(false),
alphaScaling_(0.0),
imagesRectified_(true),
imuPublished_(true),
publishInterIMU_(false),
dotIntensity_(0.0),
floodIntensity_(0.0),
detectFeatures_(0),
useHarrisDetector_(false),
minDistance_(7.0),
numTargetFeatures_(320),
threshold_(0.01),
nms_(true),
nmsRadius_(4)
#endif
{
#ifdef RTABMAP_DEPTHAI
UASSERT(imageWidth_ == 640 || imageWidth_ == 1280);
if(this->getImageRate() <= 0)
this->setImageRate(30);
#endif
}
CameraDepthAI::~CameraDepthAI()
{
#ifdef RTABMAP_DEPTHAI
if(device_.get())
device_->close();
#endif
}
void CameraDepthAI::setOutputMode(int outputMode)
{
#ifdef RTABMAP_DEPTHAI
outputMode_ = outputMode;
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
}
void CameraDepthAI::setDepthProfile(int confThreshold, int lrcThreshold)
{
#ifdef RTABMAP_DEPTHAI
confThreshold_ = confThreshold;
lrcThreshold_ = lrcThreshold;
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
}
void CameraDepthAI::setExtendedDisparity(bool extendedDisparity, bool enableCompanding)
{
#ifdef RTABMAP_DEPTHAI
extendedDisparity_ = extendedDisparity;
enableCompanding_ = enableCompanding;
if(extendedDisparity_ && enableCompanding_)
{
UWARN("Extended disparity has been enabled while companding being also enabled, disabling companding...");
enableCompanding_ = false;
}
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
}
void CameraDepthAI::setSubpixelMode(bool enabled, int fractionalBits)
{
#ifdef RTABMAP_DEPTHAI
UASSERT(fractionalBits>=3 && fractionalBits<=5);
subpixelFractionalBits_ = enabled?fractionalBits:0;
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
}
void CameraDepthAI::setDisparityWidthAndFilter(int disparityWidth, int medianFilter)
{
#ifdef RTABMAP_DEPTHAI
UASSERT(disparityWidth == 64 || disparityWidth == 96);
disparityWidth_ = disparityWidth;
medianFilter_ = medianFilter;
int maxDisp = (extendedDisparity_?2:1) * std::pow(2,subpixelFractionalBits_) * (disparityWidth_-1);
if(medianFilter_ && maxDisp > 1024)
{
UWARN("Maximum disparity value '%d' exceeds the maximum supported '1024' by median filter, disabling median filter...", maxDisp);
medianFilter_ = 0;
}
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
}
void CameraDepthAI::setRectification(bool useSpecTranslation, float alphaScaling, bool enabled)
{
#ifdef RTABMAP_DEPTHAI
useSpecTranslation_ = useSpecTranslation;
alphaScaling_ = alphaScaling;
imagesRectified_ = enabled;
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
}
void CameraDepthAI::setIMU(bool imuPublished, bool publishInterIMU)
{
#ifdef RTABMAP_DEPTHAI
imuPublished_ = imuPublished;
publishInterIMU_ = publishInterIMU;
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
}
void CameraDepthAI::setIrIntensity(float dotIntensity, float floodIntensity)
{
#ifdef RTABMAP_DEPTHAI
dotIntensity_ = dotIntensity;
floodIntensity_ = floodIntensity;
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
}
void CameraDepthAI::setDetectFeatures(int detectFeatures, const std::string & blobPath)
{
#ifdef RTABMAP_DEPTHAI
detectFeatures_ = detectFeatures;
blobPath_ = blobPath;
if(detectFeatures_ && outputMode_==2)
{
UWARN("On-device feature detectors cannot be enabled on color camera input, disabling on-device feature detector...");
detectFeatures_ = 0;
}
if(detectFeatures_>=2 && blobPath_.empty())
{
UWARN("Missing MyriadX blob file, disabling on-device feature detector");
detectFeatures_ = 0;
}
if(detectFeatures_>=2 && this->getImageRate()>15)
{
UWARN("On-device SuperPoint or HF-Net enabled, image rate is limited to 15 FPS!");
this->setImageRate(15);
}
#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;
minDistance_ = minDistance;
numTargetFeatures_ = numTargetFeatures;
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#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("");
#ifdef RTABMAP_DEPTHAI
std::vector<dai::DeviceInfo> devices = dai::Device::getAllAvailableDevices();
if(devices.empty() && mxidOrName_.empty())
{
UERROR("No DepthAI device found or specified");
return false;
}
accBuffer_.clear();
gyroBuffer_.clear();
bool deviceFound = false;
dai::DeviceInfo deviceToUse(mxidOrName_);
if(mxidOrName_.empty())
std::tie(deviceFound, deviceToUse) = dai::Device::getFirstAvailableDevice();
else if(!deviceToUse.mxid.empty())
std::tie(deviceFound, deviceToUse) = dai::Device::getDeviceByMxId(deviceToUse.mxid);
else
deviceFound = true;
if(!deviceFound)
{
UERROR("Could not find DepthAI device with MXID or IP/USB name \"%s\", found devices:", mxidOrName_.c_str());
for(auto& device : devices)
UERROR("%s", device.toString().c_str());
return false;
}
device_ = std::make_unique<dai::Device>(deviceToUse);
auto deviceName = device_->getDeviceName();
auto imuType = device_->getConnectedIMU();
UINFO("Device Name: %s, Device Serial: %s", deviceName.c_str(), device_->getMxId().c_str());
UINFO("Available Camera Sensors: ");
for(auto& sensor : device_->getCameraSensorNames()) {
UINFO("Socket: CAM_%c - %s", 'A'+(unsigned char)sensor.first, sensor.second.c_str());
}
UINFO("IMU Type: %s", imuType.c_str());
UINFO("Loading eeprom calibration data");
auto calibHandler = device_->readCalibration();
auto boardName = calibHandler.getEepromData().boardName;
stereoModel_ = StereoCameraModel();
targetSize_ = cv::Size(imageWidth_, imageWidth_/640*((outputMode_==2&&boardName!="BC2087")?360:400));
if(!calibrationFolder.empty() && !cameraName.empty() && imagesRectified_)
{
UINFO("Flashing camera...");
if(outputMode_ == 2)
{
stereoModel_.setName(cameraName, "rgb", "depth");
}
if(stereoModel_.load(calibrationFolder, cameraName, false))
{
std::vector<std::vector<float> > intrinsicsLeft(3);
std::vector<std::vector<float> > intrinsicsRight(3);
for(int row = 0; row<3; ++row)
{
intrinsicsLeft[row].resize(3);
intrinsicsRight[row].resize(3);
for(int col = 0; col<3; ++col)
{
intrinsicsLeft[row][col] = stereoModel_.left().K_raw().at<double>(row,col);
intrinsicsRight[row][col] = stereoModel_.right().K_raw().at<double>(row,col);
}
}
std::vector<float> distortionsLeft = stereoModel_.left().D_raw();
std::vector<float> distortionsRight = stereoModel_.right().D_raw();
std::vector<std::vector<float> > rotationMatrix(3);
for(int row = 0; row<3; ++row)
{
rotationMatrix[row].resize(3);
for(int col = 0; col<3; ++col)
{
rotationMatrix[row][col] = stereoModel_.stereoTransform()(row,col);
}
}
std::vector<float> translation(3);
translation[0] = stereoModel_.stereoTransform().x()*100.0f;
translation[1] = stereoModel_.stereoTransform().y()*100.0f;
translation[2] = stereoModel_.stereoTransform().z()*100.0f;
if(outputMode_ == 2)
{
// Only set RGB intrinsics
calibHandler.setCameraIntrinsics(dai::CameraBoardSocket::CAM_A, intrinsicsLeft, stereoModel_.left().imageWidth(), stereoModel_.left().imageHeight());
calibHandler.setDistortionCoefficients(dai::CameraBoardSocket::CAM_A, distortionsLeft);
std::vector<float> specTranslation = calibHandler.getCameraTranslationVector(dai::CameraBoardSocket::CAM_A, dai::CameraBoardSocket::CAM_C, true);
calibHandler.setCameraExtrinsics(dai::CameraBoardSocket::CAM_A, dai::CameraBoardSocket::CAM_C, rotationMatrix, translation, specTranslation);
}
else
{
calibHandler.setCameraIntrinsics(dai::CameraBoardSocket::CAM_B, intrinsicsLeft, stereoModel_.left().imageWidth(), stereoModel_.left().imageHeight());
calibHandler.setDistortionCoefficients(dai::CameraBoardSocket::CAM_B, distortionsLeft);
calibHandler.setCameraIntrinsics(dai::CameraBoardSocket::CAM_C, intrinsicsRight, stereoModel_.right().imageWidth(), stereoModel_.right().imageHeight());
calibHandler.setDistortionCoefficients(dai::CameraBoardSocket::CAM_C, distortionsRight);
std::vector<float> specTranslation = calibHandler.getCameraTranslationVector(dai::CameraBoardSocket::CAM_B, dai::CameraBoardSocket::CAM_C, true);
calibHandler.setCameraExtrinsics(dai::CameraBoardSocket::CAM_B, dai::CameraBoardSocket::CAM_C, rotationMatrix, translation, specTranslation);
}
try {
UINFO("Flashing camera with calibration from %s with camera name %s", calibrationFolder.c_str(), cameraName.c_str());
if(ULogger::level() <= ULogger::kInfo)
{
std::cout << "K left: " << stereoModel_.left().K_raw() << std::endl;
std::cout << "K right: " << stereoModel_.right().K_raw() << std::endl;
std::cout << "D left: " << stereoModel_.left().D_raw() << std::endl;
std::cout << "D right: " << stereoModel_.right().D_raw() << std::endl;
std::cout << "Extrinsics: " << stereoModel_.stereoTransform() << std::endl;
std::cout << "Expected K with rectification_alpha=0: " << stereoModel_.left().K()*(double(targetSize_.width)/double(stereoModel_.left().imageWidth())) << std::endl;
}
device_->flashCalibration2(calibHandler);
}
catch(const std::runtime_error & e) {
UERROR("Failed flashing calibration: %s", e.what());
}
}
else
{
UERROR("Failed loading calibration from %s with camera name %s", calibrationFolder.c_str(), cameraName.c_str());
}
//Reload calibration
calibHandler = device_->readCalibration();
}
auto cameraId = outputMode_==2?dai::CameraBoardSocket::CAM_A:dai::CameraBoardSocket::CAM_B;
cv::Mat cameraMatrix, distCoeffs, newCameraMatrix;
std::vector<std::vector<float> > matrix = calibHandler.getCameraIntrinsics(cameraId, 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],
matrix[2][0], matrix[2][1], matrix[2][2]);
std::vector<float> coeffs = calibHandler.getDistortionCoefficients(cameraId);
if(calibHandler.getDistortionModel(cameraId) == dai::CameraModel::Perspective)
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)
newCameraMatrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, targetSize_, alphaScaling_);
else
newCameraMatrix = cameraMatrix;
double fx = newCameraMatrix.at<double>(0, 0);
double fy = newCameraMatrix.at<double>(1, 1);
double cx = newCameraMatrix.at<double>(0, 2);
double cy = newCameraMatrix.at<double>(1, 2);
UINFO("fx=%f fy=%f cx=%f cy=%f (target size = %dx%d)", fx, fy, cx, cy, targetSize_.width, targetSize_.height);
if(outputMode_ == 2) {
stereoModel_ = StereoCameraModel(deviceName, fx, fy, cx, cy, 0, this->getLocalTransform(), targetSize_);
}
else {
double baseline = calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_C, dai::CameraBoardSocket::CAM_B, false)/100.0;
UINFO("baseline=%f", baseline);
stereoModel_ = StereoCameraModel(deviceName, fx, fy, cx, cy, outputMode_==0?baseline:0, this->getLocalTransform()*Transform(-calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_A)/100.0, 0, 0), targetSize_);
}
if(imuPublished_ || imuType.empty())
{
// Cannot test the following, I get "IMU calibration data is not available on device yet." with my camera
// Update: now (as March 6, 2022) it crashes in "dai::CalibrationHandler::getImuToCameraExtrinsics(dai::CameraBoardSocket, bool)"
//matrix = calibHandler.getImuToCameraExtrinsics(dai::CameraBoardSocket::CAM_B);
//imuLocalTransform_ = Transform(
// matrix[0][0], matrix[0][1], matrix[0][2], matrix[0][3],
// matrix[1][0], matrix[1][1], matrix[1][2], matrix[1][3],
// matrix[2][0], matrix[2][1], matrix[2][2], matrix[2][3]);
if(deviceName == "OAK-D")
{
imuLocalTransform_ = Transform(
0, -1, 0, 0.0525,
1, 0, 0, 0.013662,
0, 0, 1, 0);
}
else if(boardName == "BC2087") // OAK-D LR
{
imuLocalTransform_ = Transform(
1, 0, 0, 0.021425,
0, 1, 0, 0.009925,
0, 0, 1, 0);
}
else if(boardName == "DM2080") // OAK-D SR
{
imuLocalTransform_ = Transform(
-1, 0, 0, 0,
0, -1, 0, -0.0024,
0, 0, 1, 0);
}
else if(boardName == "DM9098") // OAK-D S2, OAK-D W, OAK-D Pro, OAK-D Pro W
{
imuLocalTransform_ = Transform(
0, 1, 0, 0.037945,
1, 0, 0, 0.00079,
0, 0, -1, 0);
}
else if(boardName == "NG2094") // OAK-D Pro W Dev
{
imuLocalTransform_ = Transform(
0, 1, 0, 0.0374,
1, 0, 0, 0.00176,
0, 0, -1, 0);
}
else if(boardName == "NG9097") // OAK-D S2 PoE, OAK-D W PoE, OAK-D Pro PoE, OAK-D Pro W PoE
{
if(imuType == "BMI270")
{
imuLocalTransform_ = Transform(
0, 1, 0, 0.04,
1, 0, 0, 0.020265,
0, 0, -1, 0);
}
else // BNO085/086
{
imuLocalTransform_ = Transform(
0, -1, 0, 0.04,
-1, 0, 0, 0.020265,
0, 0, -1, 0);
}
}
else
{
UWARN("Unsupported boardName (%s)! Disabling IMU!", boardName.c_str());
imuPublished_ = false;
}
}
else
{
UINFO("IMU disabled");
imuPublished_ = false;
}
dai::Pipeline pipeline;
auto sync = pipeline.create<dai::node::Sync>();
sync->setSyncThreshold(std::chrono::milliseconds(int(500 / this->getImageRate())));
if(outputMode_ == 2)
{
auto rgbCamera = pipeline.create<dai::node::Camera>();
rgbCamera->setCamera("color");
if(boardName == "BC2087")
rgbCamera->setSize(1920, 1200);
else if(boardName == "NG2094")
rgbCamera->setSize(1280, 720);
else
rgbCamera->setSize(1920, 1080);
rgbCamera->setSize(1920, 1200);
rgbCamera->setVideoSize(targetSize_.width, targetSize_.height);
rgbCamera->setPreviewSize(targetSize_.width, targetSize_.height);
rgbCamera->setFps(this->getImageRate());
rgbCamera->setMeshSource(imagesRectified_?dai::CameraProperties::WarpMeshSource::CALIBRATION:dai::CameraProperties::WarpMeshSource::NONE);
if(imagesRectified_ && alphaScaling_>-1.0f)
rgbCamera->setCalibrationAlpha(alphaScaling_);
rgbCamera->properties.ispScale.horizNumerator = rgbCamera->properties.ispScale.vertNumerator = imageWidth_/640;
rgbCamera->properties.ispScale.horizDenominator = rgbCamera->properties.ispScale.vertDenominator = boardName=="NG2094"?2:3;
auto rgbEncoder = pipeline.create<dai::node::VideoEncoder>();
rgbEncoder->setDefaultProfilePreset(this->getImageRate(), dai::VideoEncoderProperties::Profile::MJPEG);
rgbCamera->video.link(rgbEncoder->input);
rgbEncoder->bitstream.link(sync->inputs["rgb"]);
}
auto stereoDepth = pipeline.create<dai::node::StereoDepth>();
if(outputMode_ == 2)
stereoDepth->setDepthAlign(dai::CameraBoardSocket::CAM_A);
else
stereoDepth->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT);
if(subpixelFractionalBits_>=3 && subpixelFractionalBits_<=5)
{
stereoDepth->setSubpixel(true);
stereoDepth->setSubpixelFractionalBits(subpixelFractionalBits_);
}
stereoDepth->setExtendedDisparity(extendedDisparity_);
stereoDepth->enableDistortionCorrection(true);
stereoDepth->setDisparityToDepthUseSpecTranslation(useSpecTranslation_);
stereoDepth->setDepthAlignmentUseSpecTranslation(useSpecTranslation_);
if(alphaScaling_ > -1.0f)
stereoDepth->setAlphaScaling(alphaScaling_);
stereoDepth->initialConfig.setConfidenceThreshold(confThreshold_);
stereoDepth->initialConfig.setLeftRightCheck(lrcThreshold_>=0);
if(lrcThreshold_>=0)
stereoDepth->initialConfig.setLeftRightCheckThreshold(lrcThreshold_);
stereoDepth->initialConfig.setMedianFilter(dai::MedianFilter(medianFilter_));
auto config = stereoDepth->initialConfig.get();
config.censusTransform.kernelSize = dai::StereoDepthConfig::CensusTransform::KernelSize::KERNEL_7x9;
config.censusTransform.kernelMask = 0X5092A28C5152428;
config.costMatching.disparityWidth = disparityWidth_==64?dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64:dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_96;
config.costMatching.enableCompanding = enableCompanding_;
config.costMatching.linearEquationParameters.alpha = 2;
config.costMatching.linearEquationParameters.beta = 4;
config.costAggregation.horizontalPenaltyCostP1 = 100;
config.costAggregation.horizontalPenaltyCostP2 = 500;
config.costAggregation.verticalPenaltyCostP1 = 100;
config.costAggregation.verticalPenaltyCostP2 = 500;
config.postProcessing.brightnessFilter.maxBrightness = 255;
stereoDepth->initialConfig.set(config);
stereoDepth->depth.link(sync->inputs["depth"]);
if(outputMode_ < 2)
{
auto leftEncoder = pipeline.create<dai::node::VideoEncoder>();
leftEncoder->setDefaultProfilePreset(this->getImageRate(), dai::VideoEncoderProperties::Profile::MJPEG);
if(imagesRectified_)
stereoDepth->rectifiedLeft.link(leftEncoder->input);
else
stereoDepth->syncedLeft.link(leftEncoder->input);
leftEncoder->bitstream.link(sync->inputs["left"]);
}
if(!outputMode_)
{
auto rightEncoder = pipeline.create<dai::node::VideoEncoder>();
rightEncoder->setDefaultProfilePreset(this->getImageRate(), dai::VideoEncoderProperties::Profile::MJPEG);
if(imagesRectified_)
stereoDepth->rectifiedRight.link(rightEncoder->input);
else
stereoDepth->syncedRight.link(rightEncoder->input);
rightEncoder->bitstream.link(sync->inputs["right"]);
}
if(boardName == "BC2087")
{
auto leftCamera = pipeline.create<dai::node::ColorCamera>();
leftCamera->setCamera("left");
leftCamera->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1200_P);
leftCamera->setIspScale(imageWidth_/640, 3);
leftCamera->setFps(this->getImageRate());
auto rightCamera = pipeline.create<dai::node::ColorCamera>();
rightCamera->setCamera("right");
rightCamera->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1200_P);
rightCamera->setIspScale(imageWidth_/640, 3);
rightCamera->setFps(this->getImageRate());
leftCamera->isp.link(stereoDepth->left);
rightCamera->isp.link(stereoDepth->right);
}
else
{
auto leftCamera = pipeline.create<dai::node::MonoCamera>();
leftCamera->setCamera("left");
leftCamera->setResolution(imageWidth_==640?dai::MonoCameraProperties::SensorResolution::THE_400_P:dai::MonoCameraProperties::SensorResolution::THE_800_P);
leftCamera->setFps(this->getImageRate());
auto rightCamera = pipeline.create<dai::node::MonoCamera>();
rightCamera->setCamera("right");
rightCamera->setResolution(imageWidth_==640?dai::MonoCameraProperties::SensorResolution::THE_400_P:dai::MonoCameraProperties::SensorResolution::THE_800_P);
rightCamera->setFps(this->getImageRate());
leftCamera->out.link(stereoDepth->left);
rightCamera->out.link(stereoDepth->right);
}
if(detectFeatures_ == 1)
{
auto gfttDetector = pipeline.create<dai::node::FeatureTracker>();
gfttDetector->setHardwareResources(2, 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);
if(imagesRectified_)
stereoDepth->rectifiedLeft.link(gfttDetector->inputImage);
else
stereoDepth->syncedLeft.link(gfttDetector->inputImage);
gfttDetector->outputFeatures.link(sync->inputs["feat"]);
}
else if(detectFeatures_ >= 2)
{
auto imageManip = pipeline.create<dai::node::ImageManip>();
imageManip->setKeepAspectRatio(false);
imageManip->setMaxOutputFrameSize(320 * 200);
imageManip->initialConfig.setResize(320, 200);
auto neuralNetwork = pipeline.create<dai::node::NeuralNetwork>();
neuralNetwork->setBlobPath(blobPath_);
neuralNetwork->setNumInferenceThreads(2);
neuralNetwork->setNumNCEPerInferenceThread(1);
neuralNetwork->input.setBlocking(false);
if(imagesRectified_)
stereoDepth->rectifiedLeft.link(imageManip->inputImage);
else
stereoDepth->syncedLeft.link(imageManip->inputImage);
imageManip->out.link(neuralNetwork->input);
neuralNetwork->out.link(sync->inputs["feat"]);
}
auto xoutCamera = pipeline.create<dai::node::XLinkOut>();
xoutCamera->setStreamName("camera");
sync->out.link(xoutCamera->input);
if(imuPublished_)
{
auto imu = pipeline.create<dai::node::IMU>();
if(imuType == "BMI270")
imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 200);
else // BNO085/086
imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER, dai::IMUSensor::GYROSCOPE_UNCALIBRATED}, 200);
imu->setBatchReportThreshold(boardName=="NG9097"?4:1);
imu->setMaxBatchReports(10);
auto xoutIMU = pipeline.create<dai::node::XLinkOut>();
xoutIMU->setStreamName("imu");
imu->out.link(xoutIMU->input);
}
device_->startPipeline(pipeline);
if(!device_->getIrDrivers().empty())
{
UINFO("Setting IR intensity");
device_->setIrLaserDotProjectorIntensity(dotIntensity_);
device_->setIrFloodLightIntensity(floodIntensity_);
}
else if(dotIntensity_ > 0 || floodIntensity_ > 0)
{
UWARN("No IR drivers were detected! IR intensity cannot be set.");
}
cameraQueue_ = device_->getOutputQueue("camera", 8, false);
if(imuPublished_)
{
imuLocalTransform_ = this->getLocalTransform() * imuLocalTransform_;
UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str());
device_->getOutputQueue("imu", 50, false)->addCallback([this](const std::shared_ptr<dai::ADatatype> data) {
auto imuData = std::dynamic_pointer_cast<dai::IMUData>(data);
auto imuPackets = imuData->packets;
for(auto& imuPacket : imuPackets)
{
auto& acceleroValues = imuPacket.acceleroMeter;
auto& gyroValues = imuPacket.gyroscope;
double accStamp = std::chrono::duration<double>(acceleroValues.getTimestampDevice().time_since_epoch()).count();
double gyroStamp = std::chrono::duration<double>(gyroValues.getTimestampDevice().time_since_epoch()).count();
if(publishInterIMU_)
{
IMU imu(cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z), cv::Mat::eye(3,3,CV_64FC1),
cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z), cv::Mat::eye(3,3,CV_64FC1),
imuLocalTransform_);
UEventsManager::post(new IMUEvent(imu, (accStamp+gyroStamp)/2));
}
else
{
UScopeMutex lock(imuMutex_);
accBuffer_.emplace_hint(accBuffer_.end(), accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z));
gyroBuffer_.emplace_hint(gyroBuffer_.end(), gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z));
}
}
});
}
uSleep(2000); // avoid bad frames on start
return true;
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
return false;
}
bool CameraDepthAI::isCalibrated() const
{
#ifdef RTABMAP_DEPTHAI
return outputMode_ == 0?stereoModel_.isValidForProjection():stereoModel_.left().isValidForProjection();
#else
return false;
#endif
}
std::string CameraDepthAI::getSerial() const
{
#ifdef RTABMAP_DEPTHAI
return device_->getMxId();
#endif
return "";
}
SensorData CameraDepthAI::captureImage(SensorCaptureInfo * info)
{
SensorData data;
#ifdef RTABMAP_DEPTHAI
auto messageGroup = cameraQueue_->get<dai::MessageGroup>();
auto rgbOrLeft = messageGroup->get<dai::ImgFrame>(outputMode_==2?"rgb":"left");
auto depthOrRight = messageGroup->get<dai::ImgFrame>(outputMode_?"depth":"right");
double stamp = std::chrono::duration<double>(depthOrRight->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count();
if(outputMode_)
data = SensorData(cv::imdecode(rgbOrLeft->getData(), cv::IMREAD_ANYCOLOR), depthOrRight->getCvFrame(), stereoModel_.left(), this->getNextSeqID(), stamp);
else
data = SensorData(cv::imdecode(rgbOrLeft->getData(), cv::IMREAD_GRAYSCALE), cv::imdecode(depthOrRight->getData(), cv::IMREAD_GRAYSCALE), stereoModel_, this->getNextSeqID(), stamp);
if(imuPublished_ && !publishInterIMU_)
{
cv::Vec3d acc, gyro;
std::map<double, cv::Vec3f>::const_iterator iterA, iterB;
imuMutex_.lock();
while(accBuffer_.empty() || gyroBuffer_.empty() || accBuffer_.rbegin()->first < stamp || gyroBuffer_.rbegin()->first < stamp)
{
imuMutex_.unlock();
uSleep(1);
imuMutex_.lock();
}
//acc
iterB = accBuffer_.lower_bound(stamp);
iterA = iterB;
if(iterA != accBuffer_.begin())
iterA = --iterA;
if(iterA == iterB || stamp == iterB->first)
{
acc = iterB->second;
}
else if(stamp > iterA->first && stamp < iterB->first)
{
float t = (stamp-iterA->first) / (iterB->first-iterA->first);
acc = iterA->second + t*(iterB->second - iterA->second);
}
accBuffer_.erase(accBuffer_.begin(), iterB);
//gyro
iterB = gyroBuffer_.lower_bound(stamp);
iterA = iterB;
if(iterA != gyroBuffer_.begin())
iterA = --iterA;
if(iterA == iterB || stamp == iterB->first)
{
gyro = iterB->second;
}
else if(stamp > iterA->first && stamp < iterB->first)
{
float t = (stamp-iterA->first) / (iterB->first-iterA->first);
gyro = iterA->second + t*(iterB->second - iterA->second);
}
gyroBuffer_.erase(gyroBuffer_.begin(), iterB);
imuMutex_.unlock();
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 = messageGroup->get<dai::TrackedFeatures>("feat")->trackedFeatures;
std::vector<cv::KeyPoint> keypoints;
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)
{
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);
cv::dilate(scores, dilated_scores, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(nmsRadius_*2+1, nmsRadius_*2+1)));
cv::Mat max_mask = scores == dilated_scores;
cv::dilate(scores, dilated_scores, cv::Mat());
cv::Mat max_mask_r1 = scores == dilated_scores;
cv::Mat supp_mask(targetSize_, CV_8UC1);
for(size_t i=0; i<2; i++)
{
cv::dilate(max_mask, supp_mask, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(nmsRadius_*2+1, nmsRadius_*2+1)));
cv::Mat supp_scores = scores.clone();
supp_scores.setTo(0, supp_mask);
cv::dilate(supp_scores, dilated_scores, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(nmsRadius_*2+1, nmsRadius_*2+1)));
cv::Mat new_max_mask = cv::Mat::zeros(targetSize_, CV_8UC1);
cv::bitwise_not(supp_mask, supp_mask);
cv::bitwise_and(supp_scores == dilated_scores, supp_mask, new_max_mask, max_mask_r1);
cv::bitwise_or(max_mask, new_max_mask, max_mask);
}
cv::bitwise_not(max_mask, supp_mask);
scores.setTo(0, supp_mask);
}
std::vector<cv::Point> kpts;
cv::findNonZero(scores > threshold_, kpts);
std::vector<cv::KeyPoint> keypoints;
for(auto& kpt : kpts)
{
float response = scores.at<float>(kpt);
keypoints.emplace_back(cv::KeyPoint(kpt, 8, -1, response));
}
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)
{
mapX.at<float>(i) = (keypoints[i].pt.x - (targetSize_.width-1)/2) * 40/targetSize_.width + (40-1)/2;
mapY.at<float>(i) = (keypoints[i].pt.y - (targetSize_.height-1)/2) * 25/targetSize_.height + (25-1)/2;
}
cv::Mat map1, map2, descriptors;
cv::convertMaps(mapX, mapY, map1, map2, CV_16SC2);
cv::remap(coarse_desc, descriptors, map1, map2, cv::INTER_LINEAR);
descriptors.forEach<cv::Vec<float, 256>>([&](cv::Vec<float, 256>& descriptor, const int position[]) -> void {
cv::normalize(descriptor, descriptor);
});
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()).clone()));
}
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
return data;
}
} // namespace rtabmap
Reference in New Issue View Git Blame Copy Permalink