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

Fixed #197 (handling zed pose confidence == -1)

Browse Source
This commit is contained in:
matlabbe
2017-05-18 17:32:00 -04:00
parent 5223629ab1
commit 48ba55d02c
1 changed files with 18 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/CameraStereo.cpp
+18 -10
View File
@@ -1029,7 +1029,8 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
sl::Pose pose; sl::Pose pose;
zed_->getPosition(pose); zed_->getPosition(pose);
int trackingConfidence = pose.pose_confidence; int trackingConfidence = pose.pose_confidence;
if (trackingConfidence) // FIXME What does pose_confidence == -1 mean?
if (trackingConfidence>0)
{ {
info->odomPose = zedPoseToTransform(pose); info->odomPose = zedPoseToTransform(pose);
if (!info->odomPose.isNull()) if (!info->odomPose.isNull())
@@ -1037,24 +1038,31 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
//transform x->forward, y->left, z->up //transform x->forward, y->left, z->up
Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0); Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
info->odomPose = opticalTransform * info->odomPose * opticalTransform.inverse(); info->odomPose = opticalTransform * info->odomPose * opticalTransform.inverse();
}
if (lost_) if (lost_)
{ {
info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose
lost_ = false; lost_ = false;
UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f); 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
{ {
info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
lost_ = true; lost_ = true;
UWARN("ZED lost!"); UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
} }
} }
} }