i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-10 03:49:50 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add RGBD mode for OAK camera (#1179)

Browse Source 
* update depthai params

* add color+depth mode

* workarounds to avoid performance issues
This commit is contained in:
Borong Yuan
2023-12-14 09:59:35 -08:00
committed by GitHub
parent be3e6c538c
commit 45392fcfc6
4 changed files with 209 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/camera/CameraDepthAI.h
+9 -10
View File
@@ -55,13 +55,11 @@ public:
const Transform & localTransform = Transform::getIdentity()); const Transform & localTransform = Transform::getIdentity());
virtual ~CameraDepthAI(); virtual ~CameraDepthAI();
void setOutputDepth(bool enabled, int confidence = 200); void setOutputMode(int outputMode = 0);
void setUseSpecTranslation(bool useSpecTranslation); void setDepthProfile(int confThreshold = 200, int lrcThreshold = 5);
void setAlphaScaling(float alphaScaling = 0.0f); void setRectification(bool useSpecTranslation, float alphaScaling = 0.0f);
void setIMUPublished(bool published); void setIMU(bool imuPublished, bool publishInterIMU);
void publishInterIMU(bool enabled); void setIrBrightness(float dotProjectormA = 0.0f, float floodLightmA = 200.0f);
void setLaserDotBrightness(float dotProjectormA = 0.0f);
void setFloodLightBrightness(float floodLightmA = 200.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);
@@ -80,8 +78,9 @@ private:
cv::Size targetSize_; cv::Size targetSize_;
Transform imuLocalTransform_; Transform imuLocalTransform_;
std::string mxidOrName_; std::string mxidOrName_;
bool outputDepth_; int outputMode_;
int depthConfidence_; int confThreshold_;
int lrcThreshold_;
int resolution_; int resolution_;
bool useSpecTranslation_; bool useSpecTranslation_;
float alphaScaling_; float alphaScaling_;
@@ -98,7 +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> leftQueue_; std::shared_ptr<dai::DataOutputQueue> leftOrColorQueue_;
std::shared_ptr<dai::DataOutputQueue> rightOrDepthQueue_; std::shared_ptr<dai::DataOutputQueue> rightOrDepthQueue_;
std::shared_ptr<dai::DataOutputQueue> featuresQueue_; std::shared_ptr<dai::DataOutputQueue> featuresQueue_;
std::map<double, cv::Vec3f> accBuffer_; std::map<double, cv::Vec3f> accBuffer_;
corelib/src/camera/CameraDepthAI.cpp
+119 -87
View File
@@ -54,8 +54,9 @@ CameraDepthAI::CameraDepthAI(
#ifdef RTABMAP_DEPTHAI #ifdef RTABMAP_DEPTHAI
, ,
mxidOrName_(mxidOrName), mxidOrName_(mxidOrName),
outputDepth_(false), outputMode_(0),
depthConfidence_(200), confThreshold_(200),
lrcThreshold_(5),
resolution_(resolution), resolution_(resolution),
useSpecTranslation_(false), useSpecTranslation_(false),
alphaScaling_(0.0), alphaScaling_(0.0),
@@ -87,67 +88,49 @@ CameraDepthAI::~CameraDepthAI()
#endif #endif
} }
void CameraDepthAI::setOutputDepth(bool enabled, int confidence) void CameraDepthAI::setOutputMode(int outputMode)
{ {
#ifdef RTABMAP_DEPTHAI #ifdef RTABMAP_DEPTHAI
outputDepth_ = enabled; outputMode_ = outputMode;
if(outputDepth_)
{
depthConfidence_ = confidence;
}
#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
} }
void CameraDepthAI::setUseSpecTranslation(bool useSpecTranslation) 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::setRectification(bool useSpecTranslation, float alphaScaling)
{ {
#ifdef RTABMAP_DEPTHAI #ifdef RTABMAP_DEPTHAI
useSpecTranslation_ = useSpecTranslation; useSpecTranslation_ = useSpecTranslation;
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
}
void CameraDepthAI::setAlphaScaling(float alphaScaling)
{
#ifdef RTABMAP_DEPTHAI
alphaScaling_ = alphaScaling; alphaScaling_ = alphaScaling;
#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
} }
void CameraDepthAI::setIMUPublished(bool published) void CameraDepthAI::setIMU(bool imuPublished, bool publishInterIMU)
{ {
#ifdef RTABMAP_DEPTHAI #ifdef RTABMAP_DEPTHAI
imuPublished_ = published; imuPublished_ = imuPublished;
publishInterIMU_ = publishInterIMU;
#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
} }
void CameraDepthAI::publishInterIMU(bool enabled) void CameraDepthAI::setIrBrightness(float dotProjectormA, float floodLightmA)
{
#ifdef RTABMAP_DEPTHAI
publishInterIMU_ = enabled;
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
}
void CameraDepthAI::setLaserDotBrightness(float dotProjectormA)
{ {
#ifdef RTABMAP_DEPTHAI #ifdef RTABMAP_DEPTHAI
dotProjectormA_ = dotProjectormA; dotProjectormA_ = dotProjectormA;
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
}
void CameraDepthAI::setFloodLightBrightness(float floodLightmA)
{
#ifdef RTABMAP_DEPTHAI
floodLightmA_ = floodLightmA; floodLightmA_ = floodLightmA;
#else #else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
@@ -237,6 +220,16 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
auto monoLeft = p.create<dai::node::MonoCamera>(); auto monoLeft = p.create<dai::node::MonoCamera>();
auto monoRight = p.create<dai::node::MonoCamera>(); auto monoRight = p.create<dai::node::MonoCamera>();
auto stereo = p.create<dai::node::StereoDepth>(); auto stereo = p.create<dai::node::StereoDepth>();
std::shared_ptr<dai::node::Camera> colorCam;
if(outputMode_==2)
{
colorCam = p.create<dai::node::Camera>();
if(detectFeatures_)
{
UWARN("On-device feature detectors cannot be enabled on color camera input!");
detectFeatures_ = 0;
}
}
std::shared_ptr<dai::node::IMU> imu; std::shared_ptr<dai::node::IMU> imu;
if(imuPublished_) if(imuPublished_)
imu = p.create<dai::node::IMU>(); imu = p.create<dai::node::IMU>();
@@ -261,7 +254,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
} }
} }
auto xoutLeft = p.create<dai::node::XLinkOut>(); auto xoutLeftOrColor = p.create<dai::node::XLinkOut>();
auto xoutDepthOrRight = p.create<dai::node::XLinkOut>(); auto xoutDepthOrRight = p.create<dai::node::XLinkOut>();
std::shared_ptr<dai::node::XLinkOut> xoutIMU; std::shared_ptr<dai::node::XLinkOut> xoutIMU;
if(imuPublished_) if(imuPublished_)
@@ -271,17 +264,16 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
xoutFeatures = p.create<dai::node::XLinkOut>(); xoutFeatures = p.create<dai::node::XLinkOut>();
// XLinkOut // XLinkOut
xoutLeft->setStreamName("rectified_left"); xoutLeftOrColor->setStreamName(outputMode_<2?"rectified_left":"rectified_color");
xoutDepthOrRight->setStreamName(outputDepth_?"depth":"rectified_right"); xoutDepthOrRight->setStreamName(outputMode_?"depth":"rectified_right");
if(imuPublished_) if(imuPublished_)
xoutIMU->setStreamName("imu"); xoutIMU->setStreamName("imu");
if(detectFeatures_) if(detectFeatures_)
xoutFeatures->setStreamName("features"); xoutFeatures->setStreamName("features");
// MonoCamera
monoLeft->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_); monoLeft->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
monoLeft->setCamera("left");
monoRight->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_); monoRight->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
monoLeft->setCamera("left");
monoRight->setCamera("right"); monoRight->setCamera("right");
if(detectFeatures_ == 2) if(detectFeatures_ == 2)
{ {
@@ -299,17 +291,20 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
} }
// StereoDepth // StereoDepth
stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT); if(outputMode_ == 2)
stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A);
else
stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT);
stereo->setExtendedDisparity(false); stereo->setExtendedDisparity(false);
stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
stereo->enableDistortionCorrection(true); stereo->enableDistortionCorrection(true);
stereo->setDisparityToDepthUseSpecTranslation(useSpecTranslation_); stereo->setDisparityToDepthUseSpecTranslation(useSpecTranslation_);
stereo->setDepthAlignmentUseSpecTranslation(useSpecTranslation_); stereo->setDepthAlignmentUseSpecTranslation(useSpecTranslation_);
if(alphaScaling_>-1.0f) if(alphaScaling_ > -1.0f)
stereo->setAlphaScaling(alphaScaling_); stereo->setAlphaScaling(alphaScaling_);
stereo->initialConfig.setConfidenceThreshold(depthConfidence_); stereo->initialConfig.setConfidenceThreshold(confThreshold_);
stereo->initialConfig.setLeftRightCheck(true); stereo->initialConfig.setLeftRightCheck(true);
stereo->initialConfig.setLeftRightCheckThreshold(5); stereo->initialConfig.setLeftRightCheckThreshold(lrcThreshold_);
stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_7x7); stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_7x7);
auto config = stereo->initialConfig.get(); auto config = stereo->initialConfig.get();
config.censusTransform.kernelSize = dai::StereoDepthConfig::CensusTransform::KernelSize::KERNEL_7x9; config.censusTransform.kernelSize = dai::StereoDepthConfig::CensusTransform::KernelSize::KERNEL_7x9;
@@ -321,15 +316,32 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
monoLeft->out.link(stereo->left); monoLeft->out.link(stereo->left);
monoRight->out.link(stereo->right); monoRight->out.link(stereo->right);
if(outputMode_ == 2)
{
colorCam->setBoardSocket(dai::CameraBoardSocket::CAM_A);
colorCam->setSize(targetSize_.width, targetSize_.height);
if(this->getImageRate() > 0)
colorCam->setFps(this->getImageRate());
if(alphaScaling_ > -1.0f)
colorCam->setCalibrationAlpha(alphaScaling_);
}
// 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)
{ {
auto leftEnc = p.create<dai::node::VideoEncoder>(); auto leftOrColorEnc = p.create<dai::node::VideoEncoder>();
auto depthOrRightEnc = p.create<dai::node::VideoEncoder>(); auto depthOrRightEnc = p.create<dai::node::VideoEncoder>();
leftEnc->setDefaultProfilePreset(monoLeft->getFps(), dai::VideoEncoderProperties::Profile::MJPEG); leftOrColorEnc->setDefaultProfilePreset(monoLeft->getFps(), dai::VideoEncoderProperties::Profile::MJPEG);
depthOrRightEnc->setDefaultProfilePreset(monoRight->getFps(), dai::VideoEncoderProperties::Profile::MJPEG); depthOrRightEnc->setDefaultProfilePreset(monoRight->getFps(), dai::VideoEncoderProperties::Profile::MJPEG);
stereo->rectifiedLeft.link(leftEnc->input); if(outputMode_ < 2)
if(outputDepth_) {
stereo->rectifiedLeft.link(leftOrColorEnc->input);
}
else
{
colorCam->video.link(leftOrColorEnc->input);
}
if(outputMode_)
{ {
depthOrRightEnc->setQuality(100); depthOrRightEnc->setQuality(100);
stereo->disparity.link(depthOrRightEnc->input); stereo->disparity.link(depthOrRightEnc->input);
@@ -338,7 +350,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
{ {
stereo->rectifiedRight.link(depthOrRightEnc->input); stereo->rectifiedRight.link(depthOrRightEnc->input);
} }
leftEnc->bitstream.link(xoutLeft->input); leftOrColorEnc->bitstream.link(xoutLeftOrColor->input);
depthOrRightEnc->bitstream.link(xoutDepthOrRight->input); depthOrRightEnc->bitstream.link(xoutDepthOrRight->input);
} }
else else
@@ -349,8 +361,17 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
config.costMatching.disparityWidth = dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64; config.costMatching.disparityWidth = dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64;
config.costMatching.enableCompanding = true; config.costMatching.enableCompanding = true;
stereo->initialConfig.set(config); stereo->initialConfig.set(config);
stereo->rectifiedLeft.link(xoutLeft->input); if(outputMode_ < 2)
if(outputDepth_) {
stereo->rectifiedLeft.link(xoutLeftOrColor->input);
}
else
{
monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
colorCam->video.link(xoutLeftOrColor->input);
}
if(outputMode_)
stereo->depth.link(xoutDepthOrRight->input); stereo->depth.link(xoutDepthOrRight->input);
else else
stereo->rectifiedRight.link(xoutDepthOrRight->input); stereo->rectifiedRight.link(xoutDepthOrRight->input);
@@ -403,30 +424,34 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
UINFO("Loading eeprom calibration data"); UINFO("Loading eeprom calibration data");
dai::CalibrationHandler calibHandler = device_->readCalibration(); dai::CalibrationHandler calibHandler = device_->readCalibration();
cv::Mat cameraMatrix, distCoeffs, new_camera_matrix; auto cameraId = outputMode_<2?dai::CameraBoardSocket::CAM_B:dai::CameraBoardSocket::CAM_A;
cv::Mat cameraMatrix, distCoeffs, newCameraMatrix;
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(cameraId, targetSize_.width, targetSize_.height);
cameraMatrix = (cv::Mat_<double>(3,3) << cameraMatrix = (cv::Mat_<double>(3,3) <<
matrix[0][0], matrix[0][1], matrix[0][2], matrix[0][0], matrix[0][1], matrix[0][2],
matrix[1][0], matrix[1][1], matrix[1][2], matrix[1][0], matrix[1][1], matrix[1][2],
matrix[2][0], matrix[2][1], matrix[2][2]); matrix[2][0], matrix[2][1], matrix[2][2]);
std::vector<float> coeffs = calibHandler.getDistortionCoefficients(dai::CameraBoardSocket::CAM_B); std::vector<float> coeffs = calibHandler.getDistortionCoefficients(cameraId);
if(calibHandler.getDistortionModel(dai::CameraBoardSocket::CAM_B) == dai::CameraModel::Perspective) 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]); 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) if(alphaScaling_>-1.0f)
new_camera_matrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, targetSize_, alphaScaling_); newCameraMatrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, targetSize_, alphaScaling_);
else else
new_camera_matrix = cameraMatrix; newCameraMatrix = cameraMatrix;
double fx = new_camera_matrix.at<double>(0, 0); double fx = newCameraMatrix.at<double>(0, 0);
double fy = new_camera_matrix.at<double>(1, 1); double fy = newCameraMatrix.at<double>(1, 1);
double cx = new_camera_matrix.at<double>(0, 2); double cx = newCameraMatrix.at<double>(0, 2);
double cy = new_camera_matrix.at<double>(1, 2); double cy = newCameraMatrix.at<double>(1, 2);
double baseline = calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_C, dai::CameraBoardSocket::CAM_B, useSpecTranslation_)/100.0; double baseline = calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_C, dai::CameraBoardSocket::CAM_B, useSpecTranslation_)/100.0;
UINFO("left: fx=%f fy=%f cx=%f cy=%f baseline=%f", fx, fy, cx, cy, baseline); UINFO("fx=%f fy=%f cx=%f cy=%f baseline=%f", fx, fy, cx, cy, baseline);
stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize_); if(outputMode_ == 2)
stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize_);
else
stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, baseline, this->getLocalTransform()*Transform(-calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_A)/100.0, 0, 0), targetSize_);
if(imuPublished_) if(imuPublished_)
{ {
@@ -443,22 +468,29 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
{ {
imuLocalTransform_ = Transform( imuLocalTransform_ = Transform(
0, -1, 0, 0.0525, 0, -1, 0, 0.0525,
1, 0, 0, 0.0137, 1, 0, 0, 0.013662,
0, 0, 1, 0); 0, 0, 1, 0);
} }
else if(eeprom.boardName == "DM9098") else if(eeprom.boardName == "DM9098")
{ {
imuLocalTransform_ = Transform( imuLocalTransform_ = Transform(
0, 1, 0, 0.075445, 0, 1, 0, 0.037945,
1, 0, 0, 0.00079, 1, 0, 0, 0.00079,
0, 0, -1, -0.007); 0, 0, -1, 0);
}
else if(eeprom.boardName == "NG2094")
{
imuLocalTransform_ = Transform(
0, 1, 0, 0.0374,
1, 0, 0, 0.00176,
0, 0, -1, 0);
} }
else if(eeprom.boardName == "NG9097") else if(eeprom.boardName == "NG9097")
{ {
imuLocalTransform_ = Transform( imuLocalTransform_ = Transform(
0, 1, 0, 0.0775, 0, 1, 0, 0.04,
1, 0, 0, 0.020265, 1, 0, 0, 0.020265,
0, 0, -1, -0.007); 0, 0, -1, 0);
} }
else else
{ {
@@ -502,8 +534,8 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
} }
}); });
} }
leftQueue_ = device_->getOutputQueue("rectified_left", 8, false); leftOrColorQueue_ = device_->getOutputQueue(outputMode_<2?"rectified_left":"rectified_color", 8, false);
rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 8, false); rightOrDepthQueue_ = device_->getOutputQueue(outputMode_?"depth":"rectified_right", 8, false);
if(detectFeatures_) if(detectFeatures_)
featuresQueue_ = device_->getOutputQueue("features", 8, false); featuresQueue_ = device_->getOutputQueue("features", 8, false);
@@ -545,21 +577,21 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
SensorData data; SensorData data;
#ifdef RTABMAP_DEPTHAI #ifdef RTABMAP_DEPTHAI
cv::Mat left, depthOrRight; cv::Mat leftOrColor, depthOrRight;
auto rectifL = leftQueue_->get<dai::ImgFrame>(); auto rectifLeftOrColor = leftOrColorQueue_->get<dai::ImgFrame>();
auto rectifRightOrDepth = rightOrDepthQueue_->get<dai::ImgFrame>(); auto rectifRightOrDepth = rightOrDepthQueue_->get<dai::ImgFrame>();
while(rectifL->getSequenceNum() < rectifRightOrDepth->getSequenceNum()) while(rectifLeftOrColor->getSequenceNum() < rectifRightOrDepth->getSequenceNum())
rectifL = leftQueue_->get<dai::ImgFrame>(); rectifLeftOrColor = leftOrColorQueue_->get<dai::ImgFrame>();
while(rectifL->getSequenceNum() > rectifRightOrDepth->getSequenceNum()) while(rectifLeftOrColor->getSequenceNum() > rectifRightOrDepth->getSequenceNum())
rectifRightOrDepth = rightOrDepthQueue_->get<dai::ImgFrame>(); rectifRightOrDepth = rightOrDepthQueue_->get<dai::ImgFrame>();
double stamp = std::chrono::duration<double>(rectifL->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count(); 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)
{ {
left = cv::imdecode(rectifL->getData(), cv::IMREAD_GRAYSCALE); leftOrColor = cv::imdecode(rectifLeftOrColor->getData(), cv::IMREAD_ANYCOLOR);
depthOrRight = cv::imdecode(rectifRightOrDepth->getData(), cv::IMREAD_GRAYSCALE); depthOrRight = cv::imdecode(rectifRightOrDepth->getData(), cv::IMREAD_GRAYSCALE);
if(outputDepth_) if(outputMode_)
{ {
cv::Mat disp; cv::Mat disp;
depthOrRight.convertTo(disp, CV_16UC1); depthOrRight.convertTo(disp, CV_16UC1);
@@ -568,14 +600,14 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
} }
else else
{ {
left = rectifL->getFrame(true); leftOrColor = rectifLeftOrColor->getCvFrame();
depthOrRight = rectifRightOrDepth->getFrame(true); depthOrRight = rectifRightOrDepth->getCvFrame();
} }
if(outputDepth_) if(outputMode_)
data = SensorData(left, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp); data = SensorData(leftOrColor, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);
else else
data = SensorData(left, depthOrRight, stereoModel_, this->getNextSeqID(), stamp); data = SensorData(leftOrColor, depthOrRight, stereoModel_, this->getNextSeqID(), stamp);
if(imuPublished_ && !publishInterIMU_) if(imuPublished_ && !publishInterIMU_)
{ {
@@ -629,7 +661,7 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
if(detectFeatures_ == 1) if(detectFeatures_ == 1)
{ {
auto features = featuresQueue_->get<dai::TrackedFeatures>(); auto features = featuresQueue_->get<dai::TrackedFeatures>();
while(features->getSequenceNum() < rectifL->getSequenceNum()) while(features->getSequenceNum() < rectifLeftOrColor->getSequenceNum())
features = featuresQueue_->get<dai::TrackedFeatures>(); features = featuresQueue_->get<dai::TrackedFeatures>();
auto detectedFeatures = features->trackedFeatures; auto detectedFeatures = features->trackedFeatures;
@@ -641,7 +673,7 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
else if(detectFeatures_ == 2) else if(detectFeatures_ == 2)
{ {
auto features = featuresQueue_->get<dai::NNData>(); auto features = featuresQueue_->get<dai::NNData>();
while(features->getSequenceNum() < rectifL->getSequenceNum()) while(features->getSequenceNum() < rectifLeftOrColor->getSequenceNum())
features = featuresQueue_->get<dai::NNData>(); features = featuresQueue_->get<dai::NNData>();
auto heatmap = features->getLayerFp16("heatmap"); auto heatmap = features->getLayerFp16("heatmap");
guilib/src/PreferencesDialog.cpp
+17 -15
View File
@@ -804,8 +804,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->spinBox_stereoMyntEye_irControl, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->spinBox_stereoMyntEye_irControl, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->comboBox_depthai_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->comboBox_depthai_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkBox_depthai_depth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->comboBox_depthai_output_mode, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_depthai_confidence, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->spinBox_depthai_conf_threshold, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_depthai_lrc_threshold, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
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()));
@@ -2142,8 +2143,9 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->spinBox_stereoMyntEye_contrast->setValue(116); _ui->spinBox_stereoMyntEye_contrast->setValue(116);
_ui->spinBox_stereoMyntEye_irControl->setValue(0); _ui->spinBox_stereoMyntEye_irControl->setValue(0);
_ui->comboBox_depthai_resolution->setCurrentIndex(1); _ui->comboBox_depthai_resolution->setCurrentIndex(1);
_ui->checkBox_depthai_depth->setChecked(false); _ui->comboBox_depthai_output_mode->setCurrentIndex(0);
_ui->spinBox_depthai_confidence->setValue(200); _ui->spinBox_depthai_conf_threshold->setValue(200);
_ui->spinBox_depthai_lrc_threshold->setValue(5);
_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);
@@ -2633,8 +2635,9 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
settings.beginGroup("DepthAI"); settings.beginGroup("DepthAI");
_ui->comboBox_depthai_resolution->setCurrentIndex(settings.value("resolution", _ui->comboBox_depthai_resolution->currentIndex()).toInt()); _ui->comboBox_depthai_resolution->setCurrentIndex(settings.value("resolution", _ui->comboBox_depthai_resolution->currentIndex()).toInt());
_ui->checkBox_depthai_depth->setChecked(settings.value("depth", _ui->checkBox_depthai_depth->isChecked()).toBool()); _ui->comboBox_depthai_output_mode->setCurrentIndex(settings.value("output_mode", _ui->comboBox_depthai_output_mode->currentIndex()).toInt());
_ui->spinBox_depthai_confidence->setValue(settings.value("confidence", _ui->spinBox_depthai_confidence->value()).toInt()); _ui->spinBox_depthai_conf_threshold->setValue(settings.value("conf_threshold", _ui->spinBox_depthai_conf_threshold->value()).toInt());
_ui->spinBox_depthai_lrc_threshold->setValue(settings.value("lrc_threshold", _ui->spinBox_depthai_lrc_threshold->value()).toInt());
_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());
@@ -3167,8 +3170,9 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.beginGroup("DepthAI"); settings.beginGroup("DepthAI");
settings.setValue("resolution", _ui->comboBox_depthai_resolution->currentIndex()); settings.setValue("resolution", _ui->comboBox_depthai_resolution->currentIndex());
settings.setValue("depth", _ui->checkBox_depthai_depth->isChecked()); settings.setValue("output_mode", _ui->comboBox_depthai_output_mode->currentIndex());
settings.setValue("confidence", _ui->spinBox_depthai_confidence->value()); settings.setValue("conf_threshold", _ui->spinBox_depthai_conf_threshold->value());
settings.setValue("lrc_threshold", _ui->spinBox_depthai_lrc_threshold->value());
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());
@@ -6486,13 +6490,11 @@ Camera * PreferencesDialog::createCamera(
_ui->comboBox_depthai_resolution->currentIndex(), _ui->comboBox_depthai_resolution->currentIndex(),
this->getGeneralInputRate(), this->getGeneralInputRate(),
this->getSourceLocalTransform()); this->getSourceLocalTransform());
((CameraDepthAI*)camera)->setOutputDepth(_ui->checkBox_depthai_depth->isChecked(), _ui->spinBox_depthai_confidence->value()); ((CameraDepthAI*)camera)->setOutputMode(_ui->comboBox_depthai_output_mode->currentIndex());
((CameraDepthAI*)camera)->setUseSpecTranslation(_ui->checkBox_depthai_use_spec_translation->isChecked()); ((CameraDepthAI*)camera)->setDepthProfile(_ui->spinBox_depthai_conf_threshold->value(), _ui->spinBox_depthai_lrc_threshold->value());
((CameraDepthAI*)camera)->setAlphaScaling(_ui->doubleSpinBox_depthai_alpha_scaling->value()); ((CameraDepthAI*)camera)->setRectification(_ui->checkBox_depthai_use_spec_translation->isChecked(), _ui->doubleSpinBox_depthai_alpha_scaling->value());
((CameraDepthAI*)camera)->setIMUPublished(_ui->checkBox_depthai_imu_published->isChecked()); ((CameraDepthAI*)camera)->setIMU(_ui->checkBox_depthai_imu_published->isChecked(), _ui->checkbox_publishInterIMU->isChecked());
((CameraDepthAI*)camera)->publishInterIMU(_ui->checkbox_publishInterIMU->isChecked()); ((CameraDepthAI*)camera)->setIrBrightness(_ui->doubleSpinBox_depthai_laser_dot_brightness->value(), _ui->doubleSpinBox_depthai_floodlight_brightness->value());
((CameraDepthAI*)camera)->setLaserDotBrightness(_ui->doubleSpinBox_depthai_laser_dot_brightness->value());
((CameraDepthAI*)camera)->setFloodLightBrightness(_ui->doubleSpinBox_depthai_floodlight_brightness->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)
guilib/src/ui/preferencesDialog.ui
+64 -20
View File
@@ -5905,7 +5905,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</item> </item>
</widget> </widget>
</item> </item>
<item row="4" column="0"> <item row="5" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_depthai_alpha_scaling"> <widget class="QDoubleSpinBox" name="doubleSpinBox_depthai_alpha_scaling">
<property name="minimum"> <property name="minimum">
<double>-1.000000000000000</double> <double>-1.000000000000000</double>
@@ -5924,7 +5924,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item row="2" column="1"> <item row="2" column="1">
<widget class="QLabel" name="label_627"> <widget class="QLabel" name="label_627">
<property name="text"> <property name="text">
<string>Depth confidence.</string> <string>Confidence threshold.</string>
</property> </property>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>
@@ -5934,7 +5934,20 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property> </property>
</widget> </widget>
</item> </item>
<item row="6" column="0"> <item row="3" column="1">
<widget class="QLabel" name="label_741">
<property name="text">
<string>Left-right check threshold.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_depthai_laser_dot_brightness"> <widget class="QDoubleSpinBox" name="doubleSpinBox_depthai_laser_dot_brightness">
<property name="suffix"> <property name="suffix">
<string> mA</string> <string> mA</string>
@@ -5947,14 +5960,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property> </property>
</widget> </widget>
</item> </item>
<item row="5" column="0"> <item row="6" column="0">
<widget class="QCheckBox" name="checkBox_depthai_imu_published"> <widget class="QCheckBox" name="checkBox_depthai_imu_published">
<property name="text"> <property name="text">
<string/> <string/>
</property> </property>
</widget> </widget>
</item> </item>
<item row="7" 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>Floodlight brightness.</string>
@@ -5967,7 +5980,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property> </property>
</widget> </widget>
</item> </item>
<item row="6" 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>Laser dot brightness.</string>
@@ -5980,7 +5993,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property> </property>
</widget> </widget>
</item> </item>
<item row="3" column="1"> <item row="4" column="1">
<widget class="QLabel" name="label_646"> <widget class="QLabel" name="label_646">
<property name="text"> <property name="text">
<string>Use the translation information from the board design data (not the calibration data).</string> <string>Use the translation information from the board design data (not the calibration data).</string>
@@ -6007,13 +6020,31 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</widget> </widget>
</item> </item>
<item row="1" column="0"> <item row="1" column="0">
<widget class="QCheckBox" name="checkBox_depthai_depth"> <widget class="QComboBox" name="comboBox_depthai_output_mode">
<property name="text"> <property name="currentIndex">
<string/> <number>0</number>
</property> </property>
<property name="sizeAdjustPolicy">
<enum>QComboBox::AdjustToContents</enum>
</property>
<item>
<property name="text">
<string>Stereo</string>
</property>
</item>
<item>
<property name="text">
<string>Mono+Depth</string>
</property>
</item>
<item>
<property name="text">
<string>Color+Depth</string>
</property>
</item>
</widget> </widget>
</item> </item>
<item row="4" column="1"> <item row="5" column="1">
<widget class="QLabel" name="label_678"> <widget class="QLabel" name="label_678">
<property name="text"> <property name="text">
<string>Free scaling parameter between 0 (when all the pixels in the undistorted image are valid) and 1 (when all the source image pixels are retained in the undistorted image). On some high distortion lenses, and/or due to rectification (image rotated) invalid areas may appear even with alpha=0, in these cases alpha &lt; 0.0 helps removing invalid areas. If alpha is set to -1, old rectification policy is used.</string> <string>Free scaling parameter between 0 (when all the pixels in the undistorted image are valid) and 1 (when all the source image pixels are retained in the undistorted image). On some high distortion lenses, and/or due to rectification (image rotated) invalid areas may appear even with alpha=0, in these cases alpha &lt; 0.0 helps removing invalid areas. If alpha is set to -1, old rectification policy is used.</string>
@@ -6027,7 +6058,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</widget> </widget>
</item> </item>
<item row="2" column="0"> <item row="2" column="0">
<widget class="QSpinBox" name="spinBox_depthai_confidence"> <widget class="QSpinBox" name="spinBox_depthai_conf_threshold">
<property name="minimum"> <property name="minimum">
<number>0</number> <number>0</number>
</property> </property>
@@ -6039,7 +6070,20 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property> </property>
</widget> </widget>
</item> </item>
<item row="5" column="1"> <item row="3" column="0">
<widget class="QSpinBox" name="spinBox_depthai_lrc_threshold">
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>255</number>
</property>
<property name="value">
<number>5</number>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_650"> <widget class="QLabel" name="label_650">
<property name="text"> <property name="text">
<string>IMU published</string> <string>IMU published</string>
@@ -6052,7 +6096,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property> </property>
</widget> </widget>
</item> </item>
<item row="3" column="0"> <item row="4" column="0">
<widget class="QCheckBox" name="checkBox_depthai_use_spec_translation"> <widget class="QCheckBox" name="checkBox_depthai_use_spec_translation">
<property name="text"> <property name="text">
<string/> <string/>
@@ -6062,7 +6106,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item row="1" column="1"> <item row="1" column="1">
<widget class="QLabel" name="label_625"> <widget class="QLabel" name="label_625">
<property name="text"> <property name="text">
<string>Output depth.</string> <string>Output mode.</string>
</property> </property>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>
@@ -6072,7 +6116,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property> </property>
</widget> </widget>
</item> </item>
<item row="7" column="0"> <item row="8" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_depthai_floodlight_brightness"> <widget class="QDoubleSpinBox" name="doubleSpinBox_depthai_floodlight_brightness">
<property name="suffix"> <property name="suffix">
<string> mA</string> <string> mA</string>
@@ -6085,7 +6129,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property> </property>
</widget> </widget>
</item> </item>
<item row="8" column="0"> <item row="9" column="0">
<widget class="QComboBox" name="comboBox_depthai_detect_features"> <widget class="QComboBox" name="comboBox_depthai_detect_features">
<property name="currentIndex"> <property name="currentIndex">
<number>0</number> <number>0</number>
@@ -6110,7 +6154,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</item> </item>
</widget> </widget>
</item> </item>
<item row="8" column="1"> <item row="9" column="1">
<widget class="QLabel" name="label_679"> <widget class="QLabel" name="label_679">
<property name="text"> <property name="text">
<string>On-device feature detector.</string> <string>On-device feature detector.</string>
@@ -6123,7 +6167,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property> </property>
</widget> </widget>
</item> </item>
<item row="9" column="0"> <item row="10" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_15" stretch="0,1"> <layout class="QHBoxLayout" name="horizontalLayout_15" stretch="0,1">
<item> <item>
<widget class="QToolButton" name="toolButton_depthai_blob_path"> <widget class="QToolButton" name="toolButton_depthai_blob_path">
@@ -6147,7 +6191,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</item> </item>
</layout> </layout>
</item> </item>
<item row="9" 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 SuperPoint blob file.</string>