FeatExtrCam.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// 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 __FEAT_EXTRCAM_H_
#define __FEAT_EXTRCAM_H_


#ifdef MIP_HOST_APPLE
 #include <applePatch.h>
#endif


#include "HBstructs.h"
#include <Time.h>
#include <Camera.h>
#include <VisualTracking.h>
#include <Resource.h>
#include <MobileRob.h>
#include <MobileRob3D.h>
#include <ReconKhCam.h>

#include <Task.h>
#include <SharedMem.h>
#include <CommonOptions.h>
#ifndef MIP_PLATFORM_ARM
#include <DraWin.h>
#endif
#ifdef FOUND_VISP_LIBS
    #include <VisTrackVisp.h>
#else
    #include <VisTrackCamshift.h>
#endif

using namespace MipResources;
using namespace MipAlgorithms;

namespace MipTasks{




 /* @{ */

 enum FeatExtrCamTypes{
  FEAT_EXTRCAM_CAMSHIFT,
  FEAT_EXTRCAM_TYPES_NUM
 };

 static const char * FeatExtrCamTypesNames[FEAT_EXTRCAM_TYPES_NUM] ={
  "Camshift"
 };

 enum FeatExtrCamStates{
  FEAT_EXTRCAM_FIRST_RUN,
  FEAT_EXTRCAM_NORMAL_RUN,
  FEAT_EXTRCAM_LAST_RUN,
  FEAT_EXTRCAM_STATES_NUM
 };

 static const char* FeatExtrCamStatesNames[FEAT_EXTRCAM_STATES_NUM] = {
  "FirstRun",
  "NormalRun",
  "LastRun"
 };

 class FeatExtrCamOptions : public Options {
  public:
   DecimalOption   *trimRoll;          
   DecimalOption   *trimPitch;         
   DecimalOption   *trimYaw;           
   IntOption       *algoType;          
   IntOption       *visDepth;          
   DecimalOption   *inKAngle;          
   DecimalOption   *inKfeatErr;        
   DecimalOption   *inKintegral;       
   IntOption       *heightCtrlType;    
                        StringOption    *log_path;          
                        StringOption    *log_name;          
                        BoolOption      *make_log;          
                        BoolOption      *offLine;
                        
   FeatExtrCamOptions();
   
   string getObjectName() const{
     return "FeatExtrCamOptions";
   }
 };


 class FeatExtrCamVar{
  public:
   CvMat *cv_Jv, *cv_Jomega;                   
   CvMat *cv_U, *cv_V, *cv_W,  *cv_JvInv;      
   CvMat *cv_Error, *cv_velContribute, *cv_omegaContribute, *cv_totalContribute; 
   CvMat *cv_desAngles;                        
   Decimal ControlAngles[2];                   
                        short ctrlEnable;
                        ofstream    _logFile;
                        int         _numrow;
 };

 class FeatExtrCamSharedMem : public SharedMem{
  public:
   void initSharedMem();

   void setMyProtrusions(PosiFeatures &protrusions, MotionModuleTState &protrusionsTState);

   void setMyCorners(PosiFeatures &corners, MotionModuleTState &cornersTState);

   bool printFeat();

   void setMyObervations(vector<Angle> &observ, MotionModuleTState &camObsTState);
 };
 class FeatExtrCam : public Task {
  private:
   static const TaskPlate _plate = FEC_TASK;  

   // Mandatory variables: timing for the scheduler
   static const long int  _maxDurSec    = 0, _maxDurUsec    = 100000;
   static const long int  _maxSamplPeriodSec = 0, _maxSamplPeriodUsec  = 50000;
   static const long int  _minSamplPeriodSec = 0, _minSamplPeriodUsec  = 20000;

   // Reources
   //Uav  *_uav;   ///< Uav performing the task.
                        MobileRob*  _mobileRob;
   Camera *_cam;   
                        FeatExtrCamSharedMem* _sharedMem;

                        #ifdef FOUND_VISP_LIBS
                            VisTrackVisp* _visTrack;  
                        #else
                            VisTrackCamshift* _visTrack;  
                        #endif
   // Algorithms
                        ReconKhCam* _visRecon;


   IplImage*   _frame;   
   ImageFeature  imgFeat;  
   Time t;        

   FeatExtrCamOptions _options; 
   FeatExtrCamVar*  _var;  
   FeatExtrCamStates  _state;  
   FeatExtrCamTypes  _types;  

                        stringstream s;
                        fstream _saveMeasures;
                        fstream _saveIds;
                        fstream _saveCamObsTState;
                        /*
                        #ifndef MIP_PLATFORM_ARM
   DraWin*      _draWin;
   unsigned int _drawList;
   bool         dwOn;
                        #endif
   */
   void _firstRun();

   void _normalRun();

   void _lastRun();

  public:
   FeatExtrCam(ResourcePointers resources, int argc, const char* argv[]);

   ~FeatExtrCam();

   TaskPlate getPlate() const {
    return _plate;
   }

   Time getMaxDuration(){
    return Time(_maxDurSec,_maxDurUsec);
   }
   Time getMaxSamplPeriod(){
    return Time(_maxSamplPeriodSec,_maxSamplPeriodUsec);
   }
   Time getMinSamplPeriod(){
    return Time(_minSamplPeriodSec,_minSamplPeriodUsec);
   }

   TaskOutputs run();
 };
}; // end namespace MipTasks






#endif


/* @} */