VO.cpp

Go to the documentation of this file.

#include <VO/VO.h>

// define whether to use approximate nearest-neighbor search
#define USE_FLANN


CvMemStorage* storage1_pair = cvCreateMemStorage(0);
CvMemStorage* consecutive_Storage = cvCreateMemStorage(0);

using namespace std;
using namespace cv;
using namespace Eigen;
using namespace image_transport;


static CvScalar colors[] =
        {
                {{0,0,255}},
                {{0,128,255}},
                {{0,255,255}},
                {{0,255,0}},
                {{255,128,0}},
                {{255,255,0}},
                {{255,0,0}},
                {{255,0,255}},
                {{255,255,255}}
        };


class Error_Prop{

  ros::NodeHandle nh_;                                          //Nodo para suscribirse a las imagenes stereo
  image_transport::ImageTransport it_;                          //Para leer las imagenes
  image_transport::Subscriber image_sub_l_, image_sub_r_;       // Suscripcion a los topicos que publican las imagenes

  ros::NodeHandle n_pcl1, n_pcl2, n_pos, n_odom,n_tgt,n_src,n_wpts1,n_cpts1,n_src_RT,n_od;
  ros::Publisher pub_pcl1,pub_pcl2,pub_pos,pub_odom,pub_tgt,pub_src,pub_wpts1,pub_cpts1,pub_src_RT,pub_od,pub_uvl,pub_uvr;

  tf::TransformBroadcaster od_broadcaster;

  CvSURFParams params;

public:

  bool START;

  int ncol,nrow,sigma_ul,sigma_vl,sigma_ur,sigma_vr,Npoints,Nclouds,numRandPoints,size;
  float om[3],T[3],tx,ty,tz,alfa_l[2],alfa_r[2],alfa_U_l,alfa_U_r,alfa_V_l,alfa_V_r,alfa[4],uol,vol,uor,vor,wpx_,wpy_,wpz_,px,py,pz,xlims[2],ylims[2],zlims[2],meanul,meanvl,meanur,meanvr,mean[3],xl[2],yl[2],zl[2],h_r[16];
  Mat R,H_l2r,rdm,wpxyz,randomUL,randomVL,randomUR,randomVR,randomx,randomy,randomz,im1,im2;
  CvMat p_xyz,uvs_l,uvs_r,Cov,Jf,HC,d2C_dydx,RTCamFr,invC2R,Hcam2rob,H_r;
  double hcam2rob[16],inv_cam2rob[16];
  CvMat* Hcaminv;

  pcl::PointCloud<pcl::PointXYZ> cloud_a, cloud_b, cloud_c, cloud_ant, cloud_pos, cloud_odom, cloud_odom2, wpts1, cpts1, cpts_ant, wpts1RANSAC, cpts1RANSAC, cloud_w,cloud_t, cloud_wRT,wpts1RT,UV_l,UV_r;

  Eigen::Matrix<float,4,4> Hcam2rob_;
  Eigen::Matrix3f R_;
  Eigen::Matrix<float,3,4> Jf_;
  Eigen::Matrix<float,4,4> H_l2r_;           //Relacion entre las camaras (left to right)

  Eigen::Matrix<float,3,2000> Points3D;
  Eigen::Matrix<float,3,3> cam2world;

  // -------Visual odometry-------

  double x, y, z,th, vx, vy, vth;
  ros::Time current_time, last_time;

  tf::Quaternion quat_ant;
  Quaterniond fq;
  Vector4d trans_;
  geometry_msgs::Pose odom3D;

  double t_stereo;
  vector<int> ptpairs1,ptpairs2,ptpairs3,ptpairs1_resp,ptpairs2_resp,ptpairs3_resp,indices_a,indices_b;

  CvSeq *leftKeypoints1, *leftDescriptors1, *rightKeypoints1, *rightDescriptors1, *leftKeypoints2, *leftDescriptors2, *rightKeypoints2, *rightDescriptors2,             *leftKeypoints3, *leftDescriptors3, *rightKeypoints3, *rightDescriptors3;

  Matrix<float,3,3> Rot_12,Rot_i;
  Matrix<float,3,1> Tras_12,Tras_i;

  Eigen::Matrix4f transformation;

  Eigen::Matrix<float,3,500> nube1;
  Eigen::Matrix<float,3,500> nube2;

  Matrix<float,3,1> odom_1,odom_2;

  IplImage* left_;
  IplImage* left_1;
  IplImage* left_gray;
  IplImage* left_ant;
  IplImage* left_gray_ant;
  IplImage* right_;
  IplImage* right_1;
  IplImage* right_gray;

 // ---------Funciones---------

        void init();
        void ypr(const Matrix<float,3,3> Rot,Matrix<float,3,1>& ypr_);
        double compareSURFDescriptors ( const float* d1, const float* d2, double best, int length );
        int locatePlanarObject( const CvSeq* objectKeypoints, const CvSeq* objectDescriptors,const CvSeq* imageKeypoints, const CvSeq* imageDescriptors,
                          const CvPoint src_corners[4], CvPoint dst_corners[4] );
        void flannFindPairs( const CvSeq*, const CvSeq* objectDescriptors,const CvSeq*,const CvSeq* imageDescriptors,vector<int>& ptpairs );
        void findPairs( const CvSeq* objectKeypoints,const CvSeq* objectDescriptors,const CvSeq* imageKeypoints,const CvSeq* imageDescriptors,vector<int>& ptpairs);
        int naiveNearestNeighbor( const float* vec, int laplacian, const CvSeq* model_keypoints, const CvSeq* model_descriptors );

 // -------------------Visual odometry---------------------
        void run();

        //--------Image capture-------
        void imageCb_l(const sensor_msgs::ImageConstPtr& msg);
        void imageCb_r(const sensor_msgs::ImageConstPtr& msg);

        //--------Matching-------
        void VO_StereoMatching();                               // Match entre los pares de imagenes stereo
        void VO_ConsecutiveMatching();                          // Match entre las imagenes de la izquierda sucesivas

        // ------ View Matches--------
        void VO_ViewStereo();
        void VO_ViewConsecutive();
        void VO_ViewAll();

        //--------3D reconstruction-------
        void VO_matches_to_3D();                                // Conversion de la nube de puntos 3D a partir de los matches
        void VO_3D_to_matches();

        //--------Filtering-------
        void VO_matchFilter();
        void VO_cloudFilter();

        //--------RT from clouds-------
        void VO_get_RT_RANSAC();

        //--------World representation-------
        void VO_RT_to_world();

        //----------Clean------------
        void VO_clean();

 //-------- Constructor ---------

  Error_Prop():it_(nh_)
  {
        //Nodos de las camaras
        START=FALSE;

        image_sub_l_ = it_.subscribe("/teo/left_camera/camera_image", 1, &Error_Prop::imageCb_l, this);
        image_sub_r_ = it_.subscribe("/teo/right_camera/camera_image", 1, &Error_Prop::imageCb_r, this);

        //---------------Ventanas para ver las imagenes de las camaras--------------
        left_1 = cvCreateImage(cvSize(480,320), 8, 3 );        //esto es mentira en verdad es mas grande, es para que tenga un tamano
        right_1 = cvCreateImage(cvGetSize(left_1), 8, 3 );

        left_ = cvCreateImage(cvSize(480,320), 8, 3 );
        left_ant = cvCreateImage(cvGetSize(left_), 8, 3 );
        left_gray_ant = cvCreateImage(cvGetSize(left_ant), IPL_DEPTH_8U, 1);
        right_ = cvCreateImage(cvGetSize(left_), 8, 3 );

        //---------------------Keypoints & Descriptors------------------------------

        leftKeypoints1 = 0; leftDescriptors1 = 0;
        rightKeypoints1 = 0; rightDescriptors1 = 0;

        leftKeypoints2 = 0; leftDescriptors2 = 0;
        rightKeypoints2 = 0; rightDescriptors2 = 0;
  }

        //--------------------- Destructor -------------------------

