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
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
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 }
00123 }
00124
00125 # endif