From e43d9f01fcc9ac7c7c070b493378661f02f854f6 Mon Sep 17 00:00:00 2001 From: Martin Miller Date: Tue, 4 Apr 2017 13:07:15 -0500 Subject: Update statesize and remove quaternion input. Rather than use FULLSTATE define, we define STATESIZE, which removes a lot of the preprocessor ifdefs. This should be done in Body and Feature also. Quaterniond was removed as an input to methods when STATESIZE==13 and it is instead accessed from the body state. --- src/state.cpp | 138 +++++++++++++++++++++++++++++++++++++++++++++------------- src/state.h | 37 +++++++++++++--- 2 files changed, 139 insertions(+), 36 deletions(-) (limited to 'src') diff --git a/src/state.cpp b/src/state.cpp index 472ee66..393ffa2 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -20,7 +20,7 @@ State::State ( ) { body = new Body; - P = Matrix::Zero(9,9); + P = Matrix::Zero(STATESIZE,STATESIZE); P.block<3,3>(6,6) = COVBIAS*Matrix3d::Identity(); return ; } /* ----- end of method State::State ----- */ @@ -49,13 +49,20 @@ State::pos ( const UTM &utm ) *-------------------------------------------------------------------------------------- */ MatrixXd +#if STATESIZE==13 +State::H ( const std::vector &z ) +#else State::H ( const Quaterniond &q, const std::vector &z ) +#endif { +#if STATESIZE==13 + Quaterniond q = body->qhat(); +#endif Vector3d pos; pos = body->ned(); MatrixXd h; // Determine the size of H - int cols = 9 + 3*features.size(); + int cols = STATESIZE + 3*features.size(); int rows = Hrows(z); h = MatrixXd::Zero(rows,cols); for (int i=0,row=0; i &z ) fi = featureById(z[i].id); - h.block<6,9>(row,0) = fi->Hx(pos,q); + h.block<6,STATESIZE>(row,0) = fi->Hx(pos,q); h.block<6,3>(row,col) = fi->Hy(pos,q); row += 6; break; @@ -83,11 +90,11 @@ State::H ( const Quaterniond &q, const std::vector &z ) exit(1); } fi = featureById( z[i].id ); - h.block<4,9>(row,0) = fi->Hx(pos,q).block<4,9>(0,0); + h.block<4,STATESIZE>(row,0) = fi->Hx(pos,q).block<4,STATESIZE>(0,0); break; case HEIGHT: - h.block<1,9>(row,0) = body->H(); + h.block<1,STATESIZE>(row,0) = body->H(); row += 1; break; @@ -174,9 +181,17 @@ State::R ( const std::vector &z ) } /* ----- end of method State::R ----- */ MatrixXd +#if STATESIZE==13 +State::partialUpdate( const MatrixXd &h, const MatrixXd &S, + const std::vector &z) +#else State::partialUpdate( const MatrixXd &h, const MatrixXd &S, const std::vector &z, const Quaterniond &q ) +#endif { +#if STATESIZE==13 + Quaterniond q = body->qhat(); +#endif MatrixXd K; // P^T is implied here since P is symmetric K = S.fullPivLu().solve(h*P).transpose(); @@ -188,10 +203,10 @@ State::partialUpdate( const MatrixXd &h, const MatrixXd &S, // Get the update Matrix dx; dx = K*y; - assert (dx.rows()==9+3*features.size()); - body->dx(dx.segment<9>(0)); + assert (dx.rows()==STATESIZE+3*features.size()); + body->dx(dx.segment(0)); { - int row=9; + int row=STATESIZE; for (auto i=features.begin(); i!=features.end(); ++i, row+=3) { (*i)->dx(dx.segment<3>(row)); } @@ -207,9 +222,17 @@ State::partialUpdate( const MatrixXd &h, const MatrixXd &S, *-------------------------------------------------------------------------------------- */ void +#if STATESIZE==13 +State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, + const std::vector &z) +#else State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, const std::vector &z, const Quaterniond &q) +#endif { +#if STATESIZE==13 + Quaterniond q = body->qhat(); +#endif MatrixXd K; K = partialUpdate(h,S,z,q); @@ -229,8 +252,15 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, *-------------------------------------------------------------------------------------- */ Matrix +#if STATESIZE==13 +State::innovation( const std::vector &z) +#else State::innovation( const std::vector &z, const Quaterniond &q) +#endif { +#if STATESIZE==13 + Quaterniond q = body->qhat(); +#endif Matrix y; int rows = Hrows(z); y = Matrix::Zero(rows,1); @@ -278,8 +308,15 @@ State::innovation( const std::vector &z, const Quaterniond &q) *-------------------------------------------------------------------------------------- */ void +#if STATESIZE==13 +State::handle_measurements ( const std::vector &z) +#else State::handle_measurements ( const std::vector &z, const Quaterniond &q) +#endif { +#if STATESIZE==13 + Quaterniond q = body->qhat(); +#endif std::vector featuresToAdd; std::vector featuresToUpdate; double zmeas = body->ned()[2]; @@ -337,8 +374,15 @@ State::handle_measurements ( const std::vector &z, const Quaterni } /* ----- end of method State::feature_update ----- */ MatrixXd +#if STATESIZE==13 +State::blockSI ( const std::vector &z) +#else State::blockSI ( const std::vector &z, const Quaterniond &q ) +#endif { +#if STATESIZE==13 + Quaterniond q = body->qhat(); +#endif int rows = Hrows(z); MatrixXd SI; SI = MatrixXd::Zero(rows,rows); @@ -384,11 +428,11 @@ State::blockSI ( const std::vector &z, const Quaterniond &q ) * Description: Return the covariance block for feature with the given id. *-------------------------------------------------------------------------------------- */ -Matrix +Matrix State::Pxy ( int id ) { int col = rowById(id); - return P.block<9,3>(0,col) ; + return P.block(0,col) ; } /* ----- end of method State::Pxy ----- */ Matrix @@ -399,8 +443,14 @@ State::Pyy ( int id ) } /* ----- end of method State::Pyy ----- */ void -State::addFeatures ( std::vector &F, const Quaterniond &q, double z) +#if STATESIZE==13 +#else +State::addFeatures ( std::vector &F, double z) +#endif { +#if STATESIZE==13 + Quaterniond q =body->qhat(); +#endif // Add new features Vector3d pos = body->ned(); for (auto i=F.begin(); i!=F.end(); ++i) { @@ -439,7 +489,7 @@ State::addFeatures ( std::vector &F, const Quaterniond &q, double void State::initializePi ( int i, const Matrix &Pi ) { - int pt=9+3*(i-1); + int pt=STATESIZE+3*(i-1); 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; @@ -458,12 +508,12 @@ MatrixXd State::Q ( double dt ) { MatrixXd q; - int rows = 9 + 3*features.size(); + int rows = STATESIZE + 3*features.size(); q = MatrixXd::Zero(rows,rows); - q.topLeftCorner<9,9>() = body->Q(dt); + q.topLeftCorner() = body->Q(dt); { // limit i's scope auto i = features.begin(); - for (int row=9; row(row,row) = (*i)->Q(dt); } } @@ -478,22 +528,29 @@ State::Q ( double dt ) *-------------------------------------------------------------------------------------- */ MatrixXd +#if STATESIZE==13 +State::F ( const Vector3d &w, double dt ) +#else State::F ( const Quaterniond &q, const Vector3d &w, double dt ) +#endif { +#if STATESIZE==13 + Quaterniond q = body->qhat(); +#endif Vector3d v; v = body->vel(); // Allocate matrix F MatrixXd f; - int rows = 9 + 3*features.size(); + int rows = STATESIZE + 3*features.size(); f = MatrixXd::Zero(rows,rows); // Set body F - f.topLeftCorner<9,9>() = body->F(w,q,dt); + f.topLeftCorner() = body->F(w,q,dt); // Set Fxi Fyi { // limit i's scope auto i = features.begin(); - for (int row=9; row(row,0) = (*i)->Fx(dt); + for (int row=STATESIZE; row(row,0) = (*i)->Fx(dt); f.block<3,3>(row,row) = (*i)->Fy(v,w,dt); } } @@ -509,8 +566,15 @@ State::F ( const Quaterniond &q, const Vector3d &w, double dt ) *-------------------------------------------------------------------------------------- */ void +#if STATESIZE==13 +State::Pkk1 ( const Vector3d &ang, const double dt) +#else State::Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt) +#endif { +#if STATESIZE==13 + Quaterniond q = body->qhat(); +#endif MatrixXd f,fullQ; fullQ = Q(dt); f = F ( q, ang, dt ); @@ -521,9 +585,16 @@ State::Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt) } /* ----- end of method State::Pkk1 ----- */ void +#if STATESIZE==13 +State::motionModel ( const Vector3d &acc, const Vector3d &ang, const double dt) +#else State::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt) +#endif { +#if STATESIZE==13 + Quaterniond q = body->qhat(); +#endif Pkk1(ang,q,dt); body->motionModel(acc,ang,q,dt); @@ -710,7 +781,7 @@ State::exists ( int id ) int State::rowById ( int id ) { - int j=9; + int j=STATESIZE; for (auto i=features.begin(); i!=features.end(); ++i, j+=3) { if ((*i)->id()==id) return j; } @@ -741,10 +812,10 @@ State::expandP ( const Matrix3d &Pi) return ; } /* ----- end of method State::expandP ----- */ -Matrix +Matrix State::Pxx ( ) { - return P.topLeftCorner<9,9>() ; + return P.topLeftCorner() ; } /* ----- end of method State::Pxx ----- */ /* @@ -757,8 +828,8 @@ State::Pxx ( ) void State::shrinkP ( int i ) { - int N = 9 + 3*features.size(); - int I = 9 + 3*i; + int N = STATESIZE + 3*features.size(); + int I = STATESIZE + 3*i; P.block(I,0,N-I-3,I) = P.bottomLeftCorner(N-I-3,I); P.block(0,I,I,N-I-3) = P.topRightCorner(I,N-I-3); @@ -769,8 +840,15 @@ State::shrinkP ( int i ) } /* ----- end of method State::shrinkP ----- */ void +#if STATESIZE==13 +State::ransacUpdate ( std::vector &z ) +#else State::ransacUpdate ( std::vector &z, const Quaterniond &q ) +#endif { +#if STATESIZE==13 + Quaterniond q = body->qhat(); +#endif Matrix mu_old; // x_k|k-1 mu_old = asVector(); // Randomize the measurements @@ -856,10 +934,10 @@ Matrix State::asVector ( ) { Matrix m; - int rows = 9 + 3*features.size(); + int rows = STATESIZE + 3*features.size(); m = Matrix::Zero(rows,1); - m.head<9>() = body->asVector(); - int row = 9; + m.head() = body->asVector(); + int row = STATESIZE; for (auto i=features.begin(); i!=features.end(); ++i) { m.segment<3>(row) = (*i)->asVector(); row += 3; @@ -871,11 +949,11 @@ State::asVector ( ) void State::asVector ( const Matrix &m ) { - int rows = 9 + 3*features.size(); + int rows = STATESIZE + 3*features.size(); assert (rows==m.rows()); - body->asVector(m.head<9>()); + body->asVector(m.head()); - int row = 9; + int row = STATESIZE; for (auto i=features.begin(); i!=features.end(); ++i) { (*i)->asVector(m.segment<3>(row)); row += 3; diff --git a/src/state.h b/src/state.h index a7f7df5..f330b67 100644 --- a/src/state.h +++ b/src/state.h @@ -15,7 +15,7 @@ //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once #define DORANSAC /* */ //#define INLIERTEST /* */ - +#define STATESIZE 13 /* Set to 13 for quaternion estimation, or 9 for quaternion as input */ using Eigen::Dynamic; using Eigen::Matrix; using Eigen::MatrixXd; @@ -44,14 +44,20 @@ class State int rowById(int id); int iById(int id); Feature *featureById(int id); +#if STATESIZE==13 + MatrixXd F(const Vector3d &w, double dt); + MatrixXd H ( const std::vector &z ); + MatrixXd blockSI( const std::vector &z ); +#else MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt); MatrixXd H ( const Quaterniond &q, const std::vector &z ); + MatrixXd blockSI( const std::vector &z, const Quaterniond &q); +#endif int Hrows( const std::vector &z ); MatrixXd Q(double dt); MatrixXd R( const std::vector &z ); - MatrixXd blockSI( const std::vector &z, const Quaterniond &q); - Matrix Pxx(); - Matrix Pxy(int id); + Matrix Pxx(); + Matrix Pxy(int id); Matrix Pyy(int id); Matrix L(); @@ -59,16 +65,27 @@ class State void accelerometer_bias(const Vector3d &b); void asVector(const Matrix &m); void pos(const UTM &utm); +#if STATESIZE==13 + void handle_measurements( const std::vector & z); + void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, + const std::vector &z); + void motionModel(const Vector3d &acc, const Vector3d &ang, const double dt); + MatrixXd partialUpdate( const MatrixXd &h, const MatrixXd &S, + const std::vector &z); + void Pkk1 ( const Vector3d &ang, const double dt); + void ransacUpdate(std::vector &z); +#else void handle_measurements( const std::vector & z, const Quaterniond &q); - void initializePi(int i, const Matrix &Pi); void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, const std::vector &z, const Quaterniond &q); - void ransacUpdate(std::vector &z, const Quaterniond &q); void motionModel(const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt); MatrixXd partialUpdate( const MatrixXd &h, const MatrixXd &S, const std::vector &z, const Quaterniond &q ); void Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt); + void ransacUpdate(std::vector &z, const Quaterniond &q); +#endif + void initializePi(int i, const Matrix &Pi); void position_covariance(const Matrix &p); std::list::iterator removeFeature(std::list::iterator &i, bool bl); std::list::iterator removeFeature ( int id, bool bl ); @@ -76,7 +93,11 @@ class State void vel(const Matrix &v); /* ==================== OPERATORS ======================================= */ +#if STATESIZE==13 + Matrix innovation( const std::vector &z); +#else Matrix innovation( const std::vector &z, const Quaterniond &q); +#endif void unicsv(); protected: @@ -86,7 +107,11 @@ class State private: /* ==================== METHODS ======================================= */ +#if STATESIZE==13 + void addFeatures(std::vector &F, double z); +#else void addFeatures(std::vector &F, const Quaterniond &q, double z); +#endif void expandP ( const Matrix3d &Pi); void shrinkP( int i ); -- cgit v1.1