  ~Error_Prop()
  {
    //cv::destroyWindow("Left");
    //cv::destroyWindow("Right");
  }

};


/*
-------------------------------------------------------------------------------------------------------------
                                                Funciones
-------------------------------------------------------------------------------------------------------------
*/

void Error_Prop::ypr(const Matrix<float,3,3> R,Matrix<float,3,1>& ypr_){
/* Extracts angles from a rotation matrix. (yaw pitch roll angles)*/

  float yaw,pitch,roll;
  roll=atan2(R(1,0),R(0,0));  //roll
  pitch=atan2(-R(2,0),R(0,0)*cos(roll)+R(1,0)*sin(roll)); //pitch
  yaw=atan2(R(2,1),R(2,2));  //yaw
  ypr_(0,0)=yaw;ypr_(1,0)=pitch,ypr_(2,0)=roll;
}

double Error_Prop::compareSURFDescriptors( const float* d1, const float* d2, double best, int length )
{
        double total_cost = 0;
        assert( length % 4 == 0 );
        for( int i = 0; i < length; i += 4 )
        {
                double t0 = d1[i] - d2[i];
                double t1 = d1[i+1] - d2[i+1];
                double t2 = d1[i+2] - d2[i+2];
                double t3 = d1[i+3] - d2[i+3];
                total_cost += t0*t0 + t1*t1 + t2*t2 + t3*t3;
                if( total_cost > best )
                        break;
        }
        return total_cost;
}

int Error_Prop::naiveNearestNeighbor( const float* vec, int laplacian,const CvSeq* model_keypoints,const CvSeq* model_descriptors )
{
        int length = (int)(model_descriptors->elem_size/sizeof(float));
        int i, neighbor = -1;
        double d, dist1 = 1e6, dist2 = 1e6;
        CvSeqReader reader, kreader;
        cvStartReadSeq( model_keypoints, &kreader, 0 );
        cvStartReadSeq( model_descriptors, &reader, 0 );

        for( i = 0; i < model_descriptors->total; i++ )
        {
                const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
                const float* mvec = (const float*)reader.ptr;
                CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
                CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
                if( laplacian != kp->laplacian )
                        continue;
                        d = compareSURFDescriptors( vec, mvec, dist2, length );
                if( d < dist1 )
                {
                        dist2 = dist1;
                        dist1 = d;
                        neighbor = i;
                }
                else if ( d < dist2 )
                        dist2 = d;
                }
        if ( dist1 < 0.6*dist2 )
                return neighbor;
        return -1;
}

void Error_Prop::findPairs( const CvSeq* objectKeypoints, const CvSeq* objectDescriptors,
           const CvSeq* imageKeypoints, const CvSeq* imageDescriptors, vector<int>& ptpairs )
{
        int i;
        CvSeqReader reader, kreader;
        cvStartReadSeq( objectKeypoints, &kreader );
        cvStartReadSeq( objectDescriptors, &reader );
        ptpairs.clear();

        for( i = 0; i < objectDescriptors->total; i++ )
        {
                const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
                const float* descriptor = (const float*)reader.ptr;
                CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
                CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
                int nearest_neighbor = naiveNearestNeighbor( descriptor, kp->laplacian, imageKeypoints, imageDescriptors );
                if( nearest_neighbor >= 0 )
                {
                        ptpairs.push_back(i);
                        ptpairs.push_back(nearest_neighbor);
                }
        }
}

void Error_Prop::flannFindPairs( const CvSeq*, const CvSeq* objectDescriptors,const CvSeq*, const CvSeq* imageDescriptors, vector<int>& ptpairs )
{
        int length = (int)(objectDescriptors->elem_size/sizeof(float));

        cv::Mat m_object(objectDescriptors->total, length, CV_32F);  //Left
        cv::Mat m_image(imageDescriptors->total, length, CV_32F);    //Right


        // copy descriptors
        CvSeqReader obj_reader;
        float* obj_ptr = m_object.ptr<float>(0);
        cvStartReadSeq( objectDescriptors, &obj_reader );
        for(int i = 0; i < objectDescriptors->total; i++ )
        {
                const float* descriptor = (const float*)obj_reader.ptr;
                CV_NEXT_SEQ_ELEM( obj_reader.seq->elem_size, obj_reader );
                memcpy(obj_ptr, descriptor, length*sizeof(float));
                obj_ptr += length;
        }
        CvSeqReader img_reader;
        float* img_ptr = m_image.ptr<float>(0);
        cvStartReadSeq( imageDescriptors, &img_reader );
        for(int i = 0; i < imageDescriptors->total; i++ )
        {
                const float* descriptor = (const float*)img_reader.ptr;
                CV_NEXT_SEQ_ELEM( img_reader.seq->elem_size, img_reader );
                memcpy(img_ptr, descriptor, length*sizeof(float));
                img_ptr += length;
        }

        // find nearest neighbors using FLANN
        cv::Mat m_indices(objectDescriptors->total, 2, CV_32S);
        cv::Mat m_dists(objectDescriptors->total, 2, CV_32F);
        cv::flann::Index flann_index(m_image, cv::flann::KDTreeIndexParams(4));  // using 4 randomized kdtrees
        flann_index.knnSearch(m_object, m_indices, m_dists, 2, cv::flann::SearchParams(64) ); // maximum number of leafs checked

        int* indices_ptr = m_indices.ptr<int>(0);
        float* dists_ptr = m_dists.ptr<float>(0);
        for (int i=0;i<m_indices.rows;++i) {
                if (dists_ptr[2*i]<0.6*dists_ptr[2*i+1]) {
                        ptpairs.push_back(i);
                        ptpairs.push_back(indices_ptr[2*i]);
                }
        }
}

/* a rough implementation for object location */
int Error_Prop::locatePlanarObject( const CvSeq* objectKeypoints, const CvSeq* objectDescriptors, const CvSeq* imageKeypoints, const CvSeq* imageDescriptors,                                       const CvPoint src_corners[4], CvPoint dst_corners[4] )
{
        double h[9];
        CvMat _h = cvMat(3, 3, CV_64F, h);
        vector<int> ptpairs;
        vector<CvPoint2D32f> pt1, pt2;
        CvMat _pt1, _pt2;
        int i, n;

        #ifdef USE_FLANN
                flannFindPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
        #else
                findPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
        #endif

        n = ptpairs.size()/2;
        if( n < 4 )
                return 0;

        pt1.resize(n);
        pt2.resize(n);
        for( i = 0; i < n; i++ )
        {
                pt1[i] = ((CvSURFPoint*)cvGetSeqElem(objectKeypoints,ptpairs[i*2]))->pt;
                pt2[i] = ((CvSURFPoint*)cvGetSeqElem(imageKeypoints,ptpairs[i*2+1]))->pt;
        }

        _pt1 = cvMat(1, n, CV_32FC2, &pt1[0] );
        _pt2 = cvMat(1, n, CV_32FC2, &pt2[0] );
        if( !cvFindHomography( &_pt1, &_pt2, &_h, CV_RANSAC, 5 ))
                return 0;

        for( i = 0; i < 4; i++ )
        {
                double x = src_corners[i].x, y = src_corners[i].y;
                double Z = 1./(h[6]*x + h[7]*y + h[8]);
                double X = (h[0]*x + h[1]*y + h[2])*Z;
                double Y = (h[3]*x + h[4]*y + h[5])*Z;
                dst_corners[i] = cvPoint(cvRound(X), cvRound(Y));
        }

        return 1;
}


/*
----------------------------------------------------------------------------------------------------------------------
                                                Visual odometry
----------------------------------------------------------------------------------------------------------------------
*/


void Error_Prop::imageCb_l(const sensor_msgs::ImageConstPtr& msg)
  {
        ROS_INFO("Received left image");
        sensor_msgs::CvBridge bridge_;

        try
        {
                left_1 = bridge_.imgMsgToCv(msg, "bgr8");
        }
        catch (sensor_msgs::CvBridgeException error)
        {
                ROS_ERROR("Error converting left image to IplImage");
        }

        //----------Image Capture-----------

        cvResize(left_1,left_);                                                 
        cvShowImage("Left", left_);
        left_gray = cvCreateImage(cvGetSize(left_), IPL_DEPTH_8U, 1);
        cvCvtColor(left_, left_gray, CV_BGR2GRAY );

  }


