Stereo vision in ROS

I am working on a stereo vision system using ROS in order to read and publish data between some programs using topics.

I know that de actual formula is z = fT/d and I have all the parameters of my system as:

• f: 1660 m/px
• T: 0.1395m
• Angle between lens: 10º

And then I get a disparity map using OpenCv SGBM algorithm.

Then, using tracking algorithm (specifically Open TLD for ROS), I get a ROI in which I iterate over all pixels getting a disparity average of this region.

The point is, I have been doing tests, and I found that the system is not really accurate.

My last trial has been trying to get a constant that suits in z = k / d but also, I found that the value is changing every loop.

My main question would be, what am I missing?

I leave you here my code:

/*  
*   Its implements stereo vision in order to get geometrical position
*   of objets in selected in screen in the three axis.
*   
*   Publisher: /perception/object_position
*   Subscriber: /left/tracking
*   Subscriber: /left/camera_info
*   Subscriber: /right/tracking
*   Subscriber: /right/camera_info
*
*/

/**
  * @author César Hoyos
  */
// Generic libraries
#include <stdio.h>
#include <string.h>
#include <vector>   
#include <math.h>
// Ros libraries
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Time.h"
#include <image_transport/image_transport.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PolygonStamped.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <stereo_msgs/DisparityImage.h>
#include <cv_bridge/cv_bridge.h>
// OpenCv libraries
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/types_c.h>
#include <opencv2/calib3d/calib3d.hpp>

/*!
* Defines
*/
#define WIDTH 600
#define HEIGHT 800
#define FOCAL_LENGHT 1660.2266
#define BASE_LINE 0.139586

#define pass (void)0

/*!
* Namespaces used
*/
namespace enc = sensor_msgs::image_encodings;

/*!
* Global variable to set and send object_position
*/
image_transport::Publisher image_pub;   //!< Image publisher 
cv_bridge::CvImageConstPtr cv_ptr_right;    //!< Opencv bridge for right image
cv_bridge::CvImageConstPtr cv_ptr_left;     //!< Opencv bridge for left image
cv::Mat image_right;
cv::Mat image_left;
cv::Mat right_for_disp;
cv::Mat left_for_disp;
cv_bridge::CvImageConstPtr disp_pub;
cv::Mat disp, disp8;                                //!< Disparity image
volatile bool disp_ini = false;
int xl_s, xr_s, xl_e, xr_e, yl_s, yr_s, yl_e, yr_e; //!< Get ROI from both tlds
int x_s, y_s, x_e, y_e;                             //!< Actual ROI in disparity image

/*!
* @brief When a new image arrays, we recalculate disparities between both and show it.
*/
void disparity_image_SGBM(){

// Initialize disparity algorithm. The data has been chosen with rqt_reconfigure
int SADwindow_size = 3;
int min_disp = -16;
int num_disp = 256;
cv::StereoSGBM ssgbm(
    min_disp,
    num_disp,//numDisparity / disparity range
    SADwindow_size,// sadwindowSize
    8 * 3 * SADwindow_size * SADwindow_size, //P1
    32 * 3  * SADwindow_size * SADwindow_size, //P2
    1,//disp12MaxDiff
    16, //Prefiltercap
    6, //uniquenessratio
    1000,//specklewindowsize
    4, //specklerange
    false//fulldp
);

uchar * p;
int i,j;
if(!image_left.empty() && !image_right.empty()){
    try{
        ssgbm(left_for_disp, right_for_disp, disp);
        normalize(disp, disp8, 0, 255, CV_MINMAX, CV_8U);
        disp_ini = true;
        cv::imshow("Disparity", disp8);
        cv::waitKey(1);
        /*sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "mono8", disp).toImageMsg();
        image_pub.publish(msg);
        ROS_INFO("Disparity image");*/
        //cv::destroyAllWindows();
    }catch(cv::Exception& e){
        ROS_ERROR("cv exception: [%s]", e.what());
    }
}
}



/*!
* @brief When a new image mgs is published in topic /right/image_rect this functions is
* triggered in order to handle this.
*/
void right_handler(const sensor_msgs::ImageConstPtr& msg){

try{
    cv_ptr_right = cv_bridge::toCvCopy(msg, "bgr8");
    cvtColor(cv_ptr_right->image, image_right, CV_BGR2GRAY);
    resize(image_right, right_for_disp, cv::Size(), 0.5, 0.5);      
    disparity_image_SGBM();
}catch( cv_bridge::Exception& e){
    ROS_ERROR("cv_bridge exception: [%s]", e.what());
}
}

