diff options
Diffstat (limited to 'src/feature.h')
| -rw-r--r-- | src/feature.h | 20 |
1 files changed, 10 insertions, 10 deletions
diff --git a/src/feature.h b/src/feature.h index e9d1060..29c8098 100644 --- a/src/feature.h +++ b/src/feature.h @@ -8,8 +8,8 @@ #define VIEW_NOISE 1e-3 /* */ #define INITIAL_VIEW_NOISE 1e-3 /* */ #define REFLECTION_VIEW_NOISE 1e-2 /* */ -#define FEATURECOVX 1e-4 /* */ -#define FEATURECOVY 1e-4 /* */ +#define FEATURECOVX 5e-5 /* */ +#define FEATURECOVY 5e-5 /* */ #define FEATURECOVRHO 5e-3 /* */ #define FEATURECOVRHO_MONO 0.5 /* */ #define RHO_0 1./10. /* */ @@ -42,7 +42,7 @@ class Feature /* ==================== ACCESSORS ======================================= */ int id(); - Matrix<double,3,3> P0(); + Matrix<double,3,3> P0(measurement_type t); UTM utm(Vector3d &xbw, Quaterniond &q); /* ==================== MUTATORS ======================================= */ @@ -56,15 +56,16 @@ class Feature const Vector3d &xr); Matrix<double,3,9> Fx( double dt ); Matrix<double,3,3> Fy( const Vector3d &vel, const Vector3d &ang, double dt); - Matrix<double,Dynamic,9,0,6,9> Hx( const Vector3d &pos, const Quaterniond &q); - Matrix<double,Dynamic,3,0,6,3> Hy( const Vector3d &pos, const Quaterniond &q); - Matrix<double,Dynamic,1,0,6,1> h( const Vector3d &x, const Quaterniond &q); + Matrix<double,6,9> Hx( const Vector3d &pos, const Quaterniond &q); + Matrix<double,6,3> Hy( const Vector3d &pos, const Quaterniond &q); + Matrix<double,6,1> h( const Vector3d &x, const Quaterniond &q); Matrix<double,3,6> L(); - Matrix<double,Dynamic,1,0,6,1> measurement_vector(const Vector3d &xs, const Vector3d &xr); + Matrix<double,6,1> measurement_vector(const Vector3d &xs, const Vector3d &xr); + Matrix<double,4,1> measurement_vector( const Vector3d &xs ); Matrix<double,3,3> Q(const double dt); - Matrix<double,Dynamic,Dynamic,0,6,6> R(); - Matrix<double,Dynamic,Dynamic,0,6,6> S ( const Matrix<double,9,9> &Pxx, + Matrix<double,6,6> R(); + Matrix<double,6,6> S ( const Matrix<double,9,9> &Pxx, const Matrix<double,9,3> &Pxy, const Matrix<double,3,3> &Pyy, const Vector3d &pos, const Quaterniond &q); Vector3d p2x(const Vector3d &p); @@ -84,7 +85,6 @@ class Feature Vector3d X0; Quaterniond q0; Vector3d xb0w; - measurement_type fType; }; /* ----- end of class Feature ----- */ |