void Error_Prop::imageCb_r(const sensor_msgs::ImageConstPtr& msg)
  {

        ROS_INFO("Received right image");
        sensor_msgs::CvBridge bridge_;
        try
        {
                right_1 = bridge_.imgMsgToCv(msg, "bgr8");
        }
        catch (sensor_msgs::CvBridgeException error)
        {
                ROS_ERROR("Error converting right image to IplImage");
        }

        //----------Image Capture-----------

        cvResize(right_1,right_);                               
        cvShowImage("Right", right_);
        right_gray = cvCreateImage(cvGetSize(right_), IPL_DEPTH_8U, 1);
        cvCvtColor(right_, right_gray, CV_BGR2GRAY );
    }


void Error_Prop::run()
{
    //---------Stereo Matching----------
    if ((START==FALSE)||(left_gray_ant!=left_gray))
    {
    t_stereo = (double)cvGetTickCount();
    VO_StereoMatching();
    t_stereo = (double)cvGetTickCount() - t_stereo;
    printf( "Extraction time STEREO = %gms\n", t_stereo/(cvGetTickFrequency()*1000.));

    //-------Consecutive Matching-------

    t_stereo = (double)cvGetTickCount();
    VO_ConsecutiveMatching();
    t_stereo = (double)cvGetTickCount() - t_stereo;
    printf( "Extraction time CONSECUTIVE = %gms\n", t_stereo/(cvGetTickFrequency()*1000.));

    //-----------Filtering-----------

    t_stereo = (double)cvGetTickCount();
    VO_matchFilter();                                   //Filtrado de los ptpairs
    VO_cloudFilter();                                   //Filtrado de la nube de todos los puntos 3D
    t_stereo = (double)cvGetTickCount() - t_stereo;
    printf( "Extraction time FILTER = %gms\n", t_stereo/(cvGetTickFrequency()*1000.));

    //-------3D Reconstruction-------

    t_stereo = (double)cvGetTickCount();
    VO_matches_to_3D();                                 // cloud_a y cloud_b en 3D con todos los matches ecepto los filtrados hasta ahora
    t_stereo = (double)cvGetTickCount() - t_stereo;
    printf( "Extraction time 3D RECONSTRUCTION = %gms\n", t_stereo/(cvGetTickFrequency()*1000.));

    //---------Get RT-----------------

    if (cloud_a.size()==cloud_b.size())
    {
            if (cloud_a.size()>6)
            {
                    t_stereo = (double)cvGetTickCount();
                    VO_get_RT_RANSAC();
                    t_stereo = (double)cvGetTickCount() - t_stereo;
                    printf( "Extraction time RT (RANSAC) = %gms\n", t_stereo/(cvGetTickFrequency()*1000.));
            }
            else
            {
                    printf( "Not enough points..");
                    Rot_12.setIdentity();
                    Tras_12.setZero();
            }
    }
    else
    {
            printf( "Unbalanced clouds..");
            Rot_12.setIdentity();
            Tras_12.setZero();
    }

    //-----------Plot RT--------------
    t_stereo = (double)cvGetTickCount();
    VO_RT_to_world();
    t_stereo = (double)cvGetTickCount() - t_stereo;
    printf( "Extraction time RT2world = %gms\n", t_stereo/(cvGetTickFrequency()*1000.));

    //---------Limpieza -----------
    t_stereo = (double)cvGetTickCount();
    VO_clean();
    t_stereo = (double)cvGetTickCount() - t_stereo;
    printf( "Extraction time CLEAN = %gms\n", t_stereo/(cvGetTickFrequency()*1000.));

    START=TRUE;

    t_stereo = (double)cvGetTickCount() - t_stereo;
    printf( "ITERATION TIME = %gms\n", t_stereo/(cvGetTickFrequency()*1000.));
  }
}

  void Error_Prop::init()
{

        params= cvSURFParams(1000, 1); //Parametros del SURF

        pub_pcl1  = n_pcl1.advertise<pcl::PointCloud<pcl::PointXYZ> >("puntos1", 1000);
        pub_pcl2  = n_pcl2.advertise<pcl::PointCloud<pcl::PointXYZ> >("puntos2", 1000);
        pub_pos   = n_pos.advertise<pcl::PointCloud<pcl::PointXYZ> >("posicion", 1000);  //2
        pub_odom  = n_odom.advertise<pcl::PointCloud<pcl::PointXYZ> >("miodom", 1);  //

        pub_tgt   = n_tgt.advertise<pcl::PointCloud<pcl::PointXYZ> >("tgt", 6);
        pub_src   = n_src.advertise<pcl::PointCloud<pcl::PointXYZ> >("src", 6);  //
        pub_wpts1 = n_wpts1.advertise<pcl::PointCloud<pcl::PointXYZ> >("wpts1", 1000);
        pub_cpts1 = n_cpts1.advertise<pcl::PointCloud<pcl::PointXYZ> >("cpts1", 1000);

        pub_src_RT = n_src_RT.advertise<pcl::PointCloud<pcl::PointXYZ> >("srcRT", 6);

        pub_od = n_od.advertise<nav_msgs::Odometry>("od", 50);

        pub_uvl = n_pcl1.advertise<pcl::PointCloud<pcl::PointXYZ> >("uvl", 1000);
        pub_uvr = n_pcl1.advertise<pcl::PointCloud<pcl::PointXYZ> >("uvr", 1000);


        cloud_a.header.frame_id="nube1_(i)";
        cloud_ant.header.frame_id="nube1_ (i-1)";
        cloud_b.header.frame_id="nube2";
        cloud_pos.header.frame_id="posicion";
        wpts1.header.frame_id="world relativo";
        cpts1.header.frame_id="camera relativo";



        wpts1RT.header.frame_id="world rel RT";
//------------------------------------------------------------------------
        cloud_w.header.frame_id="src";
        cloud_t.header.frame_id="tgt";

        cloud_wRT.header.frame_id="tgt";
        cloud_wRT.width  = 6;
        cloud_wRT.height = 1;
        cloud_wRT.points.resize (cloud_w.width * cloud_w.height);


        cloud_w.width  = 6;
        cloud_w.height = 1;
        cloud_w.points.resize (cloud_w.width * cloud_w.height);

        cloud_t.width  = 6;
        cloud_t.height = 1;
        cloud_t.points.resize (cloud_t.width * cloud_t.height);
//------------------------------------------------------------------------

        cloud_a.width  = 2000;
        cloud_a.height = 1;
        cloud_a.points.resize (cloud_a.width * cloud_a.height);

        cloud_b.width  = 2000;
        cloud_b.height = 1;
        cloud_b.points.resize (cloud_b.width * cloud_b.height);

        cloud_pos.width  = 2000;  //2
        cloud_pos.height = 1;     //1
        cloud_pos.points.resize (cloud_b.width * cloud_b.height);

        cloud_odom.header.frame_id="miodom";
        cloud_odom.width = 1;
        cloud_odom.height = 1;
        cloud_odom.points.resize (cloud_odom.width * cloud_odom.height);

        //cloud_odom.points[0].x=0;
        //cloud_odom.points[0].y=0;
        //cloud_odom.points[0].z=0;

        ROS_INFO("Inicializando variables");

        //Image Size (width X height)
        ncol=cvGetSize(left_).width;
        nrow=cvGetSize(left_).height;

        numRandPoints=100;

        //Extrinsic Parameters

        //om[0]=0.04046;  om[1]=0.01616;  om[2]=0.01419;
        om[0]=-0.03770;  om[1]=-0.01934;  om[2]=-0.01324;
        //T[0]=-0.38571991;  T[1]=-0.00724661;  T[2]=0.00011057;
        T[0]=0.30828733;  T[1]=0.00093104;  T[2]=0.00260338;


        tx=T[0];ty=T[1];tz=T[2];

        //Intrinsic Parameters


        //Focal length

        alfa_l[0]=1753.52020;  alfa_l[1]=1753.89203;
        alfa_r[0]=1760.90953;  alfa_r[1]=1758.60452;

        alfa_U_l=alfa_l[0];   alfa_V_l=alfa_l[1];
        alfa_U_r=alfa_r[0];   alfa_V_r=alfa_r[1];

        alfa[0]=alfa_U_l;alfa[1]=alfa_V_l;alfa[2]=alfa_U_r;alfa[3]=alfa_V_r;

        //Principal Point

        uol=578.92621;
        vol=511.56295;
        uor=618.17344;
        vor=504.21283;


        //Rotation Matrix

        float sx,cx,sy,cy,sz,cz;
        sx=sin(om[0]);
        cx=cos(om[0]);
        sy=sin(om[1]);
        cy=cos(om[1]);
        sz=sin(om[2]);
        cz=cos(om[2]);

        R_ << cy*cz,sx*sy*cz-cx*sz, cz*sy*cx+sx*sz,cy*sz, sx*sy*sz+cx*cz, cx*sy*sz-sx*cz,-sy,sx*cy,cx*cy;

        H_l2r_ << R_(0,0),R_(0,1),R_(0,2),T[0],R_(1,0),R_(1,1),R_(1,2),T[1],R_(2,0),R_(2,1),R_(2,2),T[2],0,0,0,1;

  /*  World coord. frame  == robot frame at first pose
      Robot Frame: x=forwards; y=horizontal(left); z=vertical(upwards)
      Transformation between camera and robot coord. frames
      Hcam2rob  = Ry(-pi)*Rx(pi/2)*Ry(-pi/2)
      Ry -> Rotation around Y (pitch). Homogeneous matrix for a rotation about Y axis.                    */

        Hcam2rob_ << 0,0,1,0,-1,0,0,0,0,-1,0,0,0,0,0,1;

        odom_1.setZero();
        odom_2.setZero();

}

