diff options
Diffstat (limited to 'src/feature.h')
| -rw-r--r-- | src/feature.h | 13 |
1 files changed, 10 insertions, 3 deletions
diff --git a/src/feature.h b/src/feature.h index eb3e5bd..fe2afea 100644 --- a/src/feature.h +++ b/src/feature.h @@ -1,12 +1,17 @@ #ifndef feature_INC #define feature_INC #include <Eigen/Dense> +#include <iostream> #include "types.h" using Eigen::Vector3d; using Eigen::Matrix; using Eigen::MatrixXd; using Eigen::Quaterniond; +using std::cout; +using std::cerr; +using std::endl; + /* * ===================================================================================== @@ -18,8 +23,8 @@ class Feature { public: /* ==================== LIFECYCLE ======================================= */ - Feature (); - Feature ( int id, const Vector3d &xib, const Vector3d &xpb, const Vector3d &xbw, const Quaterniond &q ); /* constructor */ + Feature ( int id, const Vector3d &xs, const Vector3d &xr, + const Vector3d &xbw, const Quaterniond &q, double z ); /* constructor */ /* ==================== ACCESSORS ======================================= */ int id(); @@ -29,7 +34,9 @@ class Feature void motionModel ( const Vector3d &ang, const Vector3d &vel, const double dt); /* ==================== OPERATORS ======================================= */ - Matrix<double,3,9> Fx(); + Vector3d findDepth( const Quaterniond &q, double z, const Vector3d &xs, + 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,6,9> Hx( const Vector3d &pos, const Quaterniond &q); Matrix<double,6,3> Hy( const Vector3d &pos, const Quaterniond &q); |