Jafar
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines
lsPredictor2D.hpp
00001 # ifndef LS_PREDICTOR_2D 
00002 # define LS_PREDICTOR_2D
00003 
00004 # include <cmath>
00005 # include <iostream> 
00006 #include "lines/lsMisc.hpp"
00007 #include "filter/ConstantVelocityKalmanFilter.hpp"
00008 #include "lines/constPositionKF.hpp"
00009 
00010 namespace jafar{
00011   namespace lines{
00012     
00014 
00027     enum PredictionModel{
00028       MIDPOINT_X_Y,
00029       POLAR_COORD,
00030       ONLY_PERP_MOTION,
00031       PERP_MIDPOINT_PROJ
00032     };
00033       
00034     
00036 
00040     class LsPredictor2D{
00041     public:
00042       LsPredictor2D();
00043       
00045 
00061       void initPredictor(double mx, double my, double phi, double rho, double theta, PredictionModel model=PERP_MIDPOINT_PROJ);
00062       
00064 
00080       /*
00081       TODO Change hard encoding of variances
00082       */
00083       void updatePredictor(double mxOld, double myOld, double phiOld, double mxNew, double myNew, double phiNew, double rhoNew, double thetaNew);
00084     
00086 
00089       double getPredictionEP(double x1Old, double y1Old, double x2Old, double y2Old, double& x1Pred, double& y1Pred, double& x2Pred, double& y2Pred, double l=0);
00090       
00092 
00095       /*
00096       TODO Improve the benchmark
00097       */
00098       double benchmarkTest(){return benchmark;} 
00099       
00100     private:
00101       PredictionModel predMod; 
00102       uint nUpdates;      
00103       double cDist;       
00104       
00105       double benchmark;     
00106 
00107       filter::ConstantVelocityKalmanFilter xKF;    
00108       filter::ConstantVelocityKalmanFilter yKF;    
00109       filter::ConstantVelocityKalmanFilter phiKF;  
00110       
00111       filter::ConstantVelocityKalmanFilter rhoKF;    
00112       filter::ConstantVelocityKalmanFilter thetaKF;  
00113       
00114       filter::ConstantVelocityKalmanFilter dKF;      
00115       filter::ConstantVelocityKalmanFilter alphaKF;  
00116       
00117       filter::ConstantVelocityKalmanFilter xPKF;     
00118       filter::ConstantVelocityKalmanFilter yPKF;     
00119       filter::ConstantVelocityKalmanFilter phiPKF;   
00120       
00121     };
00122   } // namespace lines
00123 } // namespace jafar
00124 
00125 # endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines

Generated on Wed Oct 15 2014 00:37:25 for Jafar by doxygen 1.7.6.1
LAAS-CNRS