void Error_Prop::VO_ViewStereo(){
    //---------------------------- Construccion de la imagen para ver los match-----------------

            IplImage* correspond1 = cvCreateImage( cvSize(right_gray->width+left_gray->width, left_gray->height), 8, 1 );

            cvSetImageROI( correspond1, cvRect( 0, 0, left_gray->width, left_gray->height ) );
            cvCopy( left_gray, correspond1 );
            cvSetImageROI( correspond1, cvRect( left_gray->width, 0, correspond1->width, correspond1->height ) );
            cvCopy( right_gray, correspond1 );
            cvResetImageROI( correspond1 );

            for( int i = 0; i < (int)ptpairs1.size(); i += 2 )
            {
                    CvSURFPoint* r11 = (CvSURFPoint*)cvGetSeqElem( leftKeypoints1, ptpairs1[i] );
                    CvSURFPoint* r21 = (CvSURFPoint*)cvGetSeqElem( rightKeypoints1, ptpairs1[i+1] );

                    cvLine( correspond1, cvPointFrom32f(r11->pt), cvPoint(cvRound(r21->pt.x+left_gray->width), cvRound(r21->pt.y)), colors[8] );
            }

            cvShowImage( "Stereo matches", correspond1 );
}

void Error_Prop::VO_StereoMatching(){

    if (START==TRUE)
    {
        //---------------clear new and vertical ptpairs----------------
        ptpairs1.clear();
        ptpairs3.clear();

        //----------clear old keypoints and descriptors---------------
        cvClearSeq(leftKeypoints2);
        cvClearSeq(rightKeypoints2);
        cvClearSeq(leftDescriptors2);
        cvClearSeq(rightDescriptors2);

        //-----copy current keypoints and descriptors as olds----------
        leftKeypoints2 = cvCloneSeq(leftKeypoints1,consecutive_Storage);
        leftDescriptors2 = cvCloneSeq(leftDescriptors1,consecutive_Storage);
        rightKeypoints2 = cvCloneSeq(rightKeypoints1,consecutive_Storage);              //Estas se pueden eliminar solo tenemos que mantener la left vieja
        rightDescriptors2 = cvCloneSeq(rightDescriptors1,consecutive_Storage);

        //----------copy current image as old image--------------------
        left_gray_ant=left_gray;

        //---------------------Update old ptpairs----------------------
        ptpairs2=ptpairs1_resp; //Todos los ptpairs sin filtrar
        ptpairs2_resp=ptpairs1_resp;

        //--------clear memory for new keypoints and descriptors-------
        cvClearSeq(leftKeypoints1);
        cvClearSeq(rightKeypoints1);
        cvClearSeq(leftDescriptors1);
        cvClearSeq(rightDescriptors1);

        ROS_INFO("cargadas!");
    }

 //---------------------Extraction KeyPoints & Descriptors-----------------------------------

        cvExtractSURF( left_gray, 0, &leftKeypoints1, &leftDescriptors1, storage1_pair, params );
        cvExtractSURF( right_gray, 0, &rightKeypoints1, &rightDescriptors1, storage1_pair, params );
        printf("Left Keypoints: %d | Right Keypoints: %d\n", leftDescriptors1->total, rightDescriptors1->total);

 //-----------------------------------Matching---------------------------------------------

        #ifdef USE_FLANN
                printf("Using approximate nearest neighbor search\n");
                flannFindPairs( leftKeypoints1, leftDescriptors1, rightKeypoints1, rightDescriptors1, ptpairs1 );
        #else
                findPairs( leftKeypoints1, leftDescriptors1, rightKeypoints1, rightDescriptors1, ptpairs1);
        #endif

        printf(" STEREO (1): Number of matches SIN FILTRADO INICIAL: %d\n",(int)ptpairs1.size()/2);


        for( int i = 0; i < (int)ptpairs1.size(); i += 2 )
        {
                CvSURFPoint* r11 = (CvSURFPoint*)cvGetSeqElem( leftKeypoints1, ptpairs1[i] );
                CvSURFPoint* r21 = (CvSURFPoint*)cvGetSeqElem( rightKeypoints1, ptpairs1[i+1] );

                if ( (cvPointFrom32f(r11->pt).y >= cvPointFrom32f(r21->pt).y) && (cvPointFrom32f(r11->pt).x >= cvPointFrom32f(r21->pt).x) /*&&                                       ((cvPointFrom32f(r11->pt).y - cvPointFrom32f(r21->pt).y) < 40) && ((cvPointFrom32f(r11->pt).y - cvPointFrom32f(r21->pt).y) > 0)*/ )
                {
                    //cvLine( correspond1, cvPointFrom32f(r11->pt), cvPoint(cvRound(r21->pt.x+left_gray->width), cvRound(r21->pt.y)), colors[8] );
                    //printf("Borrando puntos..");
                }
                else
                {
                        //elminamos el match entre los dos puntos
                        ptpairs1.erase(ptpairs1.begin()+i);
                        ptpairs1.erase(ptpairs1.begin()+i);
                        i-=2;
                }

        }


        printf(" STEREO (2): Number of matches PRIMER FILTRO: %d\n",(int)ptpairs1.size()/2);

        ptpairs1_resp=ptpairs1;  // guardamos los pares para la proxima vez

        Error_Prop::VO_ViewStereo();


    }

void Error_Prop::VO_ViewConsecutive(){

    IplImage* correspond4 = cvCreateImage( cvSize(left_gray_ant->width, left_gray->height+left_gray->height), 8, 1 );
    cvSetImageROI( correspond4, cvRect( 0, 0, left_gray->width, left_gray->height ) );
    cvCopy( left_gray, correspond4 );
    cvSetImageROI( correspond4, cvRect( 0,left_gray->height, left_gray->width, left_gray_ant->height+left_gray->height ) );
    cvCopy( left_gray_ant, correspond4 );
    cvResetImageROI( correspond4 );

    for( int i = 0; i < (int)ptpairs3.size(); i += 2 )
    {
            CvSURFPoint* r13 = (CvSURFPoint*)cvGetSeqElem( leftKeypoints1, ptpairs3[i] );
            CvSURFPoint* r23 = (CvSURFPoint*)cvGetSeqElem( leftKeypoints2, ptpairs3[i+1] );

            cvLine( correspond4, cvPointFrom32f(r13->pt), cvPoint(cvPointFrom32f(r23->pt).x,cvPointFrom32f(r23->pt).y + left_gray->height), colors[0] );
    }

    cvShowImage( "Vertical matches", correspond4 );
}


