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: Fixed some warnings (#961)

Browse Source
This commit is contained in:
matlabbe
2023-01-28 16:33:24 -08:00
parent ff61266430
commit 128d3b0d0a
1 changed files with 21 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -732,38 +732,35 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
{
int trackingConfidence = pose.pose_confidence;
// FIXME What does pose_confidence == -1 mean?
if (trackingConfidence>0)
info->odomPose = zedPoseToTransform(pose);
if (!info->odomPose.isNull())
{
info->odomPose = zedPoseToTransform(pose);
if (!info->odomPose.isNull())
//transform from:
// x->right, y->down, z->forward
//to:
// x->forward, y->left, z->up
info->odomPose = this->getLocalTransform() * info->odomPose * this->getLocalTransform().inverse();
if(force3DoF_)
{
//transform from:
// x->right, y->down, z->forward
//to:
// x->forward, y->left, z->up
info->odomPose = this->getLocalTransform() * info->odomPose * this->getLocalTransform().inverse();
if(force3DoF_)
{
info->odomPose = info->odomPose.to3DoF();
}
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));
}
info->odomPose = info->odomPose.to3DoF();
}
else
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 if(trackingConfidence==0)
{
info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
lost_ = true;
UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
}
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
{