i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +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

6
corelib/include/rtabmap/core/camera/CameraStereoZed.h
View File

@@ -45,11 +45,11 @@ class RTABMAP_CORE_EXPORT CameraStereoZed :
{ {
public: public:
static bool available(); static bool available();
static int sdkVersion();
public: public:
CameraStereoZed( CameraStereoZed(
int deviceId, 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 quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY
int sensingMode = 0,// 0=STANDARD, 1=FILL int sensingMode = 0,// 0=STANDARD, 1=FILL
int confidenceThr = 100, int confidenceThr = 100,
@@ -61,7 +61,7 @@ public:
int texturenessConfidenceThr = 90); // introduced with ZED SDK 3 int texturenessConfidenceThr = 90); // introduced with ZED SDK 3
CameraStereoZed( CameraStereoZed(
const std::string & svoFilePath, 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 sensingMode = 0,// 0=STANDARD, 1=FILL
int confidenceThr = 100, int confidenceThr = 100,
bool computeOdometry = false, bool computeOdometry = false,

62
corelib/src/camera/CameraStereoZed.cpp
View File

@@ -240,6 +240,17 @@ bool CameraStereoZed::available()
#endif #endif
} }
int CameraStereoZed::sdkVersion()
{
#ifdef RTABMAP_ZED
return ZED_SDK_MAJOR_VERSION;
#else
return -1;
#endif
}
CameraStereoZed::CameraStereoZed( CameraStereoZed::CameraStereoZed(
int deviceId, int deviceId,
int resolution, int resolution,
@@ -274,6 +285,16 @@ CameraStereoZed::CameraStereoZed(
{ {
UDEBUG(""); UDEBUG("");
#ifdef RTABMAP_ZED #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 #if ZED_SDK_MAJOR_VERSION < 3
UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST); UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST); UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
@@ -282,11 +303,15 @@ CameraStereoZed::CameraStereoZed(
#else #else
sl::RESOLUTION res = static_cast<sl::RESOLUTION>(resolution_); sl::RESOLUTION res = static_cast<sl::RESOLUTION>(resolution_);
sl::DEPTH_MODE qual = static_cast<sl::DEPTH_MODE>(quality_); 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(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST);
UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::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); UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST);
#else
UASSERT(sensingMode_ >= 0 && sensingMode_ < 2);
#endif
UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100); UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
UASSERT(texturenessConfidenceThr_ >= 0 && texturenessConfidenceThr_ <=100); UASSERT(texturenessConfidenceThr_ >= 0 && texturenessConfidenceThr_ <=100);
#endif #endif
@@ -334,11 +359,15 @@ CameraStereoZed::CameraStereoZed(
#else #else
sl::RESOLUTION res = static_cast<sl::RESOLUTION>(resolution_); sl::RESOLUTION res = static_cast<sl::RESOLUTION>(resolution_);
sl::DEPTH_MODE qual = static_cast<sl::DEPTH_MODE>(quality_); 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(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST);
UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::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); UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST);
#else
UASSERT(sensingMode_ >= 0 && sensingMode_ < 2);
#endif
UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100); UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
UASSERT(texturenessConfidenceThr_ >= 0 && texturenessConfidenceThr_ <=100); UASSERT(texturenessConfidenceThr_ >= 0 && texturenessConfidenceThr_ <=100);
#endif #endif
@@ -465,7 +494,11 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
} }
sl::CameraInformation infos = zed_->getCameraInformation(); sl::CameraInformation infos = zed_->getCameraInformation();
#if ZED_SDK_MAJOR_VERSION < 4
sl::CalibrationParameters *stereoParams = &(infos.calibration_parameters ); sl::CalibrationParameters *stereoParams = &(infos.calibration_parameters );
#else
sl::CalibrationParameters *stereoParams = &(infos.camera_configuration.calibration_parameters );
#endif
sl::Resolution res = stereoParams->left_cam.image_size; sl::Resolution res = stereoParams->left_cam.image_size;
stereoModel_ = StereoCameraModel( stereoModel_ = StereoCameraModel(
@@ -473,7 +506,11 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
stereoParams->left_cam.fy, stereoParams->left_cam.fy,
stereoParams->left_cam.cx, stereoParams->left_cam.cx,
stereoParams->left_cam.cy, stereoParams->left_cam.cy,
#if ZED_SDK_MAJOR_VERSION < 4
stereoParams->T[0],//baseline stereoParams->T[0],//baseline
#else
stereoParams->getCameraBaseline(),
#endif
this->getLocalTransform(), this->getLocalTransform(),
cv::Size(res.width, res.height)); 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.fy,
stereoParams->left_cam.cx, stereoParams->left_cam.cx,
stereoParams->left_cam.cy, stereoParams->left_cam.cy,
#if ZED_SDK_MAJOR_VERSION < 4
stereoParams->T[0],//baseline stereoParams->T[0],//baseline
#else
stereoParams->getCameraBaseline(),
#endif
(int)res.width, (int)res.width,
(int)res.height, (int)res.height,
this->getLocalTransform().prettyPrint().c_str()); 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) if(infos.camera_model != sl::MODEL::ZED)
#endif #endif
{ {
#if ZED_SDK_MAJOR_VERSION < 4
imuLocalTransform_ = this->getLocalTransform() * zedPoseToTransform(infos.camera_imu_transform).inverse(); 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))", UINFO("IMU local transform: %s (imu2cam=%s))",
imuLocalTransform_.prettyPrint().c_str(), imuLocalTransform_.prettyPrint().c_str(),
zedPoseToTransform(infos.camera_imu_transform).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_) if(publishInterIMU_)
{ {
imuPublishingThread_ = new ZedIMUThread(200, zed_, imuLocalTransform_, true); imuPublishingThread_ = new ZedIMUThread(200, zed_, imuLocalTransform_, true);
@@ -623,8 +671,10 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
#ifdef RTABMAP_ZED #ifdef RTABMAP_ZED
#if ZED_SDK_MAJOR_VERSION < 3 #if ZED_SDK_MAJOR_VERSION < 3
sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, sl::REFERENCE_FRAME_CAMERA); 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); 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 #endif
if(zed_) if(zed_)