void Error_Prop::VO_ViewAll(){

    //---------------------------- Construccion de la imagen para ver los match-----------------

    IplImage* correspond5 = cvCreateImage( cvSize(right_gray->width+left_gray->width, left_gray->height+left_gray->height), 8, 1 );

    cvSetImageROI( correspond5, cvRect( 0, 0, left_gray->width, left_gray->height ) );
    cvCopy( left_gray, correspond5 );
    cvSetImageROI( correspond5, cvRect( left_gray->width, 0, correspond5->width, left_gray->height ) );
    cvCopy( right_gray, correspond5 );
    cvSetImageROI( correspond5, cvRect( 0,left_gray->height, left_gray->width, correspond5->height ) );
    cvCopy( left_gray_ant, correspond5 );
    cvSetImageROI( correspond5, cvRect( left_gray->width,left_gray->height, correspond5->width, correspond5->height ) );
    cvCopy( left_gray_ant, correspond5 );

    cvResetImageROI( correspond5 );

    for( int i = 0; i < (int)ptpairs1.size(); i += 2 )
    {
            CvSURFPoint* r11 = (CvSURFPoint*)cvGetSeqElem( leftKeypoints1, ptpairs1[i] );
            CvSURFPoint* r21 = (CvSURFPoint*)cvGetSeqElem( rightKeypoints1, ptpairs1[i+1] );

            cvLine( correspond5, cvPointFrom32f(r11->pt), cvPoint(cvRound(r21->pt.x+left_gray->width), cvRound(r21->pt.y)), colors[8] );
    }

   /* for( int i = 0; i < (int)ptpairs2.size(); i += 2 )
    {
            CvSURFPoint* r11 = (CvSURFPoint*)cvGetSeqElem( leftKeypoints2, ptpairs2[i] );
            CvSURFPoint* r21 = (CvSURFPoint*)cvGetSeqElem( rightKeypoints2, ptpairs2[i+1] );

            cvLine( correspond5, cvPoint( cvRound(r11->pt.x), cvRound(r11->pt.y+left_gray->height)) ,cvPoint(cvRound(r21->pt.x + left_gray->width), cvRound(r21->pt.y + left_gray->height)) , colors[8] );
    }*/

    for( int i = 0; i < (int)ptpairs3.size(); i += 2 )
    {
            CvSURFPoint* r13 = (CvSURFPoint*)cvGetSeqElem( leftKeypoints1, ptpairs3[i] );
            CvSURFPoint* r23 = (CvSURFPoint*)cvGetSeqElem( leftKeypoints2, ptpairs3[i+1] );

            cvLine( correspond5, cvPointFrom32f(r13->pt), cvPoint(cvPointFrom32f(r23->pt).x,cvPointFrom32f(r23->pt).y + left_gray->height), colors[0] );
    }

    cvShowImage( "Show all", correspond5 );
}


void Error_Prop::VO_ConsecutiveMatching(){

    if (START==FALSE)
    {   // Solo para que se ejecute la primera vez
            leftKeypoints2 = cvCloneSeq(leftKeypoints1,consecutive_Storage);
            leftDescriptors2 = cvCloneSeq(leftDescriptors1,consecutive_Storage);
            rightKeypoints2 = cvCloneSeq(rightKeypoints1,consecutive_Storage);        //Posiblemente no haya que guardar estos
            rightDescriptors2 = cvCloneSeq(rightDescriptors1,consecutive_Storage);
            left_gray_ant=left_gray;
            ptpairs2=ptpairs1_resp;
            ptpairs2_resp=ptpairs2;
            ROS_INFO("clonadas!");
    }

        printf(" CONSECUTIVE (0): Number of matches ANTES EMPEZAR: %d\n",(int)ptpairs3.size()/2);

 //-----------------------------------Matching---------------------------------------------
        #ifdef USE_FLANN
                flannFindPairs( leftKeypoints1, leftDescriptors1,leftKeypoints2, leftDescriptors2, ptpairs3 );
        #else
                findPairs( leftKeypoints1, leftDescriptors1,leftKeypoints2, leftDescriptors2, ptpairs3);
        #endif

        Error_Prop::VO_ViewConsecutive();

}


void Error_Prop::VO_matches_to_3D(){

//      printf(" 3D RECONSTRUCTION (0): starting\n");

        //Compute 3D points with randomly genereated noice in both images
        float Ul,Vl,Ur,Vr;

        Matrix<float,4,3> An;
        Matrix<float,4,1> Bn;
        Matrix<float,3,1> xyzn;

        Matrix<float,3,3> Cam2Rob;

        Cam2Rob.setZero();
        Cam2Rob(0,2)=1; Cam2Rob(1,0)=-1; Cam2Rob(2,2)=-1;

        xyzn.setZero();
        int cuenta = 0;
        int j=0;

        cloud_a.width   = (int)ptpairs1.size()/2;
        cloud_a.height  = 1;
        cloud_a.points.resize (cloud_a.width * cloud_a.height);
        cloud_b.width  = cloud_a.width;
        cloud_b.height  = 1;
        cloud_b.points.resize (cloud_b.width * cloud_b.height);


        int k=0;
        for (uint i=0; i<ptpairs1.size(); i+=2)
        {
                /* @nota Check if is any ROS package that has the 3D reconstruccion already implemented to change this section */

                //Extraemos las coordenadas de los key points en L y R
                CvSURFPoint* r11 = (CvSURFPoint*)cvGetSeqElem( leftKeypoints1, ptpairs1[i] );
                CvSURFPoint* r21 = (CvSURFPoint*)cvGetSeqElem( rightKeypoints1, ptpairs1[i+1] );
                Ul = cvPointFrom32f(r11->pt).x;
                Vl = cvPointFrom32f(r11->pt).y;
                Ur = cvPointFrom32f(r21->pt).x;
                Vr = cvPointFrom32f(r21->pt).y;
                j++;

                An.setZero();
                An(0,0)= -alfa_l[0]; An(0,2)= Ul-uol;
                An(1,1)= -alfa_l[1]; An(1,2)= Vl-vol;
                An.row(2)= ( (Ur-uor)*R_.row(2) ) - ( alfa_r[0]*R_.row(0) );
                An.row(3)= ( (Vr-vor)*R_.row(2) ) - ( alfa_r[1]*R_.row(1) );
                Bn.setZero();
                Bn(2,0) = ((uor-Ur)*tz + alfa_r[0]*tx);
                Bn(3,0) = ((vor-Vr)*tz + alfa_r[1]*ty);

                //xyzn.setZero();
                xyzn=(((An.transpose()*An).inverse())*An.transpose())*Bn;



                cloud_a.points[k].x=xyzn(2,0);
                cloud_a.points[k].y=(-1)*xyzn(0,0);
                cloud_a.points[k].z=(-1)*xyzn(1,0);

                k++;
           }

        k=0;
        for (uint i=0; i<ptpairs2.size(); i+=2)
        {
                /* @nota Check if is any ROS package that has the 3D reconstruccion already implemented to change this section */

                //Extraemos las coordenadas de los key points en L y R
                CvSURFPoint* r12 = (CvSURFPoint*)cvGetSeqElem( leftKeypoints2, ptpairs2[i] );
                CvSURFPoint* r22 = (CvSURFPoint*)cvGetSeqElem( rightKeypoints2, ptpairs2[i+1] );
                Ul = cvPointFrom32f(r12->pt).x;
                Vl = cvPointFrom32f(r12->pt).y;
                Ur = cvPointFrom32f(r22->pt).x;
                Vr = cvPointFrom32f(r22->pt).y;
                j++;

                An.setZero();
                An(0,0)= -alfa_l[0]; An(0,2)= Ul-uol;
                An(1,1)= -alfa_l[1]; An(1,2)= Vl-vol;
                An.row(2)= ( (Ur-uor)*R_.row(2) ) - ( alfa_r[0]*R_.row(0) );
                An.row(3)= ( (Vr-vor)*R_.row(2) ) - ( alfa_r[1]*R_.row(1) );
                Bn.setZero();
                Bn(2,0) = ((uor-Ur)*tz + alfa_r[0]*tx);
                Bn(3,0) = ((vor-Vr)*tz + alfa_r[1]*ty);

                //xyzn.setZero();
                xyzn=(((An.transpose()*An).inverse())*An.transpose())*Bn;

                cloud_b.points[k].x=xyzn(2,0);
                cloud_b.points[k].y=(-1)*xyzn(0,0);
                cloud_b.points[k].z=(-1)*xyzn(1,0);

                k++;
           }


        //Publicar para ver todos los puntos sin filtrar
        pub_pcl1.publish(cloud_a);
        pub_pcl2.publish(cloud_b);

        printf(" 3D RECONSTRUCTION (1): tamaño cloud_a %i cloud_b %i \n",(int)cloud_a.size(), (int)cloud_b.size());
 }

