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

Update IMU local transform

Browse Source
This commit is contained in:
Borong Yuan
2023-05-25 00:01:55 +08:00
parent 42fbdda567
commit 5592a1ebfe
1 changed files with 36 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -57,7 +57,9 @@ CameraDepthAI::CameraDepthAI(
depthConfidence_(200), depthConfidence_(200),
resolution_(resolution), resolution_(resolution),
imuFirmwareUpdate_(false), imuFirmwareUpdate_(false),
imuPublished_(true) imuPublished_(true),
dotProjectormA_(0.0),
floodLightmA_(200.0)
#endif #endif
{ {
#ifdef RTABMAP_DEPTHAI #ifdef RTABMAP_DEPTHAI
@@ -264,12 +266,28 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
// matrix[0][0], matrix[0][1], matrix[0][2], matrix[0][3], // 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[1][0], matrix[1][1], matrix[1][2], matrix[1][3],
// matrix[2][0], matrix[2][1], matrix[2][2], matrix[2][3]); // matrix[2][0], matrix[2][1], matrix[2][2], matrix[2][3]);
// Hard-coded: x->down, y->left, z->forward auto eeprom = calibHandler.getEepromData();
imuLocalTransform_ = Transform( if(eeprom.boardName == "OAK-D")
0, 0, 1, 0, {
0, 1, 0, 0, // Hard-coded: x->down, y->left, z->forward
-1 ,0, 0, 0); imuLocalTransform_ = Transform(
UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str()); 0, 0, 1, 0,
0, 1, 0, -0.0525,
-1, 0, 0, -0.0137);
}
else if(eeprom.boardName == "DM9098")
{
// Hard-coded: x->down, y->right, z->backward
imuLocalTransform_ = Transform(
0, 0, -1, -0.007,
0, -1, 0, -0.0754,
-1, 0, 0, -0.0026);
}
else
{
UWARN("Unknown boardName! Disabling IMU!");
imuPublished_ = false;
}
} }
else else
{ {
@@ -278,17 +296,18 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
if(imuPublished_) if(imuPublished_)
{ {
UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str());
imuQueue_ = device_->getOutputQueue("imu", 50, false); imuQueue_ = device_->getOutputQueue("imu", 50, false);
} }
leftQueue_ = device_->getOutputQueue("rectified_left", 1, false); leftQueue_ = device_->getOutputQueue("rectified_left", 1, false);
rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 1, false); rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 1, false);
std::vector<std::tuple<std::string, int, int>> irDrivers = device_->getIrDrivers(); std::vector<std::tuple<std::string, int, int>> irDrivers = device_->getIrDrivers();
if(!irDrivers.empty()) if(!irDrivers.empty())
{ {
device_->setIrLaserDotProjectorBrightness(dotProjectormA_); device_->setIrLaserDotProjectorBrightness(dotProjectormA_);
device_->setIrFloodLightBrightness(floodLightmA_); device_->setIrFloodLightBrightness(floodLightmA_);
} }
uSleep(2000); // avoid bad frames on start uSleep(2000); // avoid bad frames on start
@@ -382,11 +401,11 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
break; break;
} }
} }
if((UTimer::now() - stampStart) > 0.01) // if((UTimer::now() - stampStart) > 0.01)
{ // {
UWARN("Could not received IMU after 10 ms! Disabling IMU!"); // UWARN("Could not received IMU after 10 ms! Disabling IMU!");
imuPublished_ = false; // imuPublished_ = false;
} // }
} }
cv::Vec3d acc, gyro; cv::Vec3d acc, gyro;