diff options
author | Martin Miller | 2017-03-25 16:16:02 -0500 |
---|---|---|
committer | Martin Miller | 2017-03-25 16:16:02 -0500 |
commit | ec60ec9eba475fcfbdfc5d7bd157095affb7d66f (patch) | |
tree | 8e45e498b7dcc861d6975279c12c51ce46dbd890 /src/state.h | |
parent | 1c233f6fd05502585928e058d5d2c0ff5e23881a (diff) | |
download | refslam-ec60ec9eba475fcfbdfc5d7bd157095affb7d66f.zip refslam-ec60ec9eba475fcfbdfc5d7bd157095affb7d66f.tar.gz |
Update State::motionModel and addFeatures
Add the feature motion model update to the state method. It is
implemented in two ways: one is the for loop that operates on each
feature instance directly, which is how it has been implemented in the
past. The second method is to compose a large L matrix for all features
and compute the per feature dx in one go. It is expected that the second
method is faster, but it is not yet tested for speed.
Diffstat (limited to 'src/state.h')
-rw-r--r-- | src/state.h | 10 |
1 files changed, 7 insertions, 3 deletions
diff --git a/src/state.h b/src/state.h index 1d8dd63..67b235f 100644 --- a/src/state.h +++ b/src/state.h @@ -9,7 +9,8 @@ #include "feature.h" #include "types.h" -#define MAXFEATURES 30 +#define MAXFEATURES 50 +#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once using Eigen::Matrix; using Eigen::MatrixXd; using Eigen::Dynamic; @@ -34,6 +35,8 @@ class State bool exists(int id); MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt); MatrixXd Q(double dt); + Matrix<double,9,9> Pxx(); + Matrix<double,Dynamic,6> L(); /* ==================== MUTATORS ======================================= */ void accelerometer_bias(const Vector3d &b); @@ -58,12 +61,13 @@ class State private: /* ==================== METHODS ======================================= */ - void expandP(int Nnew); + void expandP(); void addFeatures(std::vector<measurement_t> &F, const Quaterniond &q); /* ==================== DATA MEMBERS ======================================= */ Body *body; - Matrix<double, Dynamic, Dynamic, 0, 9+3*MAXFEATURES, 9+3*MAXFEATURES> P; + //Matrix<double, Dynamic, Dynamic, 0, 9+3*MAXFEATURES, 9+3*MAXFEATURES> P; + Matrix<double, Dynamic, Dynamic> P; char utm_c; int utm_i; std::list<Feature *> features; |