00001
00002
00003 #ifndef _LINE_FITTER_KALMAN2_H_
00004 #define _LINE_FITTER_KALMAN2_H_
00005
00006 #include <kernel/jafarMacro.hpp>
00007 #include <image/Image.hpp>
00008
00009 #include "jmath/jblas.hpp"
00010
00011 namespace jafar {
00012 namespace jmath {
00013 class VariableSizeLinearLeastSquares;
00014 }
00015 namespace dseg {
00020 class LineFitterKalman2 {
00021 public:
00027 LineFitterKalman2(double x_o, double y_o, double angle_o);
00028 LineFitterKalman2(const LineFitterKalman2& _lf);
00029 ~LineFitterKalman2();
00037 void addPoint(double t, const jblas::vec2& _point, double angle, double sigmaPx);
00041 double direction() const;
00045 double angle() const;
00049 const jblas::vec2& vecNormal() const;
00053 jblas::vec2 pointAt( double t ) const;
00057 double errorAt( double t ) const;
00058 jblas::sym_mat22 covAt( double t ) const;
00059 void dump() const;
00063 void merge( const LineFitterKalman2* );
00067 bool compatible( const LineFitterKalman2* _lf ) const;
00068 const jblas::vec2& x() const;
00072 double quality() const;
00076 jblas::sym_mat22 vecDirectionCov() const;
00080 jblas::sym_mat22 originCov() const;
00081 jblas::sym_mat22 originMergeCov() const;
00085 jblas::vec2 origin() const;
00089 bool isCompatibleObservation( double _x, double _y ) const;
00093 bool isCompatibleDirectionObservation( double _u, double _v ) const;
00097 double distance( double _x, double _y ) const;
00101 void setUncertainty( double sigma_u, double sigma_x0, double sigma_v, double sigma_y0 );
00105 const jblas::vec4& state() const;
00106 private:
00107 void update();
00108 struct Private;
00109 Private* const d;
00110 };
00111 }
00112 }
00113
00114 #endif