9
guilib/src/PreferencesDialog.cpp
View File

@@ -380,6 +380,13 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->comboBox_cameraStereo->setItemData(kSrcStereoZed - kSrcStereo, 0, Qt::UserRole - 1); _ui->comboBox_cameraStereo->setItemData(kSrcStereoZed - kSrcStereo, 0, Qt::UserRole - 1);
_ui->comboBox_odom_sensor->setItemData(2, 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()) if (!CameraStereoTara::available())
{ {
_ui->comboBox_cameraStereo->setItemData(kSrcStereoTara - kSrcStereo, 0, Qt::UserRole - 1); _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_stereo_right_device->setValue(-1);
_ui->spinBox_stereousbcam_streamWidth->setValue(0); _ui->spinBox_stereousbcam_streamWidth->setValue(0);
_ui->spinBox_stereousbcam_streamHeight->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->comboBox_stereoZed_quality->setCurrentIndex(1);
_ui->checkbox_stereoZed_selfCalibration->setChecked(true); _ui->checkbox_stereoZed_selfCalibration->setChecked(true);
_ui->comboBox_stereoZed_sensingMode->setCurrentIndex(0); _ui->comboBox_stereoZed_sensingMode->setCurrentIndex(0);

28
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>0</y> <y>-373</y>
<width>756</width> <width>756</width>
<height>3657</height> <height>3657</height>
</rect> </rect>
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum> <enum>QFrame::Raised</enum>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>22</number> <number>5</number>
</property> </property>
<widget class="QWidget" name="page_22"> <widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0"> <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> <item>
<widget class="QStackedWidget" name="stackedWidget_src"> <widget class="QStackedWidget" name="stackedWidget_src">
<property name="currentIndex"> <property name="currentIndex">
<number>3</number> <number>1</number>
</property> </property>
<widget class="QWidget" name="page_41"> <widget class="QWidget" name="page_41">
<layout class="QVBoxLayout" name="verticalLayout_64"> <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> <item>
<widget class="QStackedWidget" name="stackedWidget_stereo"> <widget class="QStackedWidget" name="stackedWidget_stereo">
<property name="currentIndex"> <property name="currentIndex">
<number>10</number> <number>4</number>
</property> </property>
<widget class="QWidget" name="page_49"> <widget class="QWidget" name="page_49">
<layout class="QVBoxLayout" name="verticalLayout_91"/> <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> <string>ULTRA</string>
</property> </property>
</item> </item>
<item>
<property name="text">
<string>NEURAL</string>
</property>
</item>
</widget> </widget>
</item> </item>
<item row="5" column="1"> <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> <string>HD1080</string>
</property> </property>
</item> </item>
<item>
<property name="text">
<string>HD1200</string>
</property>
</item>
<item> <item>
<property name="text"> <property name="text">
<string>HD720</string> <string>HD720</string>
</property> </property>
</item> </item>
<item>
<property name="text">
<string>SVGA</string>
</property>
</item>
<item> <item>
<property name="text"> <property name="text">
<string>VGA</string> <string>VGA</string>
</property> </property>
</item> </item>
<item>
<property name="text">
<string>AUTO</string>
</property>
</item>
</widget> </widget>
</item> </item>
</layout> </layout>