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

update imu callback

Browse Source
This commit is contained in:
Borong Yuan
2023-07-17 23:35:03 +08:00
parent 0b4f5d2925
commit d391f237eb
1 changed files with 20 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/camera/CameraDepthAI.cpp
+20 -23
View File
@@ -477,32 +477,29 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
{ {
imuLocalTransform_ = this->getLocalTransform() * imuLocalTransform_; imuLocalTransform_ = this->getLocalTransform() * imuLocalTransform_;
UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str()); UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str());
device_->getOutputQueue("imu", 50, false)->addCallback([this](std::shared_ptr<dai::ADatatype> callback) { device_->getOutputQueue("imu", 50, false)->addCallback([this](const std::shared_ptr<dai::ADatatype> data) {
if(dynamic_cast<dai::IMUData*>(callback.get()) != nullptr) auto imuData = std::dynamic_pointer_cast<dai::IMUData>(data);
auto imuPackets = imuData->packets;
for(auto& imuPacket : imuPackets)
{ {
dai::IMUData* imuData = static_cast<dai::IMUData*>(callback.get()); auto& acceleroValues = imuPacket.acceleroMeter;
auto imuPackets = imuData->packets; auto& gyroValues = imuPacket.gyroscope;
double accStamp = std::chrono::duration<double>(acceleroValues.getTimestampDevice().time_since_epoch()).count();
double gyroStamp = std::chrono::duration<double>(gyroValues.getTimestampDevice().time_since_epoch()).count();
for(auto& imuPacket : imuPackets) if(publishInterIMU_)
{ {
auto& acceleroValues = imuPacket.acceleroMeter; IMU imu(cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z), cv::Mat::eye(3,3,CV_64FC1),
auto& gyroValues = imuPacket.gyroscope; cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z), cv::Mat::eye(3,3,CV_64FC1),
double accStamp = std::chrono::duration<double>(acceleroValues.getTimestampDevice().time_since_epoch()).count(); imuLocalTransform_);
double gyroStamp = std::chrono::duration<double>(gyroValues.getTimestampDevice().time_since_epoch()).count(); UEventsManager::post(new IMUEvent(imu, (accStamp+gyroStamp)/2));
}
if(publishInterIMU_) else
{ {
IMU imu(cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z), cv::Mat::eye(3,3,CV_64FC1), UScopeMutex lock(imuMutex_);
cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z), cv::Mat::eye(3,3,CV_64FC1), accBuffer_.emplace_hint(accBuffer_.end(), std::make_pair(accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z)));
imuLocalTransform_); gyroBuffer_.emplace_hint(gyroBuffer_.end(), std::make_pair(gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z)));
UEventsManager::post(new IMUEvent(imu, (accStamp+gyroStamp)/2));
}
else
{
UScopeMutex lock(imuMutex_);
accBuffer_.emplace_hint(accBuffer_.end(), std::make_pair(accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z)));
gyroBuffer_.emplace_hint(gyroBuffer_.end(), std::make_pair(gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z)));
}
} }
} }
}); });