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

Updated CameraStereoZed to latest zed sdk (2.4)

Browse Source
This commit is contained in:
matlabbe
2018-06-03 02:40:19 -04:00
parent 432b0dc6f6
commit 9f22a2b1f8
2 changed files with 47 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
corelib/src/CameraStereo.cpp
View File

@@ -855,7 +855,7 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
param.depth_mode=(sl::DEPTH_MODE)quality_; param.depth_mode=(sl::DEPTH_MODE)quality_;
param.coordinate_units=sl::UNIT_METER; param.coordinate_units=sl::UNIT_METER;
param.coordinate_system=(sl::COORDINATE_SYSTEM)sl::COORDINATE_SYSTEM_IMAGE ; param.coordinate_system=(sl::COORDINATE_SYSTEM)sl::COORDINATE_SYSTEM_IMAGE ;
param.sdk_verbose=false; param.sdk_verbose=true;
param.sdk_gpu_id=-1; param.sdk_gpu_id=-1;
param.depth_minimum_distance=-1; param.depth_minimum_distance=-1;
param.camera_disable_self_calib=!selfCalibration_; param.camera_disable_self_calib=!selfCalibration_;
@@ -877,7 +877,7 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
if(r!=sl::ERROR_CODE::SUCCESS) if(r!=sl::ERROR_CODE::SUCCESS)
{ {
UERROR("Camera initialization failed: \"%s\"", errorCode2str(r).c_str()); UERROR("Camera initialization failed: \"%s\"", toString(r).c_str());
delete zed_; delete zed_;
zed_ = 0; zed_ = 0;
return false; return false;
@@ -888,19 +888,26 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
quality_, sl::UNIT_METER, sl::COORDINATE_SYSTEM_IMAGE , selfCalibration_?"true":"false"); quality_, sl::UNIT_METER, sl::COORDINATE_SYSTEM_IMAGE , selfCalibration_?"true":"false");
UDEBUG(""); UDEBUG("");
zed_->setConfidenceThreshold(confidenceThr_); if(quality_!=sl::DEPTH_MODE_NONE)
{
zed_->setConfidenceThreshold(confidenceThr_);
}
if (computeOdometry_) if (computeOdometry_)
{ {
sl::TrackingParameters tparam; sl::TrackingParameters tparam;
tparam.enable_spatial_memory=false; tparam.enable_spatial_memory=false;
zed_->enableTracking(tparam); zed_->enableTracking(tparam);
if(r!=sl::ERROR_CODE::SUCCESS)
{
UERROR("Camera tracking initialization failed: \"%s\"", toString(r).c_str());
}
} }
sl::CameraInformation infos = zed_->getCameraInformation(); sl::CameraInformation infos = zed_->getCameraInformation();
sl::CalibrationParameters *stereoParams = &(infos.calibration_parameters ); sl::CalibrationParameters *stereoParams = &(infos.calibration_parameters );
sl::Resolution res = stereoParams->left_cam.image_size; sl::Resolution res = stereoParams->left_cam.image_size;
stereoModel_ = StereoCameraModel( stereoModel_ = StereoCameraModel(
stereoParams->left_cam.fx, stereoParams->left_cam.fx,
stereoParams->left_cam.fy, stereoParams->left_cam.fy,
@@ -1032,28 +1039,37 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
if (computeOdometry_ && info) if (computeOdometry_ && info)
{ {
sl::Pose pose; sl::Pose pose;
zed_->getPosition(pose); sl::TRACKING_STATE tracking_state = zed_->getPosition(pose);
int trackingConfidence = pose.pose_confidence; if (tracking_state == sl::TRACKING_STATE_OK)
// FIXME What does pose_confidence == -1 mean?
if (trackingConfidence>0)
{ {
info->odomPose = zedPoseToTransform(pose); int trackingConfidence = pose.pose_confidence;
if (!info->odomPose.isNull()) // FIXME What does pose_confidence == -1 mean?
if (trackingConfidence>0)
{ {
//transform x->forward, y->left, z->up info->odomPose = zedPoseToTransform(pose);
Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0); if (!info->odomPose.isNull())
info->odomPose = opticalTransform * info->odomPose * opticalTransform.inverse();
if (lost_)
{ {
info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose //transform x->forward, y->left, z->up
lost_ = false; Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f); info->odomPose = opticalTransform * info->odomPose * opticalTransform.inverse();
if (lost_)
{
info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose
lost_ = false;
UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f);
}
else
{
info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence);
UDEBUG("Run %s (var=%f)", info->odomPose.prettyPrint().c_str(), 1.0f / float(trackingConfidence));
}
} }
else else
{ {
info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence); info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
UDEBUG("Run %s (var=%f)", info->odomPose.prettyPrint().c_str(), 1.0f / float(trackingConfidence)); lost_ = true;
UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
} }
} }
else else
@@ -1065,9 +1081,7 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
} }
else else
{ {
info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost UWARN("Tracking not ok: state=\"%s\"", toString(tracking_state).c_str());
lost_ = true;
UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
} }
} }
} }

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

@@ -63,9 +63,9 @@
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>-567</y>
<width>681</width> <width>680</width>
<height>2811</height> <height>2812</height>
</rect> </rect>
</property> </property>
<layout class="QVBoxLayout" name="verticalLayout_16"> <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum> <enum>QFrame::Raised</enum>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>12</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">
@@ -3958,7 +3958,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>5</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">
@@ -4299,6 +4299,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>QUALITY</string> <string>QUALITY</string>
</property> </property>
</item> </item>
<item>
<property name="text">
<string>ULTRA</string>
</property>
</item>
</widget> </widget>
</item> </item>
<item row="4" column="1"> <item row="4" column="1">