Add RGBD mode for OAK camera (#1179)

* update depthai params * add color+depth mode * workarounds to avoid performance issues
2026-09-02 09:30:25 +08:00 · 2023-12-15 01:59:35 +08:00
parent be3e6c538c
commit 45392fcfc6
4 changed files with 209 additions and 132 deletions
--- a/corelib/src/camera/CameraDepthAI.cpp
+++ b/corelib/src/camera/CameraDepthAI.cpp
@@ -54,8 +54,9 @@ CameraDepthAI::CameraDepthAI(
 #ifdef RTABMAP_DEPTHAI
 	,
 	mxidOrName_(mxidOrName),
-	outputDepth_(false),
-	depthConfidence_(200),
+	outputMode_(0),
+	confThreshold_(200),
+	lrcThreshold_(5),
 	resolution_(resolution),
 	useSpecTranslation_(false),
 	alphaScaling_(0.0),
@@ -87,67 +88,49 @@ CameraDepthAI::~CameraDepthAI()
 #endif
 }

-void CameraDepthAI::setOutputDepth(bool enabled, int confidence)
+void CameraDepthAI::setOutputMode(int outputMode)
 {
 #ifdef RTABMAP_DEPTHAI
-	outputDepth_ = enabled;
-	if(outputDepth_)
-	{
-		depthConfidence_ = confidence;
-	}
+	outputMode_ = outputMode;
 #else
 	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
 #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
 	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;
 #else
 	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
 #endif
 }

-void CameraDepthAI::setIMUPublished(bool published)
+void CameraDepthAI::setIMU(bool imuPublished, bool publishInterIMU)
 {
 #ifdef RTABMAP_DEPTHAI
-	imuPublished_ = published;
+	imuPublished_ = imuPublished;
+	publishInterIMU_ = publishInterIMU;
 #else
 	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
 #endif
 }

-void CameraDepthAI::publishInterIMU(bool enabled)
-{
-#ifdef RTABMAP_DEPTHAI
-	publishInterIMU_ = enabled;
-#else
-	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
-#endif
-}
-
-void CameraDepthAI::setLaserDotBrightness(float dotProjectormA)
+void CameraDepthAI::setIrBrightness(float dotProjectormA, float floodLightmA)
 {
 #ifdef RTABMAP_DEPTHAI
 	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;
 #else
 	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 monoRight = p.create<dai::node::MonoCamera>();
 	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;
 	if(imuPublished_)
 		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>();
 	std::shared_ptr<dai::node::XLinkOut> xoutIMU;
 	if(imuPublished_)
@@ -271,17 +264,16 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		xoutFeatures = p.create<dai::node::XLinkOut>();

 	// XLinkOut
-	xoutLeft->setStreamName("rectified_left");
-	xoutDepthOrRight->setStreamName(outputDepth_?"depth":"rectified_right");
+	xoutLeftOrColor->setStreamName(outputMode_<2?"rectified_left":"rectified_color");
+	xoutDepthOrRight->setStreamName(outputMode_?"depth":"rectified_right");
 	if(imuPublished_)
 		xoutIMU->setStreamName("imu");
 	if(detectFeatures_)
 		xoutFeatures->setStreamName("features");

-	// MonoCamera
 	monoLeft->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
-	monoLeft->setCamera("left");
 	monoRight->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
+	monoLeft->setCamera("left");
 	monoRight->setCamera("right");
 	if(detectFeatures_ == 2)
 	{
@@ -299,17 +291,20 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 	}

 	// 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->setRectifyEdgeFillColor(0); // black, to better see the cutout
 	stereo->enableDistortionCorrection(true);
 	stereo->setDisparityToDepthUseSpecTranslation(useSpecTranslation_);
 	stereo->setDepthAlignmentUseSpecTranslation(useSpecTranslation_);
-	if(alphaScaling_>-1.0f)
+	if(alphaScaling_ > -1.0f)
 		stereo->setAlphaScaling(alphaScaling_);
-	stereo->initialConfig.setConfidenceThreshold(depthConfidence_);
+	stereo->initialConfig.setConfidenceThreshold(confThreshold_);
 	stereo->initialConfig.setLeftRightCheck(true);
-	stereo->initialConfig.setLeftRightCheckThreshold(5);
+	stereo->initialConfig.setLeftRightCheckThreshold(lrcThreshold_);
 	stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_7x7);
 	auto config = stereo->initialConfig.get();
 	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);
 	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
 	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>();
-		leftEnc->setDefaultProfilePreset(monoLeft->getFps(), dai::VideoEncoderProperties::Profile::MJPEG);
+		leftOrColorEnc->setDefaultProfilePreset(monoLeft->getFps(), dai::VideoEncoderProperties::Profile::MJPEG);
 		depthOrRightEnc->setDefaultProfilePreset(monoRight->getFps(), dai::VideoEncoderProperties::Profile::MJPEG);
-		stereo->rectifiedLeft.link(leftEnc->input);
-		if(outputDepth_)
+		if(outputMode_ < 2)
+		{
+			stereo->rectifiedLeft.link(leftOrColorEnc->input);
+		}
+		else
+		{
+			colorCam->video.link(leftOrColorEnc->input);
+		}
+		if(outputMode_)
 		{
 			depthOrRightEnc->setQuality(100);
 			stereo->disparity.link(depthOrRightEnc->input);
@@ -338,7 +350,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		{
 			stereo->rectifiedRight.link(depthOrRightEnc->input);
 		}
-		leftEnc->bitstream.link(xoutLeft->input);
+		leftOrColorEnc->bitstream.link(xoutLeftOrColor->input);
 		depthOrRightEnc->bitstream.link(xoutDepthOrRight->input);
 	}
 	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.enableCompanding = true;
 		stereo->initialConfig.set(config);
