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

62
corelib/src/camera/CameraStereoZed.cpp
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_)