VisualOdometryPTAMM.h

Go to the documentation of this file.

//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno
//
// This file is part of MIP.
//
// MIP is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// MIP is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------



#ifndef _VISUALODOMETRYPTAMM_H
#define _VISUALODOMETRYPTAMM_H


#include "VisualOdometry.h"

#include <cvd/Linux/v4lbuffer.h>
#include <cvd/colourspace_convert.h>
#include <cvd/colourspaces.h>
#include <cvd/image.h>
#include <cvd/byte.h>
#include <cvd/rgb.h>
#include <netinet/in.h>
#include <netdb.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <pthread.h>
#include <sys/types.h>
#include <sys/un.h>
#include <string.h>

//#include <opencv2/cv.h>

//#include <opencv/highgui.h>
#include "opencv2/highgui/highgui.hpp"

//#include <opencv2/cvaux.h>

#include "opencv2/imgproc/imgproc.hpp"
#include <iostream>
#include <sstream>
#include <pthread.h>

#include <baselib.h>
#include <VrpnTracker.h>

#include <Camera.h>

#include <time.h>

#define PORT 7777

using namespace MipBaselib;
//using namespace CVD;
using namespace std;

namespace MipAlgorithms{


 /* @{ */


    //Pose3DandTime PTAMMdata;

    //pthread_mutex_t mutex2 = PTHREAD_MUTEX_INITIALIZER;
    //int poseReady = 0;


    class PTAMMVrpnHandler : public VrpnHandler{
    public:
      PTAMMVrpnHandler(Pose3DandTime* PTAMMdata);
//      inline bool isDataReady(){ return dataReady;}
//      inline void setDataState(bool value){ dataReady = value;}
    private:
        Pose3DandTime* handlerData;
        //bool dataReady;
        //timeval prevTime;

        virtual void handleVrpnData(){
                timeval measureTime;
                //Decimal x,y,z,roll,pitch,yaw;
  double matrix[3][3];
  VrpnTracker::quat2Mat(VrpnTracker::instance()->currPos.quat, matrix);
  double rpy[3];
  VrpnTracker::matToEA(matrix, rpy);

  handlerData->time = VrpnTracker::instance()->currPos.msg_time;
                //timeStamp = (double) packageTime.tv_sec;
  handlerData->x = VrpnTracker::instance()->currPos.pos[0] / 1000.0;
  handlerData->y = VrpnTracker::instance()->currPos.pos[1] / 1000.0;
  handlerData->z = VrpnTracker::instance()->currPos.pos[2] / 1000.0;
  handlerData->roll  = rpy[0];
  handlerData->pitch = rpy[1];
  handlerData->yaw   = rpy[2];
                //handlerData->upToDate = true;
//                if ((handlerData->time.tv_sec - prevTime.tv_sec) * 1000000 + (handlerData->time.tv_usec - prevTime.tv_usec) > 0 )
//                {
//                    dataReady = true;
//                }
//                else
//                {
//                    cout << "PTAMMVrpnHandler :handleVrpnData() - WARNING! TIME IS NEGATIVE" << endl;
//                }

                //poseReady=1;
                //cout << "time=" << timeStamp <<",x="<< x << ",y=" << y << ",z=" << z << ",r=" << roll << ",p=" << pitch << ",y=" << yaw << endl;
  //printf("time=%ld.%06ld,x=%5.2f, y=%5.2f, z=%5.2f, r=%5.2f, p=%5.2f, y=%5.2f\n",handlerData->time.tv_sec,handlerData->time.tv_usec,handlerData->x,handlerData->y,handlerData->z,handlerData->roll,handlerData->pitch,handlerData->yaw);
        }
    };


 class VisualOdometryPTAMM : public VisualOdometry {

    public:
        VisualOdometryPTAMM(IplImage* _imFrame);
        // Brief overload to also set the path of the logfile
        VisualOdometryPTAMM(IplImage* _imFrame, char* logFilePath);
        ~VisualOdometryPTAMM();
        bool getPose3DUnscaled(Pose3DandTime* pose);
        bool readDataFromPTAMM(Pose3DandTime* pose, Time* measTime, Decimal* delay);
        void checkAndSendImage(); // video-stream Thread

        void setImage(CVD::Image<CVD::byte>* imFrame); //TODO just in case we get CVD frame from Cam->getFrame()
        void setImageAndPose(IplImage* _imFrame, Pose3DandTime* pose = 0, int dataType = 0); // brief method overload to set external time stamp and pose
        void setImage(IplImage* imFrame);
        void show();
        bool loadImage();
        void clear();
        inline bool gotConnection(){return connection;};
        inline int getPoseType(){return poseType;};
        inline void setPoseType(int value){poseType = value;};
        inline void setLogActive(bool value){activateLog = value;};
        void setLogFile(char* value);
        /*void startDataLoop(){
        _ptammThread.startWork();
 }
 void stopDataLoop(){
        _ptammThread.stopWork();
        }*/

    private:
        
        int    serversock, clientsock;
        // Image stream variables
        pthread_t   thread_s;
        CVD::Image<CVD::byte> imageBW;
        pthread_mutex_t mutex;
        IplImage* _myFrame;
        //timeval frameTimeStamp;
        bool image_ready;
        // Vrpn variables
        pthread_t threadVrpn;
        PTAMMVrpnHandler* PTAMMdataHandler;
        bool connection; //check if PTAMM has connected to video stream server

        Pose3DandTime myRecPose3D; // Pose received via VRPN from PTAMM

        Pose3DandTime mySndPose;   // Pose to be sent to PTAMM for various porpouses
        int poseType;              // PTAMM map transformation, Filter est.

        bool poseReady;
        timeval prevTime;
        
        bool activateLog;
        char* ptammLog;
        ofstream logFilePTAMM;
        /*
        //Thread      _ptammThread;
        MipBaselib::Time lastFrameTime; //tempo del frame precedente
        MipBaselib::Time newFrameTime;  //tempo del nuovo frame
        int is_data_ready;
        int         serversock, clientsock;
        pthread_mutex_t mutex;
        CVD::Image<CVD::byte> imFrame;  //frame da trasmettere via socket
        //posa unscaled (SE3)
        */
    };
 /* @} */
};// end namespace MipAlgorithms
 
#endif