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

CameraDepthAI: fixed imu transform and depth mode with latest depthai-core version

Browse Source
This commit is contained in:
matlabbe
2022-03-06 22:25:01 -05:00
parent 1e82fd3110
commit 4321c3040a
1 changed files with 13 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -140,7 +140,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
auto xoutDepthOrRight = p.create<dai::node::XLinkOut>();
auto xoutIMU = p.create<dai::node::XLinkOut>();
// XLinkOut
// XLinkOut
xoutLeft->setStreamName("rectified_left");
xoutDepthOrRight->setStreamName(outputDepth_?"depth":"rectified_right");
xoutIMU->setStreamName("imu");
@@ -158,9 +158,9 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
// StereoDepth
stereo->initialConfig.setConfidenceThreshold(depthConfidence_);
stereo->initialConfig.setLeftRightCheckThreshold(5);
stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
stereo->setRectifyMirrorFrame(false);
stereo->setLeftRightCheck(false);
stereo->setLeftRightCheck(true);
stereo->setSubpixel(false);
stereo->setExtendedDisparity(false);
@@ -170,7 +170,11 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
if(outputDepth_)
{
stereo->rectifiedLeft.link(xoutLeft->input);
// Depth is registered to right image by default, so subscribe to right image when depth is used
if(outputDepth_)
stereo->rectifiedRight.link(xoutLeft->input);
else
stereo->rectifiedLeft.link(xoutLeft->input);
stereo->depth.link(xoutDepthOrRight->input);
}
else
@@ -206,17 +210,17 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
stereoModel_ = StereoCameraModel(device_->getMxId(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize);
// 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::LEFT);
//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]);
// Hard-coded acc: x->left, y->up, z->forward
// Hard-coded gyro: x->down, y->left, z->forward
// Hard-coded: x->down, y->left, z->forward
imuLocalTransform_ = Transform(
0, 0, 1, 0,
1, 0, 0, 0,
0 ,1, 0, 0);
0, 0, 1, 0,
0, 1, 0, 0,
-1 ,0, 0, 0);
UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str());
leftQueue_ = device_->getOutputQueue("rectified_left", 8, false);
@@ -279,7 +283,6 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
}
else
{
cv::flip(depthOrRight, depthOrRight, 1);
data = SensorData(left, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);
}
@@ -408,10 +411,6 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
UWARN("Could not find gyro data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
}
}
// Rotate gyro frame (x->down, y->left, z->forward) in acc frame (x->left, y->up, z->forward)
double tmp = gyro[0];
gyro[0] = gyro[1];
gyro[1] = -tmp;
}
if(valid)