diff options
| -rw-r--r-- | src/state.cpp | 84 | ||||
| -rw-r--r-- | src/state.h | 10 |
2 files changed, 77 insertions, 17 deletions
diff --git a/src/state.cpp b/src/state.cpp index 959cf22..a5a09fd 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -47,7 +47,7 @@ State::update ( const Matrix<double,1,1> &z ) Matrix<double,1,9> H; H=body->H(); Matrix<double,1,1> S; - S=body->S(P); + S=body->S(Pxx()); Matrix<double,9,1> K; K = P*H.transpose()*S.inverse(); P = (Matrix<double,9,9>::Identity()-K*H)*P; @@ -78,9 +78,6 @@ State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond & void State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q) { - // Expand P matrix all at once - expandP(F.size()); - // // Add new features for (auto i=F.begin(); i!=F.end(); ++i) { // Estimate initial depth @@ -88,7 +85,14 @@ State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q) // Create feature Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q, -0.44); + if (!f->sane()) { + delete f; + continue; + } features.push_back(f); + + // Expand P + expandP(); // Initialize P values int j=features.size(); @@ -110,8 +114,6 @@ State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q) State::initializePi ( int i, const Matrix<double,3,3> &Pi ) { int pt=9+3*(i-1); - std::cout << pt << std::endl; - std::cout << P.cols() << std::endl; P.block(pt,0,3,P.cols()) = MatrixXd::Zero(3,P.cols()); P.block(0,pt,P.rows(),3) = MatrixXd::Zero(P.rows(),3); P.block<3,3>(pt,pt) = Pi; @@ -196,11 +198,63 @@ void State::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt) { - Pkk1(ang,q,dt); + //Pkk1(ang,q,dt); body->motionModel(acc,ang,q,dt); + + Vector3d vel; + vel = body->vel(); + +#ifdef FASTMOTIONMODEL + Matrix<double,Dynamic,6> fullL; + fullL = L(); + + Matrix<double,6,1> V; + V << vel[1], vel[2], vel[0], ang[1], ang[2], ang[0]; + + Matrix<double,Dynamic,1> f; + f = fullL*V*dt; + + // Update features' states + { + int rows = 3*features.size(); + auto i = features.begin(); + for (int row=0; row<rows; row+=3, ++i) { + Vector3d dx; + dx = f.segment<3>(row); + (*i)->dx(dx); + } + } + +#else /* ----- not FASTMOTIONMODEL ----- */ + for (auto i=features.begin(); i!=features.end(); ++i) { + (*i)->motionModel(ang,vel,dt); + } +#endif /* ----- not FASTMOTIONMODEL ----- */ return ; } /* ----- end of method State::motionModel ----- */ +/* + *-------------------------------------------------------------------------------------- + * Class: State + * Method: State :: L + * Description: Return the composition of all Li's of the features + *-------------------------------------------------------------------------------------- + */ +Matrix<double,Dynamic,6> +State::L ( ) +{ + Matrix<double,Dynamic,6> l; + int rows = 3*features.size(); + l = Matrix<double,Dynamic,6>::Zero(rows,6); + { + auto i=features.begin(); + for (int row=0; row<rows; row+=3, ++i) { + l.block<3,6>(row,0) = (*i)->L(); + } + } + return l; +} /* ----- end of method State::L ----- */ + void State::position_covariance ( const Matrix<double,3,3> &p ) { @@ -249,17 +303,19 @@ State::exists ( int id ) *-------------------------------------------------------------------------------------- * Class: State * Method: State :: expandP - * Description: Grows P so that Nnew new features can be added. This could mean - * explicitly resizing the P matrix or P being a block of a preallocated matrix. + * Description: Expand the P matrix to fit one more feature. *-------------------------------------------------------------------------------------- */ void -State::expandP ( int Nnew ) +State::expandP ( ) { - int curRows, newRows; - curRows = P.rows(); - newRows = curRows + 3*Nnew; - P.conservativeResize(newRows,newRows); + P.conservativeResize(3+P.rows(),3+P.cols()); return ; } /* ----- end of method State::expandP ----- */ +Matrix<double,9,9> +State::Pxx ( ) +{ + return P.topLeftCorner<9,9>() ; +} /* ----- end of method State::Pxx ----- */ + 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; |