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

zed 2.0: fixed sensing mode order, fixed conversion from zed pose to transform (when zed odometry is enabled)

Browse Source
This commit is contained in:
matlabbe
2017-04-18 20:16:04 -04:00
parent 732223e832
commit 42fb02c36f
4 changed files with 29 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/include/rtabmap/core/CameraStereo.h
View File

@@ -118,7 +118,7 @@ public:
int deviceId, int deviceId,
int resolution = 2, // 0=HD2K, 1=HD1080, 2=HD720, 3=VGA int resolution = 2, // 0=HD2K, 1=HD1080, 2=HD720, 3=VGA
int quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY int quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY
int sensingMode = 1,// 0=FULL, 1=RAW int sensingMode = 0,// 0=STANDARD, 1=FILL
int confidenceThr = 100, int confidenceThr = 100,
bool computeOdometry = false, bool computeOdometry = false,
float imageRate=0.0f, float imageRate=0.0f,
@@ -127,7 +127,7 @@ public:
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
int sensingMode = 1,// 0=FULL, 1=RAW int sensingMode = 0,// 0=STANDARD, 1=FILL
int confidenceThr = 100, int confidenceThr = 100,
bool computeOdometry = false, bool computeOdometry = false,
float imageRate=0.0f, float imageRate=0.0f,

24
corelib/src/CameraStereo.cpp
View File

@@ -787,7 +787,7 @@ CameraStereoZed::CameraStereoZed(
#ifdef RTABMAP_ZED #ifdef RTABMAP_ZED
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);
UASSERT(sensingMode_ >= sl::SENSING_MODE_FILL && sensingMode_ <sl::SENSING_MODE_LAST); UASSERT(sensingMode_ >= sl::SENSING_MODE_STANDARD && sensingMode_ <sl::SENSING_MODE_LAST);
UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100); UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
#endif #endif
} }
@@ -900,6 +900,16 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
this->getLocalTransform(), this->getLocalTransform(),
cv::Size(res.width, res.height)); cv::Size(res.width, res.height));
UINFO("Calibration: fx=%f, fy=%f, cx=%f, cy=%f, baseline=%f, width=%d, height=%d, transform=%s",
stereoParams->left_cam.fx,
stereoParams->left_cam.fy,
stereoParams->left_cam.cx,
stereoParams->left_cam.cy,
stereoParams->T[0],//baseline
(int)res.width,
(int)res.height,
this->getLocalTransform().prettyPrint().c_str());
return true; return true;
#else #else
UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!"); UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
@@ -956,6 +966,14 @@ static cv::Mat slMat2cvMat(sl::Mat& input) {
} }
#endif #endif
Transform zedPoseToTransform(const sl::Pose & pose)
{
return Transform(
pose.pose_data.m[0], pose.pose_data.m[1], pose.pose_data.m[2], pose.pose_data.m[3],
pose.pose_data.m[4], pose.pose_data.m[5], pose.pose_data.m[6], pose.pose_data.m[7],
pose.pose_data.m[8], pose.pose_data.m[9], pose.pose_data.m[10], pose.pose_data.m[11]);
}
SensorData CameraStereoZed::captureImage(CameraInfo * info) SensorData CameraStereoZed::captureImage(CameraInfo * info)
{ {
SensorData data; SensorData data;
@@ -1012,9 +1030,7 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
int trackingConfidence = pose.pose_confidence; int trackingConfidence = pose.pose_confidence;
if (trackingConfidence) if (trackingConfidence)
{ {
Transform t; info->odomPose = zedPoseToTransform(pose);
for(int i=0;i<16;i++)t.data()[i]=pose.pose_data.m[i];
info->odomPose = t;
if (!info->odomPose.isNull()) if (!info->odomPose.isNull())
{ {
//transform x->forward, y->left, z->up //transform x->forward, y->left, z->up

2
guilib/src/PreferencesDialog.cpp
View File

@@ -1420,7 +1420,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->comboBox_stereoZed_resolution->setCurrentIndex(2); _ui->comboBox_stereoZed_resolution->setCurrentIndex(2);
_ui->comboBox_stereoZed_quality->setCurrentIndex(1); _ui->comboBox_stereoZed_quality->setCurrentIndex(1);
_ui->checkbox_stereoZed_selfCalibration->setChecked(false); _ui->checkbox_stereoZed_selfCalibration->setChecked(false);
_ui->comboBox_stereoZed_sensingMode->setCurrentIndex(1); _ui->comboBox_stereoZed_sensingMode->setCurrentIndex(0);
_ui->spinBox_stereoZed_confidenceThr->setValue(100); _ui->spinBox_stereoZed_confidenceThr->setValue(100);
_ui->checkbox_stereoZed_odom->setChecked(false); _ui->checkbox_stereoZed_odom->setChecked(false);
_ui->lineEdit_zedSvoPath->clear(); _ui->lineEdit_zedSvoPath->clear();

12
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>-675</y> <y>-433</y>
<width>678</width> <width>678</width>
<height>2701</height> <height>2701</height>
</rect> </rect>
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum> <enum>QFrame::Raised</enum>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>15</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,1"> <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -2582,7 +2582,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>0</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">
@@ -3820,12 +3820,12 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<widget class="QComboBox" name="comboBox_stereoZed_sensingMode"> <widget class="QComboBox" name="comboBox_stereoZed_sensingMode">
<item> <item>
<property name="text"> <property name="text">
<string>FILL</string> <string>STANDARD</string>
</property> </property>
</item> </item>
<item> <item>
<property name="text"> <property name="text">
<string>STANDARD</string> <string>FILL</string>
</property> </property>
</item> </item>
</widget> </widget>
@@ -3908,7 +3908,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item row="1" column="1"> <item row="1" column="1">
<widget class="QLabel" name="label_290"> <widget class="QLabel" name="label_290">
<property name="text"> <property name="text">
<string>Quality. If NONE, the disparity is not computed on the GPU.</string> <string>Quality. If NONE, the disparity is not computed on the GPU, but on CPU using OpenCV (see StereoBM panel for parameters).</string>
</property> </property>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>