void Error_Prop::VO_3D_to_matches(){

//      printf(" 3D RECONSTRUCTION (0): starting\n");

        Matrix<float,3,4> An;

        Matrix<float,3,1> UVl,UVr;
        Matrix<float,4,1> xyz;


        cloud_c   = cloud_a;

        UV_l.width  = cloud_a.width;
        UV_l.height  = 1;
        UV_l.points.resize (UV_l.width * UV_l.height);
        UV_r.width  = cloud_a.width;
        UV_r.height  = 1;
        UV_r.points.resize (UV_r.width * UV_r.height);

        for (uint i=0; i<cloud_a.size(); i++)
        {

                xyz(0) = cloud_c.points[i].x;
                xyz(1) = cloud_c.points[i].y;
                xyz(2) = cloud_c.points[i].z;
                xyz(3) = 1;

                An.setZero();
                An(0,0)= -alfa_l[0]; An(0,2)= uol;
                An(1,1)= -alfa_l[1]; An(1,2)= vol;
                An(2,2)=1;

                UVl= An*xyz;

                An.setZero();
                An(0,0)= -alfa_r[0]; An(0,2)= uor;
                An(1,1)= -alfa_r[1]; An(1,2)= vor;
                An(2,2)=1;

                UVr = An*H_l2r_*xyz;

                UV_l.points[i].x=UVl(0);
                UV_l.points[i].y=UVl(1);
                UV_l.points[i].z=1;
                UV_r.points[i].x=UVr(0);
                UV_r.points[i].y=UVr(1);
                UV_r.points[i].z=1;
           }
        pub_uvl.publish(UV_l);
        pub_uvr.publish(UV_r);

 }


void Error_Prop::VO_matchFilter(){

        //--------Filtrado de matches, quedan solo los que sirven (de las 4 imagenes)-------

        printf(" MATCH FILTER (0): Pares en 1 %i Pares en 2 %i Pares verticales %i \n",(int)ptpairs1.size()/2,(int)ptpairs2.size()/2,(int)ptpairs3.size()/2);

        int limp=0;

        // 1.- Filtramos los matches verticales con los nuevos horizontales (3 con 1)

        for (uint i=0;i<ptpairs3.size();i+=2){
                for(uint j=0;j<ptpairs1.size();j+=2)
                {
                        if ( (ptpairs3[i]==ptpairs1[j]))
                        {
                                limp=1;                                 //los que estan en 1 y 3
                        }
                        else if ( ptpairs3[i]<=ptpairs1[j])
                        {
                                j=ptpairs1.size();
                        }
                }
                if(limp==0)
                {                                                               //el match horizontal no tiene match vertical
                        ptpairs3.erase(ptpairs3.begin()+i);
                        ptpairs3.erase(ptpairs3.begin()+i);
                        i-=2;
                }
                limp=0;
        }

        //Error_Prop::VO_ViewStereo();
        //Error_Prop::VO_ViewConsecutive();

        printf(" MATCH FILTER (1): Pares en 1 %i Pares en 2 %i Pares verticales %i \n",(int)ptpairs1.size()/2,(int)ptpairs2.size()/2,(int)ptpairs3.size()/2);


        // 2.- Filtramos los matches verticales con los viejos horizontales (3 con 2)

        limp=0;
        for (uint i=0;i<ptpairs3.size();i+=2){
                for(uint j=0;j<ptpairs2.size();j+=2)
                {                       //se puede mejorar la eficiencia de esto
                        if ( (ptpairs3[i+1]==ptpairs2[j]))
                        {
                                limp=1;                                 //los que estan en 1 y 3
                        }
                }
                if(limp==0)
                {                                                               //el match horizontal no tiene match vertical
                        ptpairs3.erase(ptpairs3.begin()+i);
                        ptpairs3.erase(ptpairs3.begin()+i);
                        i-=2;
                }
                limp=0;
        }

        printf(" MATCH FILTER (2): Pares en 1 %i Pares en 2 %i Pares verticales %i \n",(int)ptpairs1.size()/2,(int)ptpairs2.size()/2,(int)ptpairs3.size()/2);

        // 3.- Filtramos los matches horizontales presentes en el vertical (1 con 3)

        limp=0;

        for (int i=0;i<(int)ptpairs1.size();i+=2){
                if ( (ptpairs1[i]!=ptpairs3[i])){
          //            printf("eliminando..\n");
                        ptpairs1.erase(ptpairs1.begin()+i);
                        ptpairs1.erase(ptpairs1.begin()+i);
                        i-=2;
                }
        }

        printf(" MATCH FILTER (3): Pares en 1 %i Pares en 2 %i Pares verticales %i \n",(int)ptpairs1.size()/2,(int)ptpairs2.size()/2,(int)ptpairs3.size()/2);

        // 4.- Filtramos los matches horizontales viejos presentes en el vertical (2 con 3)

        vector<int> lista;
        lista.clear();

        for (uint i=0;i<ptpairs3.size();i+=2){
                for(uint j=0;j<ptpairs2.size();j+=2)
                {                       //se puede mejorar la eficiencia de esto
                        if ( (ptpairs3[i+1]==ptpairs2[j]))
                        {
                            lista.push_back(ptpairs2[j]);
                            lista.push_back(ptpairs2[j+1]);
                        }
                }
        }

        ptpairs2.clear();
        ptpairs2=lista;

        printf(" MATCH FILTER (4): Pares en 1 %i Pares en 2 %i Pares verticales %i \n",(int)ptpairs1.size()/2,(int)ptpairs2.size()/2,(int)ptpairs3.size()/2);

        //Error_Prop::VO_ViewAll();
    }


