diff options
Diffstat (limited to 'src/feature.h')
| -rw-r--r-- | src/feature.h | 24 |
1 files changed, 17 insertions, 7 deletions
diff --git a/src/feature.h b/src/feature.h index ce42d9f..2754f89 100644 --- a/src/feature.h +++ b/src/feature.h @@ -4,15 +4,19 @@ #include <iostream> #include "types.h" -#define FEATURE_NOISE 1 /* Feature process noise */ -#define VIEW_NOISE .33 /* */ -#define INITIAL_VIEW_NOISE .33 /* */ -#define REFLECTION_VIEW_NOISE 1.1 /* */ -#define FEATURECOVX .33 /* */ -#define FEATURECOVY .33 /* */ -#define FEATURECOVRHO 5e-2 /* */ +#define FEATURE_NOISE 1e-3 /* Feature process noise */ +#define VIEW_NOISE 5e-3 /* */ +#define INITIAL_VIEW_NOISE 1e-1 /* */ +#define REFLECTION_VIEW_NOISE 5e-3 /* */ +#define FEATURECOVX .01 /* */ +#define FEATURECOVY .01 /* */ +#define FEATURECOVRHO 25e-4 /* */ #define FEATURECOVRHO_MONO 0.5 /* */ #define RHO_0 1./10. /* */ +#define INLIER_THRESHOLD 5.9915 /* */ +#define RANSAC_LI_THRESHOLD 4e-5 /* */ +#define RANSAC_HI_THRESHOLD 5e-2 /* */ + using Eigen::Dynamic; using Eigen::Matrix; @@ -43,14 +47,20 @@ class Feature /* ==================== ACCESSORS ======================================= */ int id(); Matrix<double,3,3> P0(measurement_type t); + Vector3d asVector(); UTM utm(Vector3d &xbw, Quaterniond &q); /* ==================== MUTATORS ======================================= */ + void asVector(const Vector3d &m); void motionModel ( const Vector3d &ang, const Vector3d &vel, const double dt); void dx( const Vector3d &del); /* ==================== OPERATORS ======================================= */ bool sane(); + bool isInlier(const measurement_t &z, const Matrix<double,9,9> &Pxx, + const Matrix<double,9,3> &Pxy, const Matrix<double,3,3> &Pyy, + const Vector3d &pos, const Quaterniond &q, double thr); + bool isRansacInlier(const measurement_t &z, const Vector3d &pos, const Quaterniond &q); bool inFOV(); Vector3d findDepth( const Quaterniond &q, double z, const Vector3d &xs, const Vector3d &xr); |