Zed-ros-wrapper: zed.resetTracking decrease the camera frame rate. from 30 to 24;

[Garfield-kh]

In my code, I count the time when zed.pose_confidence > 40;
after the counter more than 20;
I ask the camera to do zed.resetTracking;
but I found that it freezes the camera and decrease the frame rate.
I have 2 zed_mini and 1 zed. zed works okay but zed_mini cannot get frequency of 30.
I searched and found at https://github.com/stereolabs/zed-ros-wrapper/issues/161; but the solution update SDK 2.5.1 does not solve my problem.
Seek for help plz.
Thanks.

[Myzhar]

Hi @Garfield-kh,
can you post more information about the system on which you are having this problem?
How are you performing the "resetTracking"? Are you using an initial guess for the position or do you reset to the origin?

[Garfield-kh]

=====================main.cpp=========================
int main(int argc, char **argv) {

    //read launch file parameters and setup node handles
    //--------------------------------------------------

    ros::init(argc, argv, "zed_tracking");
    ROS_INFO("ZED_TRACKING Node initialized");
    ros::NodeHandle nh;
    ros::NodeHandle nh_ns("~");
//    LogHelper::getInstance().assignNodeHandle(nh);
    CameraForRos myCamera(nh_ns,nh);
    myCamera.start();
    ROS_INFO("back to main thread !!!!!!!!!!!!!!!!!!!!!");
    ros::spin();


    return 0;
}

