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

OAK bug fixes and improvements (#1143)

Browse Source 
* *fix distortion correction problem of small FOV devices
*remove deprecated IMU firmware update API
*add useSpecTranslation option

* reduce depth image noise and simplify calculation for compressed transport
This commit is contained in:
Borong Yuan
2023-10-08 11:40:30 +08:00
committed by GitHub
parent 2aa139b579
commit f06cc0b17f
4 changed files with 37 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -57,8 +57,8 @@ CameraDepthAI::CameraDepthAI(
outputDepth_(false),
depthConfidence_(200),
resolution_(resolution),
useSpecTranslation_(false),
alphaScaling_(0.0),
imuFirmwareUpdate_(false),
imuPublished_(true),
publishInterIMU_(false),
dotProjectormA_(0.0),
@@ -100,19 +100,19 @@ void CameraDepthAI::setOutputDepth(bool enabled, int confidence)
#endif
}
void CameraDepthAI::setAlphaScaling(float alphaScaling)
void CameraDepthAI::setUseSpecTranslation(bool useSpecTranslation)
{
#ifdef RTABMAP_DEPTHAI
alphaScaling_ = alphaScaling;
useSpecTranslation_ = useSpecTranslation;
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
}
void CameraDepthAI::setIMUFirmwareUpdate(bool enabled)
void CameraDepthAI::setAlphaScaling(float alphaScaling)
{
#ifdef RTABMAP_DEPTHAI
imuFirmwareUpdate_ = enabled;
alphaScaling_ = alphaScaling;
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
@@ -315,6 +315,9 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
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)
stereo->setAlphaScaling(alphaScaling_);
stereo->initialConfig.setConfidenceThreshold(depthConfidence_);
@@ -340,9 +343,14 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
depthOrRightEnc->setDefaultProfilePreset(monoRight->getFps(), dai::VideoEncoderProperties::Profile::MJPEG);
stereo->rectifiedLeft.link(leftEnc->input);
if(outputDepth_)
{
depthOrRightEnc->setQuality(100);
stereo->disparity.link(depthOrRightEnc->input);
}
else
{
stereo->rectifiedRight.link(depthOrRightEnc->input);
}
leftEnc->bitstream.link(xoutLeft->input);
depthOrRightEnc->bitstream.link(xoutDepthOrRight->input);
}
@@ -374,8 +382,6 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
// Link plugins IMU -> XLINK
imu->out.link(xoutIMU->input);
imu->enableFirmwareUpdate(imuFirmwareUpdate_);
}
if(detectFeatures_ == 1)
@@ -431,7 +437,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
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 baseline = calibHandler.getBaselineDistance()/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);
stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize_);
@@ -568,11 +574,9 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
depthOrRight = cv::imdecode(rectifRightOrDepth->getData(), cv::IMREAD_GRAYSCALE);
if(outputDepth_)
{
cv::Mat depth(targetSize_, CV_16UC1);
depth.forEach<uint16_t>([&](uint16_t& pixel, const int * position) -> void {
pixel = stereoModel_.computeDepth(depthOrRight.at<uint8_t>(position))*1000;
});
depthOrRight = depth;
cv::Mat disp;
depthOrRight.convertTo(disp, CV_16UC1);
cv::divide(-stereoModel_.right().Tx() * 1000, disp, depthOrRight);
}
}
else