void Error_Prop::VO_cloudFilter(){

//      printf(" CLOUD FILTER (0): Size cloud_wpts %i cloud_cpts %i \n",(int)wpts1.size(),(int)cpts1.size());
        //El tamaño de las nubes deben ser iguales

        if (START==TRUE)
        {
                cpts_ant.width  = cpts1.size();
                cpts_ant.height = 1;
                cpts_ant.points.resize (cpts_ant.width * cpts_ant.height);
                pcl::copyPointCloud(cpts1,cpts_ant);
        }

        wpts1.width  = ptpairs2.size()/2;
        wpts1.height = 1;
        wpts1.points.resize (wpts1.width * wpts1.height);

        wpts1RT.width  = ptpairs2.size()/2;
        wpts1RT.height = 1;
        wpts1RT.points.resize (wpts1RT.width * wpts1RT.height);

        cpts1.width  = ptpairs1.size()/2;
        cpts1.height = 1;
        cpts1.points.resize (cpts1.width * cpts1.height);

        indices_a.clear();
        indices_b.clear();

        indices_a.resize(ptpairs1.size()/2);
        indices_b.resize(ptpairs2.size()/2);

        int limp=0;

        for (uint i=0;i<ptpairs1.size();i+=2)
        {
                for (uint j=0;j<ptpairs1_resp.size();j+=2)
                {
                        if (ptpairs1[i]==ptpairs1_resp[j])
                        {
                                indices_a[i/2]=j/2;
                                limp=1;
                        }
                }
                if (limp==0)
                {
                        cout << "NO ENCONTRO EL MATCH!!!!!! \n" <<endl;
                }
                limp=0;

        }

        for (uint i=0;i<ptpairs2.size();i+=2)
        {
                for (uint j=0;j<ptpairs2_resp.size();j+=2)
                {
                        if (ptpairs2[i]==ptpairs2_resp[j])
                        {
                                indices_b[i/2]=j/2;
                                limp=1;
                        }
                }
                if (limp==0)
                {
                        cout << "NO ENCONTRO EL MATCH!!!!!! \n" <<endl;
                }
                limp=0;
        }

        //Nube en el frame de la camara (puntos i)
        pcl::copyPointCloud(cloud_a,indices_a,cpts1);
        //Nube en el frame del mundo (puntos i-1)
        pcl::copyPointCloud(cloud_b,indices_b,wpts1);


//      printf("size: cloud_a %i cloud_b %i cpts1 %i wpts1 %i ",(int)cloud_a.size(),(int)cloud_b.size(),(int)cpts1.size(),(int)wpts1.size());

        if (START==FALSE)
        {
                pcl::copyPointCloud(cpts1,cpts_ant);
        }

        pub_cpts1.publish(cpts1); //pub_pcl1.publish(cpts1);            //Publicamos los puntos actuales
        pub_wpts1.publish(wpts1); //pub_pcl2.publish(cpts_ant);         //Publicamos los puntos anteriores (los que fueron publicados antes)

//      printf(" CLOUD FILTER (0): Size cloud_wpts %i cloud_cpts %i \n",(int)wpts1.size(),(int)cpts1.size());

}


void Error_Prop::VO_get_RT_RANSAC()
{
        int num;
        float err,errorant,errRT,errRTo;
        Eigen::Vector4f centroid_wpts, centroid_cpts,centroid_new,centroid_nubet,centroid_nubew,centroid_wpts1RT;

        srand (time(NULL));

        err=1000;
        errorant=1000;

        Rot_12.setIdentity();
        Tras_12.setZero();

        //calculo de los centroides originales

        printf(" RANSAC (0): Size cloud a %i Size cloud b %i \n",(int)cloud_a.size(),(int)cloud_b.size());

        pcl::compute3DCentroid (cloud_b, centroid_wpts);
        pcl::compute3DCentroid (cloud_a, centroid_cpts);   //cloud_a -> cpts1

        pub_cpts1.publish(cloud_a);
        pub_wpts1.publish(cloud_b);

        vector<int> ind;

        Eigen::Matrix4f transf;

        transformation.setIdentity();

        for (int j=0; j<150 ;j++)               // Numero de veces que repetimos este ciclo nos dice cuanto vamos a buscar la mejor R y T
        {
                ind.clear();
                for (int i=0; i<6; i++)
                {       //Seleccion de los puntos aleatorios de la nube para el calculo de la R y T
                        num=rand() % cloud_b.size();
                        ind.push_back(num);
                        if(ind.size()>6)
                            cout << "INDICES > 6!!!!!!!!!!!!!"<< endl;
                }

                pcl::copyPointCloud(cloud_a,ind,cloud_t);
                pcl::copyPointCloud(cloud_b,ind,cloud_w);
                pcl::copyPointCloud(cloud_w,cloud_wRT);

                transf.setIdentity();

                // Estimate the centroids of source, target
                Eigen::Vector4f centroid_src, centroid_tgt;
                pcl::compute3DCentroid (cloud_w, centroid_src);
                pcl::compute3DCentroid (cloud_t, centroid_tgt);

                // Subtract the centroids from source, target
                Eigen::MatrixXf cloud_src_demean;
                Eigen::MatrixXf cloud_tgt_demean;
                pcl::demeanPointCloud (cloud_w, centroid_src, cloud_src_demean);  //cloud_w
                pcl::demeanPointCloud (cloud_t, centroid_tgt, cloud_tgt_demean);  //cloud_t

                // Assemble the correlation matrix H = source * target'
                Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();

                // Compute the Singular Value Decomposition
                Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);  //puede ser thin
                Eigen::Matrix3f u = svd.matrixU ();
                Eigen::Matrix3f v = svd.matrixV ();

                // Compute R = V * U'
                Eigen::Matrix3f R;

                if (u.determinant () * v.determinant () < 0)
                {
                        //printf(" RANSAC (1): inv RANSAC\n");
                        for (int x = 0; x < 3; ++x)
                        {
                                v (x, 2) *= -1;
                        }
                }


                R = v * u.transpose ();

                transf.setIdentity();
                transf.topLeftCorner<3, 3> () = R;

                // Traslacion
                Eigen::Vector3f R_src = R * centroid_src.head<3> ();
                Eigen::Vector3f T = centroid_tgt.head<3> () - R_src;
                transf.block<3,1>(0,3) = T;

                //transformamos la nube w con la R y T calculadas
                pcl::transformPointCloud(cloud_w,cloud_wRT,transf);


                // transformation matrix evaluation
                err=0;
                for (int i=0; i< (int)cloud_t.size(); i++)
                {
                        Eigen::Vector3f err_pto = cloud_t.points[i].getVector3fMap() - cloud_wRT.points[i].getVector3fMap() ;
                        err += (pow(err_pto(0),2)+pow(err_pto(1),2)+pow(err_pto(2),2));
                }

                if( (err<errorant)/*&&(err<2)*/ )
                {
                        Rot_12  = R;
                        Tras_12 = T;
                        transformation=transf;
                        errorant=err;
                }


        }

        cout << "best transf \n" << transformation << endl;

        //Utilizamos la mejor rotacion y traslacion calculada
        pcl::transformPointCloud(cloud_b,wpts1RT,transformation);               //rotacion y traslacion de la nube de puntos wpts1 (world frame)
        pcl::compute3DCentroid (wpts1RT, centroid_wpts1RT);             //calculo de la media

        vector<int> copiar;
        copiar.clear();

        // cloud filter, the points with a error bigger than the threshold are deleted
        float thres = 1;
        err=0;
        for (int i=0; i< (int)cloud_b.size(); i++)
        {
                Eigen::Vector3f err_pto = cloud_a.points[i].getVector3fMap() - wpts1RT.points[i].getVector3fMap() ;
                err = (pow(err_pto(0),2)+pow(err_pto(1),2)+pow(err_pto(2),2));
                if( err<thres )
                {
                        //los que tienen mucho error no lo copiamos
                        copiar.push_back(i);
                }
        }

        pcl::copyPointCloud(cloud_b,copiar,wpts1RANSAC);
        pcl::copyPointCloud(cloud_a,copiar,cpts1RANSAC);

        cout << " size without outliers " << wpts1RANSAC.size() << " and " << cpts1RANSAC.size() << endl;

        // At this point we have the two point clouds without outliers
        if (wpts1RANSAC.size() >= 6)
        {
                //--------------------------Calculo de la nueva R y T con los puntos que pasaron el RANSAC-------------------------------

                // Estimate the centroids of source, target
                Eigen::Vector4f centroid_src_RANSAC, centroid_tgt_RANSAC;
                pcl::compute3DCentroid (wpts1RANSAC, centroid_src_RANSAC);
                pcl::compute3DCentroid (cpts1RANSAC, centroid_tgt_RANSAC);

                // Subtract the centroids from source, target
                Eigen::MatrixXf cloud_src_demean_RANSAC;
                Eigen::MatrixXf cloud_tgt_demean_RANSAC;
                pcl::demeanPointCloud (wpts1RANSAC, centroid_src_RANSAC, cloud_src_demean_RANSAC);  //cloud_w
                pcl::demeanPointCloud (cpts1RANSAC, centroid_tgt_RANSAC, cloud_tgt_demean_RANSAC);  //cloud_t

                // Assemble the correlation matrix H = source * target'
                Eigen::Matrix3f H_RANSAC = (cloud_src_demean_RANSAC * cloud_tgt_demean_RANSAC.transpose ()).topLeftCorner<3, 3>();

                // Compute the Singular Value Decomposition
                Eigen::JacobiSVD<Eigen::Matrix3f> svd_RANSAC (H_RANSAC, Eigen::ComputeFullU | Eigen::ComputeFullV);  //puede ser thin
                Eigen::Matrix3f u_RANSAC = svd_RANSAC.matrixU ();
                Eigen::Matrix3f v_RANSAC = svd_RANSAC .matrixV ();

                // Compute R = V * U'
                Eigen::Matrix3f R_RANSAC;

                if (u_RANSAC.determinant () * v_RANSAC.determinant () < 0)
                {
                        printf(" ============================== RANSAC (1): inv \n");
                        for (int x = 0; x < 3; ++x)
                        {
                                v_RANSAC (x, 2) *= -1;
                        }
                }

                //Rotacion
                R_RANSAC = v_RANSAC * u_RANSAC.transpose ();

                // Traslacion
                Eigen::Vector3f Rc_RANSAC = R_RANSAC * centroid_src_RANSAC.head<3> ();
                Eigen::Vector3f T_RANSAC = centroid_tgt_RANSAC.head<3> () - Rc_RANSAC;

                Rot_12  = R_RANSAC;
                Tras_12 = T_RANSAC;

                transformation.setIdentity();
                transformation.topLeftCorner<3, 3> () = R_RANSAC;
                transformation.block<3,1>(0,3) = T_RANSAC;

                pub_src.publish(wpts1RANSAC);                                   //inliers de ransac antes de trasladar y rotar
                pub_src_RT.publish(wpts1RT);                                    //publicacion de la nube rotada y trasladada solo con los inliers

                cout << "def transf \n" << transformation << endl;
        }
        else
        {
                ROS_INFO("NO HAY PUNTOS SUFICIENTES CON ESTE THRESHOLD DE ERROR PARA EL RANSAC");
                Rot_12.setIdentity();
                Tras_12.setZero();
                transformation.setIdentity();
        }



}