/*!
* @brief When a new image mgs is published in topic /left/image_rect this 
functions is
* triggered in order to handle this.
*/
void left_handler(const sensor_msgs::ImageConstPtr& msg){

try{
    cv_ptr_left = cv_bridge::toCvCopy(msg, "bgr8");
    cvtColor(cv_ptr_left->image, image_left, CV_BGR2GRAY);
    resize(image_left, left_for_disp, cv::Size(), 0.5, 0.5);
    disparity_image_SGBM();
}catch( cv_bridge::Exception& e){
    ROS_ERROR("cv_bridge exception: [%s]", e.what());
}
}

void refresh_ROI(){

x_s = floor((xl_s + xr_s)/2);
y_s = floor((yl_s + yr_s)/2);
x_e = ceil((xl_e + xr_e)/2);
y_e = ceil((yl_e + yr_e)/2);

cv::Mat disp_t = disp8.clone();
cv::rectangle(disp_t, cv::Point(x_s, y_s), cv::Point(x_e, y_e), cv::Scalar(255, 255, 255), -1, 8);
cv::imshow("ROI", disp_t);
cv::waitKey(1);

}

void get_ROI_from_polygon_right(const geometry_msgs::PolygonStamped::ConstPtr& msg){

/* Get ROI from topic */
geometry_msgs::Polygon polygon = geometry_msgs::Polygon(msg->polygon);

xr_s = floor(polygon.points[0].x / 2);
yr_s = floor(polygon.points[0].y / 2);
xr_e = ceil(polygon.points[1].x / 2);
yr_e = ceil(polygon.points[1].y / 2);


/* Se dibuja el roi sobre la imagen y se muestra */
if(xr_s != -1 && disp_ini){
    refresh_ROI();
}else{
    pass;
}
}

void get_ROI_from_polygon_left(const geometry_msgs::PolygonStamped::ConstPtr& msg){

/* Get ROI from topic */
geometry_msgs::Polygon polygon = geometry_msgs::Polygon(msg->polygon);

xl_s = floor(polygon.points[0].x / 2);
yl_s = floor(polygon.points[0].y / 2);
xl_e = ceil(polygon.points[1].x / 2);
yl_e = ceil(polygon.points[1].y / 2);


/* Se dibuja el roi sobre la imagen y se muestra */
if(xl_s != -1 && disp_ini){
    refresh_ROI();
}else{
    pass;
}

}


int main(int argc, char **argv)
{

//Ros stuff
ros::init(argc, argv, "DepthFromDisparity");    //!< Initialize ros node with name StereoTracker (but then in *.launch its name will be reasigned)
ros::NodeHandle my_node;    //!< The get an instance object from NodeHandle. This creates the node 

image_transport::ImageTransport it(my_node);
image_pub = it.advertise("probe/image", 1);

image_transport::Subscriber right_ima = it.subscribe( "/right/image_rect", 1, right_handler);   //!< Image transport subscriber on topic /right
image_transport::Subscriber left_ima = it.subscribe("/left/image_rect", 1, left_handler);   //!< Image transport subscriber on topic /left
ros::Subscriber pose_tld_l = my_node.subscribe("/left/tracking", 1, get_ROI_from_polygon_left);
ros::Subscriber pose_tld_r = my_node.subscribe("/right/tracking", 1, get_ROI_from_polygon_right);
ros::Rate loop_rate(1000000);   //!< Set loop rate at 10Hz (msgs each 0.1s)

// Declarations of variables
uchar * p, * q;
int i,j;
float sum = 0;
float disparity = 0;
float depth = 0;
int count = 0;
int errors = 0;
float k = 0;

// Main node loop
while(ros::ok()){


    if(disp_ini && x_s != -1){
        sum = 0;
        depth = 0;
        disparity = 0;
        count = 0;
        errors = 0;
        for(i = ((x_s + x_e)/2 - 2); i < ((x_e + x_s)/2 + 2); i++){
            p = disp8.ptr<uchar> (i);
            q = disp.ptr<uchar> (i);
            for(j = y_s; j < y_e; j++){
                if(q[j] > 0){
                    sum += p[j];
                    disparity += q[j];
                    count++;
                }else{
                    errors++;
                }
            }
        }
        if(disparity){
            disparity /= count;
            //depth = FOCAL_LENGTH * BASE_LINE / disparity;
            depth = 1.30;
            k = depth * disparity;
            ROS_INFO("Constant: [%f]", k);
        }else{
            ROS_ERROR("ERROR: No disparity");
        }

    }else{
        ROS_ERROR("ERROR: No tracking");
    }

    ros::spinOnce();    //!< It is here to proccess callbacks
    loop_rate.sleep();  //!< Keeps rate as set before loop
}

return 0;
}

Thanks for your suggestions and help.