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

ZED SDK 4 support (#1013)

Browse Source
This commit is contained in:
matlabbe
2023-04-16 18:47:15 -07:00
parent e300d4c5c1
commit e54195c47f
4 changed files with 91 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/camera/CameraStereoZed.h
+3 -3
View File
@@ -45,11 +45,11 @@ class RTABMAP_CORE_EXPORT CameraStereoZed :
{
public:
static bool available();
static int sdkVersion();
public:
CameraStereoZed(
int deviceId,
int resolution = 2, // 0=HD2K, 1=HD1080, 2=HD720, 3=VGA
int resolution = 6, // 0=HD2K, 1=HD1080, 2=HD1200, 3=HD720, 4=SVGA, 5=VGA, 6=AUTO
int quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY
int sensingMode = 0,// 0=STANDARD, 1=FILL
int confidenceThr = 100,
@@ -61,7 +61,7 @@ public:
int texturenessConfidenceThr = 90); // introduced with ZED SDK 3
CameraStereoZed(
const std::string & svoFilePath,
int quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY
int quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY, 3=NEURAL
int sensingMode = 0,// 0=STANDARD, 1=FILL
int confidenceThr = 100,
bool computeOdometry = false,
corelib/src/camera/CameraStereoZed.cpp
+56 -6
View File
@@ -240,6 +240,17 @@ bool CameraStereoZed::available()
#endif
}
int CameraStereoZed::sdkVersion()
{
#ifdef RTABMAP_ZED
return ZED_SDK_MAJOR_VERSION;
#else
return -1;
#endif
}
CameraStereoZed::CameraStereoZed(
int deviceId,
int resolution,
@@ -274,6 +285,16 @@ CameraStereoZed::CameraStereoZed(
{
UDEBUG("");
#ifdef RTABMAP_ZED
#if ZED_SDK_MAJOR_VERSION < 4
if(resolution_ == 3)
{
resolution_ = 2;
}
else if(resolution_ == 5)
{
resolution_ = 3;
}
#endif
#if ZED_SDK_MAJOR_VERSION < 3
UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
@@ -282,11 +303,15 @@ CameraStereoZed::CameraStereoZed(
#else
sl::RESOLUTION res = static_cast<sl::RESOLUTION>(resolution_);
sl::DEPTH_MODE qual = static_cast<sl::DEPTH_MODE>(quality_);
sl::SENSING_MODE sens = static_cast<sl::SENSING_MODE>(sensingMode_);
UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST);
UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::LAST);
#if ZED_SDK_MAJOR_VERSION < 4
sl::SENSING_MODE sens = static_cast<sl::SENSING_MODE>(sensingMode_);
UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST);
#else
UASSERT(sensingMode_ >= 0 && sensingMode_ < 2);
#endif
UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
UASSERT(texturenessConfidenceThr_ >= 0 && texturenessConfidenceThr_ <=100);
#endif
@@ -334,11 +359,15 @@ CameraStereoZed::CameraStereoZed(
#else
sl::RESOLUTION res = static_cast<sl::RESOLUTION>(resolution_);
sl::DEPTH_MODE qual = static_cast<sl::DEPTH_MODE>(quality_);
sl::SENSING_MODE sens = static_cast<sl::SENSING_MODE>(sensingMode_);
UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST);
UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::LAST);
#if ZED_SDK_MAJOR_VERSION < 4
sl::SENSING_MODE sens = static_cast<sl::SENSING_MODE>(sensingMode_);
UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST);
#else
UASSERT(sensingMode_ >= 0 && sensingMode_ < 2);
#endif
UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
UASSERT(texturenessConfidenceThr_ >= 0 && texturenessConfidenceThr_ <=100);
#endif
@@ -465,7 +494,11 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
}
sl::CameraInformation infos = zed_->getCameraInformation();
#if ZED_SDK_MAJOR_VERSION < 4
sl::CalibrationParameters *stereoParams = &(infos.calibration_parameters );
#else
sl::CalibrationParameters *stereoParams = &(infos.camera_configuration.calibration_parameters );
#endif
sl::Resolution res = stereoParams->left_cam.image_size;
stereoModel_ = StereoCameraModel(
@@ -473,7 +506,11 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
stereoParams->left_cam.fy,
stereoParams->left_cam.cx,
stereoParams->left_cam.cy,
#if ZED_SDK_MAJOR_VERSION < 4
stereoParams->T[0],//baseline
#else
stereoParams->getCameraBaseline(),
#endif
this->getLocalTransform(),
cv::Size(res.width, res.height));
@@ -482,7 +519,11 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
stereoParams->left_cam.fy,
stereoParams->left_cam.cx,
stereoParams->left_cam.cy,
#if ZED_SDK_MAJOR_VERSION < 4
stereoParams->T[0],//baseline
#else
stereoParams->getCameraBaseline(),
#endif
(int)res.width,
(int)res.height,
this->getLocalTransform().prettyPrint().c_str());
@@ -493,11 +534,18 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
if(infos.camera_model != sl::MODEL::ZED)
#endif
{
#if ZED_SDK_MAJOR_VERSION < 4
imuLocalTransform_ = this->getLocalTransform() * zedPoseToTransform(infos.camera_imu_transform).inverse();
#else
imuLocalTransform_ = this->getLocalTransform() * zedPoseToTransform(infos.sensors_configuration.camera_imu_transform).inverse();
#endif
UINFO("IMU local transform: %s (imu2cam=%s))",
imuLocalTransform_.prettyPrint().c_str(),
zedPoseToTransform(infos.camera_imu_transform).prettyPrint().c_str());
imuLocalTransform_.prettyPrint().c_str(),
#if ZED_SDK_MAJOR_VERSION < 4
zedPoseToTransform(infos.camera_imu_transform).prettyPrint().c_str());
#else
zedPoseToTransform(infos.sensors_configuration.camera_imu_transform).prettyPrint().c_str());
#endif
if(publishInterIMU_)
{
imuPublishingThread_ = new ZedIMUThread(200, zed_, imuLocalTransform_, true);
@@ -623,8 +671,10 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
#ifdef RTABMAP_ZED
#if ZED_SDK_MAJOR_VERSION < 3
sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, sl::REFERENCE_FRAME_CAMERA);
#else
#elif ZED_SDK_MAJOR_VERSION < 4
sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, confidenceThr_, texturenessConfidenceThr_, sl::REFERENCE_FRAME::CAMERA);
#else
sl::RuntimeParameters rparam(quality_ > 0, sensingMode_ == 1, confidenceThr_, texturenessConfidenceThr_, sl::REFERENCE_FRAME::CAMERA);
#endif
if(zed_)
guilib/src/PreferencesDialog.cpp
+8 -1
View File
@@ -380,6 +380,13 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->comboBox_cameraStereo->setItemData(kSrcStereoZed - kSrcStereo, 0, Qt::UserRole - 1);
_ui->comboBox_odom_sensor->setItemData(2, 0, Qt::UserRole - 1);
}
else if(CameraStereoZed::sdkVersion() < 4)
{
_ui->comboBox_stereoZed_resolution->setItemData(2, 0, Qt::UserRole - 1);
_ui->comboBox_stereoZed_resolution->setItemData(4, 0, Qt::UserRole - 1);
_ui->comboBox_stereoZed_resolution->setItemData(6, 0, Qt::UserRole - 1);
_ui->comboBox_stereoZed_quality->setItemData(3, 0, Qt::UserRole - 1);
}
if (!CameraStereoTara::available())
{
_ui->comboBox_cameraStereo->setItemData(kSrcStereoTara - kSrcStereo, 0, Qt::UserRole - 1);
@@ -2033,7 +2040,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->spinBox_stereo_right_device->setValue(-1);
_ui->spinBox_stereousbcam_streamWidth->setValue(0);
_ui->spinBox_stereousbcam_streamHeight->setValue(0);
_ui->comboBox_stereoZed_resolution->setCurrentIndex(2);
_ui->comboBox_stereoZed_resolution->setCurrentIndex(CameraStereoZed::sdkVersion()<4?3:6);
_ui->comboBox_stereoZed_quality->setCurrentIndex(1);
_ui->checkbox_stereoZed_selfCalibration->setChecked(true);
_ui->comboBox_stereoZed_sensingMode->setCurrentIndex(0);
guilib/src/ui/preferencesDialog.ui
+24 -4
View File
@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<y>-373</y>
<width>756</width>
<height>3657</height>
</rect>
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>22</number>
<number>5</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -3188,7 +3188,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item>
<widget class="QStackedWidget" name="stackedWidget_src">
<property name="currentIndex">
<number>3</number>
<number>1</number>
</property>
<widget class="QWidget" name="page_41">
<layout class="QVBoxLayout" name="verticalLayout_64">
@@ -4971,7 +4971,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item>
<widget class="QStackedWidget" name="stackedWidget_stereo">
<property name="currentIndex">
<number>10</number>
<number>4</number>
</property>
<widget class="QWidget" name="page_49">
<layout class="QVBoxLayout" name="verticalLayout_91"/>
@@ -5240,6 +5240,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>ULTRA</string>
</property>
</item>
<item>
<property name="text">
<string>NEURAL</string>
</property>
</item>
</widget>
</item>
<item row="5" column="1">
@@ -5398,16 +5403,31 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>HD1080</string>
</property>
</item>
<item>
<property name="text">
<string>HD1200</string>
</property>
</item>
<item>
<property name="text">
<string>HD720</string>
</property>
</item>
<item>
<property name="text">
<string>SVGA</string>
</property>
</item>
<item>
<property name="text">
<string>VGA</string>
</property>
</item>
<item>
<property name="text">
<string>AUTO</string>
</property>
</item>
</widget>
</item>
</layout>