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: 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; int trackingConfidence = pose.pose_confidence;
// FIXME What does pose_confidence == -1 mean? // FIXME What does pose_confidence == -1 mean?
if (trackingConfidence>0) info->odomPose = zedPoseToTransform(pose);
if (!info->odomPose.isNull())
{ {
info->odomPose = zedPoseToTransform(pose); //transform from:
if (!info->odomPose.isNull()) // 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: info->odomPose = info->odomPose.to3DoF();
// 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));
}
} }
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 info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
lost_ = true; lost_ = true;
UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence); 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 else
{ {