diff options
Diffstat (limited to 'src/state.cpp')
| -rw-r--r-- | src/state.cpp | 98 |
1 files changed, 69 insertions, 29 deletions
diff --git a/src/state.cpp b/src/state.cpp index 8a31535..d43be62 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -50,9 +50,9 @@ State::pos ( const UTM &utm ) */ MatrixXd #if STATESIZE==13 -State::H ( const std::vector<measurement_t> &z ) +State::H ( const vector<measurement_t> &z ) #else -State::H ( const Quaterniond &q, const std::vector<measurement_t> &z ) +State::H ( const Quaterniond &q, const vector<measurement_t> &z ) #endif { #if STATESIZE==13 @@ -114,7 +114,7 @@ State::H ( const Quaterniond &q, const std::vector<measurement_t> &z ) *-------------------------------------------------------------------------------------- */ int -State::Hrows ( const std::vector<measurement_t> &z) +State::Hrows ( const vector<measurement_t> &z) { int rows = 0; for (auto i=z.begin(); i!=z.end(); ++i) { @@ -146,7 +146,7 @@ State::Hrows ( const std::vector<measurement_t> &z) *-------------------------------------------------------------------------------------- */ MatrixXd -State::R ( const std::vector<measurement_t> &z ) +State::R ( const vector<measurement_t> &z ) { int rows = Hrows(z); MatrixXd r; @@ -183,10 +183,10 @@ State::R ( const std::vector<measurement_t> &z ) MatrixXd #if STATESIZE==13 State::partialUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector<measurement_t> &z) + const vector<measurement_t> &z) #else State::partialUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector<measurement_t> &z, const Quaterniond &q ) + const vector<measurement_t> &z, const Quaterniond &q ) #endif { MatrixXd K; @@ -229,10 +229,10 @@ State::partialUpdate( const MatrixXd &h, const MatrixXd &S, void #if STATESIZE==13 State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector<measurement_t> &z) + const vector<measurement_t> &z) #else State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector<measurement_t> &z, const Quaterniond &q) + const vector<measurement_t> &z, const Quaterniond &q) #endif { MatrixXd K; @@ -259,9 +259,9 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, */ Matrix<double,Dynamic,1> #if STATESIZE==13 -State::innovation( const std::vector<measurement_t> &z) +State::innovation( const vector<measurement_t> &z) #else -State::innovation( const std::vector<measurement_t> &z, const Quaterniond &q) +State::innovation( const vector<measurement_t> &z, const Quaterniond &q) #endif { #if STATESIZE==13 @@ -315,13 +315,13 @@ State::innovation( const std::vector<measurement_t> &z, const Quaterniond &q) */ void #if STATESIZE==13 -State::handle_measurements ( const std::vector<measurement_t> &z) +State::handle_measurements ( const vector<measurement_t> &z) #else -State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterniond &q) +State::handle_measurements ( const vector<measurement_t> &z, const Quaterniond &q) #endif { - std::vector<measurement_t> featuresToAdd; - std::vector<measurement_t> featuresToUpdate; + vector<measurement_t> featuresToAdd; + vector<measurement_t> featuresToUpdate; double zmeas = body->ned()[2]; for (auto i=z.begin(); i!=z.end(); ++i) { if (i->z_type==HEIGHT) { @@ -397,9 +397,9 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni MatrixXd #if STATESIZE==13 -State::blockSI ( const std::vector<measurement_t> &z) +State::blockSI ( const vector<measurement_t> &z) #else -State::blockSI ( const std::vector<measurement_t> &z, const Quaterniond &q ) +State::blockSI ( const vector<measurement_t> &z, const Quaterniond &q ) #endif { #if STATESIZE==13 @@ -466,9 +466,9 @@ State::Pyy ( int id ) void #if STATESIZE==13 -State::addFeatures ( std::vector<measurement_t> &F, double z) +State::addFeatures ( vector<measurement_t> &F, double z) #else -State::addFeatures(std::vector<measurement_t> &F, const Quaterniond &q, double z) +State::addFeatures(vector<measurement_t> &F, const Quaterniond &q, double z) #endif { #if STATESIZE==13 @@ -483,12 +483,15 @@ State::addFeatures(std::vector<measurement_t> &F, const Quaterniond &q, double z Feature *f; if (i->z_type==REFLECTION) { #ifdef INITDEPTH - f = new Feature(i->id, i->source, i->reflection, pos, q, z); + f = new Feature(i->id, i->source, i->reflection, pos, q, z, i->patch); #else /* ----- not INITDEPTH ----- */ - f = new Feature(i->id, i->source, pos, q); + f = new Feature(i->id, i->source, pos, q, i->patch); #endif /* ----- not INITDEPTH ----- */ + } else if (i->z_type==MONO) { + f = new Feature(i->id, i->source, pos, q, i->patch); } else { - f = new Feature(i->id, i->source, pos, q); + fprintf(stderr, "Measurement type: %d not known, quitting\n", i->z_type); + exit(1); } if (!f->sane()) { delete f; @@ -868,9 +871,9 @@ State::shrinkP ( int i ) void #if STATESIZE==13 -State::ransacUpdate ( std::vector<measurement_t> &z ) +State::ransacUpdate ( vector<measurement_t> &z ) #else -State::ransacUpdate ( std::vector<measurement_t> &z, const Quaterniond &q ) +State::ransacUpdate ( vector<measurement_t> &z, const Quaterniond &q ) #endif { #if STATESIZE==13 @@ -883,10 +886,10 @@ State::ransacUpdate ( std::vector<measurement_t> &z, const Quaterniond &q ) int N0 = z.size(); int N = z.size(); - std::vector<measurement_t> inliers; + vector<measurement_t> inliers; for (int i=0; i<N && i<N0; ++i) { measurement_t zi = z[i]; - std::vector<measurement_t> Z; + vector<measurement_t> Z; Z.push_back(zi); MatrixXd h; #if STATESIZE==13 @@ -941,7 +944,7 @@ State::ransacUpdate ( std::vector<measurement_t> &z, const Quaterniond &q ) #endif // High innovation update - std::vector<measurement_t> hi_inliers; + vector<measurement_t> hi_inliers; for (auto i=z.begin(); i!=z.end(); ++i) { bool found = false; for (auto j=inliers.begin(); j!=inliers.end(); ++j) { @@ -1046,9 +1049,9 @@ State::quaternion_covariance ( const Matrix<double,4,4> &covq ) */ void #if STATESIZE==13 -State::featuresAsMeasurements ( std::vector<measurement_t> &yk ) +State::featuresAsMeasurements ( vector<measurement_t> &yk ) #else -State::featuresAsMeasurements ( std::vector<measurement_t> &yk, +State::featuresAsMeasurements ( vector<measurement_t> &yk, const Quaterniond &q) #endif { @@ -1065,10 +1068,11 @@ State::featuresAsMeasurements ( std::vector<measurement_t> &yk, pbr << h(4), h(5), 1; measurement_t z; - z.id = 0; + z.id = (*i)->id(); z.source = (*i)->p2x(pbs); z.reflection = (*i)->p2x(pbr); z.z_type = REFLECTION; + z.patch = (*i)->patch(); // Get ellipse Matrix<double,6,6> s; @@ -1081,3 +1085,39 @@ State::featuresAsMeasurements ( std::vector<measurement_t> &yk, return ; } /* ----- end of method State::featuresAsMeasurements ----- */ +void +#if STATESIZE==13 +State::doMeasurements ( vector<measurement_t> &z, Vision &viz, Camera &cam ) +#else +State::doMeasurements ( vector<measurement_t> &z, Vision &viz, + const Quaterniond &q, Camera &cam) +#endif +{ + measurement_t hgt = z[0]; + assert (hgt.z_type==HEIGHT); + double hgtmeas = hgt.height; + vector<measurement_t> newFeatures; + if (features.size()<MINFEATURES) { + viz.acquireFeatures(cam, newFeatures); + } + // Measure existing features + vector<measurement_t> currentFeatures, featureZ; +#if STATESIZE==13 + featuresAsMeasurements(currentFeatures); +#else + featuresAsMeasurements(currentFeatures,q); +#endif + viz.measurements(cam, currentFeatures, featureZ); + + // Initialize new features + if (features.size()<MINFEATURES) { +#if STATESIZE==13 + addFeatures(newFeatures, hgtmeas); +#else + addFeatures(newFeatures, q, hgtmeas); +#endif + } + + return ; +} /* ----- end of method State::doMeasurements ----- */ + |