void Error_Prop::VO_RT_to_world(){

            Matrix<float,3,3> R_cam2rob;
            R_cam2rob.setZero();
            R_cam2rob(0,2) = 1; R_cam2rob(1,0) = -1; R_cam2rob(2,1) = -1;
            //R_cam2rob(0,2) = -1; R_cam2rob(1,2) = 1; R_cam2rob(2,0) = -1;  //Da la inclinada hacia arriba

            current_time = ros::Time::now();

            //since all odometry is 6DOF we'll need a quaternion created from yaw
            Matrix<float,3,1> ang;

            //Matrix<float,3,3> Rota;
            //Rota = transformation.topLeftCorner<3, 3> ();
            //Rota = Rota*R_cam2rob;

            geometry_msgs::Quaternion quat_msg;

            if (START==FALSE)
            {
                Rot_i.setIdentity();
                Tras_i.setZero();

                ypr(Rot_i,ang);
                tf::Quaternion quat = tf::createQuaternionFromRPY(ang(2,0),ang(1,0),ang(0,0)); //0 2 1
                //Rot_i = Rot_i*R_cam2rob;

                    //trans_ = Vector4d(0,0,0,1);
                    //fq.coeffs() = Vector4d(0,0,0,1);


            //ypr(transformation.topLeftCorner<3, 3> (),ang);

            //Tras_12 = R_cam2rob * Tras_12;

            //if (START==FALSE)
            //{
    //ODOMETRY
                odom3D.position.x=Tras_i(0,0);
                odom3D.position.y=Tras_i(1,0);
                odom3D.position.z=Tras_i(2,0);
                tf::quaternionTFToMsg(quat, quat_msg);
                odom3D.orientation= quat_msg;

                quat_ant = quat;

            }
            else
            {
                //Rot_12 = R_cam2rob * Rot_12;
                //Tras_12 = R_cam2rob * Tras_12;


                Tras_i = Rot_i * Tras_12 + Tras_i;
                cout << Tras_i<< endl;
                Rot_i = Rot_i * Rot_12;

                ypr(Rot_i,ang);
                tf::Quaternion quat = tf::createQuaternionFromRPY(ang(2,0),ang(1,0),ang(0,0)); //0 2 1

                quat = quat * quat_ant;
                tf::quaternionTFToMsg(quat, quat_msg);


     /*               if (transformation(0,3)>0.5)
                        odom3D.position.x+=0.5;
                    else if (transformation(0,3)<-0.5)
                        odom3D.position.x+=-0.5;
                    else */

                odom3D.position.x=Tras_i(0);//transformation(0,3);
    /*
                    if (transformation(1,3)>0.5)
                        odom3D.position.y+=0.5;
                    else if (transformation(1,3)<-0.5)
                        odom3D.position.y+=-0.5;
                    else */

                odom3D.position.y=Tras_i(1);//transformation(1,3);
    /*
                    if(transformation(2,3)>0.5)
                        odom3D.position.z+=0.5;
                    else if (transformation(2,3)<-0.5)
                        odom3D.position.z+=-0.5;
                    else */

               odom3D.position.z=0;//Tras_i(2);//transformation(2,3)
               odom3D.orientation=quat_msg;

               pcl::copyPointCloud(cloud_odom,cloud_odom2);

               quat_ant=quat;

            }

            //we'll publish the odometry message over ROS

            nav_msgs::Odometry od;
            od.header.stamp = current_time;
            od.header.frame_id = "od";

            //set the position
            od.pose.pose.position.x = odom3D.position.x;
            od.pose.pose.position.y = odom3D.position.y;
            od.pose.pose.position.z = odom3D.position.z;
            od.pose.pose.orientation = odom3D.orientation;

            //publish the message
            pub_od.publish(od);


            cout << "transf \n" << transformation << endl;
            cout << "\n odom msg \n" << odom3D << endl;

            cloud_odom.points[0].x = odom3D.position.x;
            cloud_odom.points[0].y = odom3D.position.y;
            cloud_odom.points[0].z = odom3D.position.z;

            pub_odom.publish(cloud_odom);

 }

void Error_Prop::VO_clean()
{
        //----------clear memory for consecutive storage--------------
        cvClearMemStorage(consecutive_Storage);

}


void EnviromentConfiguration(){

    cv::namedWindow("Left");
    cv::namedWindow("Right");
    cv::namedWindow("Stereo matches");
    cv::namedWindow("Vertical matches");
    cv::namedWindow("Show all");
    cvStartWindowThread();
    ROS_INFO("running VO environment configuration..");

}

/*void viewImage(const Mat& Image,vector<KeyPoint>& Keypoints,Mat& outImage)
{
    drawKeypoints(Image,Keypoints,outImage);
    namedWindow(Name,1);
    imshow(Name, Image);
    namedWindow(kp,1);
    imshow(kp,outImage);
    waitKey();
}*/


/*-----------------------------------------------------------------------------------------------------------
                                                  Main
-----------------------------------------------------------------------------------------------------------*/


int main(int argc, char** argv)
{
        ros::init(argc, argv, "error_prop");

        Error_Prop Error;

        Error.init();
        EnviromentConfiguration();

//--------------------------Calculo a tiempo real-----------------------------

        ros::Rate loop_rate(1);   //1 Hz
        loop_rate.sleep();
        loop_rate.sleep();

        while(TRUE)
        {
                ROS_INFO("while");
                ros::spinOnce();

                loop_rate.sleep();
                Error.run();
    
                int wait = cvWaitKey(10) & 255;
                if( wait == 27 ) break;                 //wait for escape
      }


        return 0;
}