======================CameraForRos.cpp====================
/*
 * cameraForRos.cpp
 *
#include "cameraForRos.h"
using namespace sl;


CameraForRos::CameraForRos(ros::NodeHandle nh_ns, ros::NodeHandle nh):
    _loopRate(200),
    //_resolution(sl::zed::HD720),
    _resolution(RESOLUTION_HD720),
    _rate(15),
    _enable_spatial_mem(1),
    _depth_mode(0),
    _depth_minimum_distance(0.7),
    _depth_maximum_distance(15),
    _confidence_value(100),
    _odometry_DB(""),
    _odometry_topic("odom"),
    _odometry_frame_id("/zed_initial_frame"),
    _updated(false),
    _relocalization(false),
    _resetRequired(false),
    resetCounter(0),
    _pointCloudHelper(0),
    _streamLiveVideo(1)

{
    // Get parameters from launch file
    nh_ns.getParam("resolution", _resolution);
    nh_ns.getParam("frame_rate", _rate);
    nh_ns.getParam("sensing_mode", _depth_mode);
    nh_ns.getParam("enable_spatial_mem", _enable_spatial_mem);

    nh_ns.getParam("min_depth_dist", _depth_minimum_distance);
    nh_ns.getParam("max_depth_dist", _depth_maximum_distance);
    nh_ns.getParam("confidence_value", _confidence_value);

    nh_ns.getParam("odometry_DB", _odometry_DB);
    nh_ns.getParam("odometry_topic", _odometry_topic);
    nh_ns.getParam("odometry_frame_id", _odometry_frame_id);
    nh_ns.getParam("brightness",_brightness);
    nh_ns.getParam("buffer_count",_buffer_count);

    ROS_INFO("brightness is %d",_brightness);
    ROS_INFO("sensing mode is %d, enable spatial mem is %d, min depth is %lf, max depth is %lf, confidence value is %d,buffer count is %d",_depth_mode,
             _enable_spatial_mem,_depth_minimum_distance,_depth_maximum_distance,_confidence_value,_buffer_count);


    _posePub = nh.advertise<zed_tracking::camera_inc_frame>(std::string("zedIncrementalPose"), 1);
    _trackingStatePub = nh.advertise<std_msgs::Int32>(std::string("zedTrackingState"), 1);

    _resetSub = nh.subscribe ("resetZedCamera", 1, &CameraForRos::resetCallback,this);

    _poseMsg.header.frame_id = std::string("world");

    _pointCloudHelper = new PointcloudPublisherHelper(nh);

}

CameraForRos::~CameraForRos() {
    ROS_INFO_STREAM("quiting camera thread");

    if(_threadCamera.joinable())
    {
        _threadCamera.join();
    }
    //delete _streamHelper;
    //  _streamHelper = 0;
    ROS_INFO_STREAM("camera thread in history");

}

void CameraForRos::start()
{
    ROS_INFO_STREAM("---------CameraForRos::start now----------");

    _threadCamera = boost::thread(&CameraForRos::cameraThreadMain, this);



}

void CameraForRos::cameraThreadMain()
{
    sl::ERROR_CODE err;
    sl::Camera zed;
    InitParameters init_params;
    init_params.camera_resolution = (sl::RESOLUTION)_resolution; //RESOLUTION_HD720 RESOLUTION_VGA Use HD720 video mode (default fps: 60)
    init_params.camera_fps = _rate;
    init_params.camera_buffer_count_linux = _buffer_count;
    init_params.coordinate_system = COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP ; // Use a right-handed Z-up coordinate system COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP
    init_params.coordinate_units = UNIT_METER; // Set units in meters
    init_params.camera_disable_self_calib = true;
    init_params.depth_mode = DEPTH_MODE_PERFORMANCE;
    init_params.depth_minimum_distance = _depth_minimum_distance;




    sl::TrackingParameters tracking_parameters;
    tracking_parameters.initial_world_transform = sl::Transform::identity();
    tracking_parameters.enable_pose_smoothing = false;
    tracking_parameters.enable_spatial_memory = (bool)_enable_spatial_mem;





    while(!zed.isOpened()) {
        err = zed.open(init_params);
        ros::Duration(0.5).sleep();
    }

    zed.setCameraSettings(sl::CAMERA_SETTINGS_BRIGHTNESS, _brightness);
    zed.setDepthMaxRangeValue(_depth_maximum_distance);
    zed.setConfidenceThreshold(_confidence_value);

    err = zed.enableTracking(tracking_parameters);
    if (err != SUCCESS)
    {
        exit(-1);
    }

    _imgHeight = zed.getResolution().height;
    _imgWidth = zed.getResolution().width;

    //enter the main loop
    //----------------------------------
    int debugLoopCounter = 0;
    while (ros::ok())
    {
        //    ros::spinOnce();
        if(_resetRequired)
        {
            sl::Transform   path;
            sl::Rotation    identity;
            sl::Translation   zero(0,0,0);
            identity.setIdentity();
            path.setRotation(identity);
            path.setTranslation(zero);

            err = zed.resetTracking(path);

            ROS_INFO("reset tracking!\n ");
            if (err == SUCCESS)
            {
                resetCounter = 0;
                _resetRequired = false;
            }
        }


        sl::Pose zed_pose;
        double time_before_grab = ros::Time::now().toSec();
        if (zed.grab() == SUCCESS)
        {


            debugLoopCounter ++;
            TRACKING_STATE tracking_state = zed.getPosition(zed_pose, sl::REFERENCE_FRAME_CAMERA); //REFERENCE_FRAME_WORLD, REFERENCE_FRAME_CAMERA
            _poseMsg.header.stamp = ros::Time::now();
            _poseMsg.pose.position.x = zed_pose.getTranslation().ty;
            _poseMsg.pose.position.y = -zed_pose.getTranslation().tx;
            _poseMsg.pose.position.z = zed_pose.getTranslation().tz;
            _poseMsg.pose.orientation.x = zed_pose.getOrientation().oy;
            _poseMsg.pose.orientation.y = -zed_pose.getOrientation().ox;
            _poseMsg.pose.orientation.z = zed_pose.getOrientation().oz;
            _poseMsg.pose.orientation.w = zed_pose.getOrientation().ow;
            _poseMsg.confidence = zed_pose.pose_confidence;
            _poseMsg.timestamp = zed_pose.timestamp;


            if(tracking_state == TRACKING_STATE_OK )
            {
                _posePub.publish(_poseMsg);
                if(zed_pose.pose_confidence > 40)
                    resetCounter = 0;
                else
                    resetCounter++;
            }
            else
            {
                ROS_INFO("Lost track!");
                resetCounter++;
            }




            if(resetCounter >= 20)
            {
                _resetRequired = true;
                resetCounter = 0;
            }


            _trackingStateIntMsg.data = (int)tracking_state;
            _trackingStatePub.publish(_trackingStateIntMsg);


        }


        _loopRate.sleep();
    }

}



void CameraForRos::resetCallback(std_msgs::Bool::ConstPtr msgPtr)
{
    if(msgPtr->data)
    {
        _resetRequired = true;
    }
}

[Myzhar]

Thank you for the detailed source code. So you are not using the official ROS node.
I will test your node to try to replicate the problem

Can you tell me what kind of system are you working on? Laptop, desktop, NVidia Jetson...

[Garfield-kh]

Thank you for your reply.
I am using TX2.

[Garfield-kh]

I find that the confidence API for ZED and ZEDmini is different, and ZED mini we didn't find its confidence, which I think may cause the frame rate drop.

[Garfield-kh]

Because confidence API for ZED and ZEDmini is different, and we cannot find the API for ZEDmini, the always to be 0 or -1 for ZEDmini, which makes the program resetTracking all the time and decrease the frame-rate.
I am going to email ZED company and ask for the API.

[Garfield-kh]

I tried printing the confidence value on ZEDwrapper using Zedmini, it looks ok like 98 97, but in my personal code, it's not. What's wrong.. XD

[Garfield-kh]

summary: ZED ZEDmini both works okay on zed-ros-wrapper;
but in my code, only ZED works, ZEDmini has no results on pose and confidence

[Myzhar]

@Garfield-kh a first thing: when you post code on Github use markdown to format it (click "insert code" in the tool bar), so it's more clear.

Second, instead of using COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP use the coordinate system made for ROS: COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD.
Replace
init_params.coordinate_system = COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP ;
with
init_params.coordinate_system = COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD ;
so you must not perform coordinate changing manually if you get position in this way:
TRACKING_STATE tracking_state = zed.getPosition(zed_pose, sl::REFERENCE_FRAME_WORLD);
...

            _poseMsg.pose.position.x = zed_pose.getTranslation().tx;
            _poseMsg.pose.position.y = zed_pose.getTranslation().ty;
            _poseMsg.pose.position.z = zed_pose.getTranslation().tz;
            _poseMsg.pose.orientation.x = zed_pose.getOrientation().ox;
            _poseMsg.pose.orientation.y = zed_pose.getOrientation().oy;
            _poseMsg.pose.orientation.z = zed_pose.getOrientation().oz;
            _poseMsg.pose.orientation.w = zed_pose.getOrientation().ow;

this is available since SDK v2.5.1

The API for ZED and ZED mini is the same:
https://www.stereolabs.com/developers/documentation/API/v2.6.0/classsl_1_1Camera.html#a33271fce55b57b769c76e7b30a6d0f28

The new SDK v2.6.0 is available for download since yesterday:
https://www.stereolabs.com/developers/release/2.6/#sdkdownloads_anchor

Try with it and let me know if you continue to have different results.

[Garfield-kh]

Thank you for your suggestion.
I update firmware and change the code COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP;
but the problem is still not solved.

May I confirm with that API is the same for ZED and ZED mini for grab>getPosition?
I will try reading the wrapper code and see if it helps.
Thank you!

[Myzhar]

Which SDK version are you using?

Is it the same if you change sl::REFERENCE_FRAME_CAMERA with sl::REFERENCE_FRAME_WORLD?

[Garfield-kh]

the SDK now is 2.6.0;
Change to World frame is still not working.
There must be something wrong with my code. XD

[Myzhar]

I'm investigating your code. Let me know if you find the solution before me.

[Garfield-kh]

test-zedTracking.tar.gz
this is the ROS file workspace, which has the problem. Could you help me have a try?
Yes, of course, I will update you if I find any solution.
Thank you!

[Myzhar]

I think that your problem is here:
init_params.camera_disable_self_calib = true;
Setting it to false I get correct values...

Why are you disabling self calibration?

[Garfield-kh]

Oh!!!! It works!!

[Myzhar]

I close the issue. You can comment anyway if you need further help

[Garfield-kh]

The reason why we disable self calibration is that: we mount ZED on drone application, because the drone keeps move, we are not sure if self_calib affects its VO performance.
Thank you!