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

depthai: fixed app freezing if oakd device (oak-d lite) doesn't have imu and imu is requested.

Browse Source
This commit is contained in:
matlabbe
2022-10-24 11:13:27 -07:00
parent a8cdf1f940
commit df888962f4
3 changed files with 45 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -61,7 +61,7 @@ CameraDepthAI::CameraDepthAI(
#endif #endif
{ {
#ifdef RTABMAP_DEPTHAI #ifdef RTABMAP_DEPTHAI
UASSERT(resolution_>=0 && resolution_<=2); UASSERT(resolution_>=(int)dai::MonoCameraProperties::SensorResolution::THE_720_P && resolution_<=(int)dai::MonoCameraProperties::SensorResolution::THE_1200_P);
#endif #endif
} }
@@ -149,7 +149,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
// look for calibration files // look for calibration files
stereoModel_ = StereoCameraModel(); stereoModel_ = StereoCameraModel();
cv::Size targetSize(resolution_<2?1280:640, resolution_==0?720:resolution_==1?800:400); cv::Size targetSize(resolution_<2?1280:resolution_==4?1920:640, resolution_==0?720:resolution_==1?800:resolution_==2?400:resolution_==3?480:1200);
dai::Pipeline p; dai::Pipeline p;
auto monoLeft = p.create<dai::node::MonoCamera>(); auto monoLeft = p.create<dai::node::MonoCamera>();
@@ -337,34 +337,37 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
double stampStart = UTimer::now(); double stampStart = UTimer::now();
while(imuPublished_ && imuQueue_.get()) while(imuPublished_ && imuQueue_.get())
{ {
auto imuData = imuQueue_->get<dai::IMUData>(); if(imuQueue_->has())
auto imuPackets = imuData->packets;
double accStamp = 0.0;
double gyroStamp = 0.0;
for(auto& imuPacket : imuPackets) {
auto& acceleroValues = imuPacket.acceleroMeter;
auto& gyroValues = imuPacket.gyroscope;
accStamp = double(acceleroValues.timestamp.get().time_since_epoch().count())/10e8;
gyroStamp = double(gyroValues.timestamp.get().time_since_epoch().count())/10e8;
accBuffer_.insert(accBuffer_.end(), std::make_pair(accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z)));
gyroBuffer_.insert(gyroBuffer_.end(), std::make_pair(gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z)));
if(accBuffer_.size() > 1000)
{
accBuffer_.erase(accBuffer_.begin());
}
if(gyroBuffer_.size() > 1000)
{
gyroBuffer_.erase(gyroBuffer_.begin());
}
++added;
}
if(accStamp >= stamp && gyroStamp >= stamp)
{ {
break; auto imuData = imuQueue_->get<dai::IMUData>();
auto imuPackets = imuData->packets;
double accStamp = 0.0;
double gyroStamp = 0.0;
for(auto& imuPacket : imuPackets) {
auto& acceleroValues = imuPacket.acceleroMeter;
auto& gyroValues = imuPacket.gyroscope;
accStamp = double(acceleroValues.timestamp.get().time_since_epoch().count())/10e8;
gyroStamp = double(gyroValues.timestamp.get().time_since_epoch().count())/10e8;
accBuffer_.insert(accBuffer_.end(), std::make_pair(accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z)));
gyroBuffer_.insert(gyroBuffer_.end(), std::make_pair(gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z)));
if(accBuffer_.size() > 1000)
{
accBuffer_.erase(accBuffer_.begin());
}
if(gyroBuffer_.size() > 1000)
{
gyroBuffer_.erase(gyroBuffer_.begin());
}
++added;
}
if(accStamp >= stamp && gyroStamp >= stamp)
{
break;
}
} }
else 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;

3
guilib/src/PreferencesDialog.cpp
View File

@@ -3931,9 +3931,10 @@ void PreferencesDialog::selectSourceDriver(Src src, int variant)
// disable IMU filtering (zed sends already quaternion) // disable IMU filtering (zed sends already quaternion)
_ui->comboBox_imuFilter_strategy->setCurrentIndex(0); _ui->comboBox_imuFilter_strategy->setCurrentIndex(0);
} }
else if(src == kSrcStereoDepthAI) // OAK-D, OAK-D Lite else if(src == kSrcStereoDepthAI) // OAK-D (variant==1), OAK-D Lite (variant==0)
{ {
_ui->checkBox_depthai_imu_published->setChecked(variant == 1); _ui->checkBox_depthai_imu_published->setChecked(variant == 1);
_ui->comboBox_depthai_resolution->setCurrentIndex(variant == 1?1:3);
} }
} }
else if(src >= kSrcRGB && src<kSrcDatabase) else if(src >= kSrcRGB && src<kSrcDatabase)

14
guilib/src/ui/preferencesDialog.ui
View File

@@ -63,7 +63,7 @@
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>-548</y> <y>-543</y>
<width>756</width> <width>756</width>
<height>3657</height> <height>3657</height>
</rect> </rect>
@@ -5815,7 +5815,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item row="0" column="0"> <item row="0" column="0">
<widget class="QComboBox" name="comboBox_depthai_resolution"> <widget class="QComboBox" name="comboBox_depthai_resolution">
<property name="currentIndex"> <property name="currentIndex">
<number>1</number> <number>0</number>
</property> </property>
<property name="sizeAdjustPolicy"> <property name="sizeAdjustPolicy">
<enum>QComboBox::AdjustToContents</enum> <enum>QComboBox::AdjustToContents</enum>
@@ -5835,6 +5835,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>400p</string> <string>400p</string>
</property> </property>
</item> </item>
<item>
<property name="text">
<string>480p</string>
</property>
</item>
<item>
<property name="text">
<string>1200p</string>
</property>
</item>
</widget> </widget>
</item> </item>
<item row="2" column="0"> <item row="2" column="0">