diff options
Diffstat (limited to 'src/state.cpp')
| -rw-r--r-- | src/state.cpp | 84 |
1 files changed, 70 insertions, 14 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 ----- */ + |