-		stereo->rectifiedLeft.link(xoutLeft->input);
-		if(outputDepth_)
+		if(outputMode_ < 2)
+		{
+			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);
 		else
 			stereo->rectifiedRight.link(xoutDepthOrRight->input);
@@ -403,30 +424,34 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 	UINFO("Loading eeprom calibration data");
 	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) <<
 		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(dai::CameraBoardSocket::CAM_B);
-	if(calibHandler.getDistortionModel(dai::CameraBoardSocket::CAM_B) == dai::CameraModel::Perspective)
+	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)
-		new_camera_matrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, targetSize_, alphaScaling_);
+		newCameraMatrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, targetSize_, alphaScaling_);
 	else
-		new_camera_matrix = cameraMatrix;
+		newCameraMatrix = cameraMatrix;

-	double fx = new_camera_matrix.at<double>(0, 0);
-	double fy = new_camera_matrix.at<double>(1, 1);
-	double cx = new_camera_matrix.at<double>(0, 2);
-	double cy = new_camera_matrix.at<double>(1, 2);
+	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);
 	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);
-	stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize_);
+	UINFO("fx=%f fy=%f cx=%f cy=%f baseline=%f", fx, fy, cx, cy, baseline);
+	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_)
 	{
@@ -443,22 +468,29 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		{
 			imuLocalTransform_ = Transform(
 				 0, -1,  0,  0.0525,
-				 1,  0,  0,  0.0137,
+				 1,  0,  0,  0.013662,
 				 0,  0,  1,  0);
 		}
 		else if(eeprom.boardName == "DM9098")
 		{
 			imuLocalTransform_ = Transform(
-				 0,  1,  0,  0.075445,
+				 0,  1,  0,  0.037945,
 				 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")
 		{
 			imuLocalTransform_ = Transform(
-				 0,  1,  0,  0.0775,
+				 0,  1,  0,  0.04,
 				 1,  0,  0,  0.020265,
-				 0,  0, -1, -0.007);
+				 0,  0, -1,  0);
 		}
 		else
 		{
@@ -502,8 +534,8 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 			}
 		});
 	}
-	leftQueue_ = device_->getOutputQueue("rectified_left", 8, false);
-	rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 8, false);
+	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);

@@ -545,21 +577,21 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
 	SensorData data;
 #ifdef RTABMAP_DEPTHAI

-	cv::Mat left, depthOrRight;
-	auto rectifL = leftQueue_->get<dai::ImgFrame>();
+	cv::Mat leftOrColor, depthOrRight;
+	auto rectifLeftOrColor = leftOrColorQueue_->get<dai::ImgFrame>();
 	auto rectifRightOrDepth = rightOrDepthQueue_->get<dai::ImgFrame>();

-	while(rectifL->getSequenceNum() < rectifRightOrDepth->getSequenceNum())
-		rectifL = leftQueue_->get<dai::ImgFrame>();
-	while(rectifL->getSequenceNum() > rectifRightOrDepth->getSequenceNum())
+	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>(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)
 	{
-		left = cv::imdecode(rectifL->getData(), cv::IMREAD_GRAYSCALE);
+		leftOrColor = cv::imdecode(rectifLeftOrColor->getData(), cv::IMREAD_ANYCOLOR);
 		depthOrRight = cv::imdecode(rectifRightOrDepth->getData(), cv::IMREAD_GRAYSCALE);
-		if(outputDepth_)
+		if(outputMode_)
 		{
 			cv::Mat disp;
 			depthOrRight.convertTo(disp, CV_16UC1);
@@ -568,14 +600,14 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
 	}
 	else
 	{
-		left = rectifL->getFrame(true);
-		depthOrRight = rectifRightOrDepth->getFrame(true);
+		leftOrColor = rectifLeftOrColor->getCvFrame();
+		depthOrRight = rectifRightOrDepth->getCvFrame();
 	}

-	if(outputDepth_)
-		data = SensorData(left, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);
+	if(outputMode_)
+		data = SensorData(leftOrColor, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);
 	else
-		data = SensorData(left, depthOrRight, stereoModel_, this->getNextSeqID(), stamp);
+		data = SensorData(leftOrColor, depthOrRight, stereoModel_, this->getNextSeqID(), stamp);

 	if(imuPublished_ && !publishInterIMU_)
 	{
@@ -629,7 +661,7 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
 	if(detectFeatures_ == 1)
 	{
 		auto features = featuresQueue_->get<dai::TrackedFeatures>();
-		while(features->getSequenceNum() < rectifL->getSequenceNum())
+		while(features->getSequenceNum() < rectifLeftOrColor->getSequenceNum())
 			features = featuresQueue_->get<dai::TrackedFeatures>();
 		auto detectedFeatures = features->trackedFeatures;

@@ -641,7 +673,7 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
 	else if(detectFeatures_ == 2)
 	{
 		auto features = featuresQueue_->get<dai::NNData>();
-		while(features->getSequenceNum() < rectifL->getSequenceNum())
+		while(features->getSequenceNum() < rectifLeftOrColor->getSequenceNum())
 			features = featuresQueue_->get<dai::NNData>();

 		auto heatmap = features->getLayerFp16("heatmap");