From cbc3a7a308d1edde429022a9492eeedce24c5fd9 Mon Sep 17 00:00:00 2001 From: Martin Miller Date: Sat, 8 Apr 2017 19:15:25 -0500 Subject: Add code to perform a guided measurement. The Vision class can template match in a search region. The search region result is masked by an ellipse related to the S matrix of the feature. --- src/state.cpp | 98 +++++++++++++++++++++++++++++++++++++++++------------------ 1 file changed, 69 insertions(+), 29 deletions(-) (limited to 'src/state.cpp') 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 &z ) +State::H ( const vector &z ) #else -State::H ( const Quaterniond &q, const std::vector &z ) +State::H ( const Quaterniond &q, const vector &z ) #endif { #if STATESIZE==13 @@ -114,7 +114,7 @@ State::H ( const Quaterniond &q, const std::vector &z ) *-------------------------------------------------------------------------------------- */ int -State::Hrows ( const std::vector &z) +State::Hrows ( const vector &z) { int rows = 0; for (auto i=z.begin(); i!=z.end(); ++i) { @@ -146,7 +146,7 @@ State::Hrows ( const std::vector &z) *-------------------------------------------------------------------------------------- */ MatrixXd -State::R ( const std::vector &z ) +State::R ( const vector &z ) { int rows = Hrows(z); MatrixXd r; @@ -183,10 +183,10 @@ State::R ( const std::vector &z ) MatrixXd #if STATESIZE==13 State::partialUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector &z) + const vector &z) #else State::partialUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector &z, const Quaterniond &q ) + const vector &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 &z) + const vector &z) #else State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector &z, const Quaterniond &q) + const vector &z, const Quaterniond &q) #endif { MatrixXd K; @@ -259,9 +259,9 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, */ Matrix #if STATESIZE==13 -State::innovation( const std::vector &z) +State::innovation( const vector &z) #else -State::innovation( const std::vector &z, const Quaterniond &q) +State::innovation( const vector &z, const Quaterniond &q) #endif { #if STATESIZE==13 @@ -315,13 +315,13 @@ State::innovation( const std::vector &z, const Quaterniond &q) */ void #if STATESIZE==13 -State::handle_measurements ( const std::vector &z) +State::handle_measurements ( const vector &z) #else -State::handle_measurements ( const std::vector &z, const Quaterniond &q) +State::handle_measurements ( const vector &z, const Quaterniond &q) #endif { - std::vector featuresToAdd; - std::vector featuresToUpdate; + vector featuresToAdd; + vector 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 &z, const Quaterni MatrixXd #if STATESIZE==13 -State::blockSI ( const std::vector &z) +State::blockSI ( const vector &z) #else -State::blockSI ( const std::vector &z, const Quaterniond &q ) +State::blockSI ( const vector &z, const Quaterniond &q ) #endif { #if STATESIZE==13 @@ -466,9 +466,9 @@ State::Pyy ( int id ) void #if STATESIZE==13 -State::addFeatures ( std::vector &F, double z) +State::addFeatures ( vector &F, double z) #else -State::addFeatures(std::vector &F, const Quaterniond &q, double z) +State::addFeatures(vector &F, const Quaterniond &q, double z) #endif { #if STATESIZE==13 @@ -483,12 +483,15 @@ State::addFeatures(std::vector &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 &z ) +State::ransacUpdate ( vector &z ) #else -State::ransacUpdate ( std::vector &z, const Quaterniond &q ) +State::ransacUpdate ( vector &z, const Quaterniond &q ) #endif { #if STATESIZE==13 @@ -883,10 +886,10 @@ State::ransacUpdate ( std::vector &z, const Quaterniond &q ) int N0 = z.size(); int N = z.size(); - std::vector inliers; + vector inliers; for (int i=0; i Z; + vector Z; Z.push_back(zi); MatrixXd h; #if STATESIZE==13 @@ -941,7 +944,7 @@ State::ransacUpdate ( std::vector &z, const Quaterniond &q ) #endif // High innovation update - std::vector hi_inliers; + vector 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 &covq ) */ void #if STATESIZE==13 -State::featuresAsMeasurements ( std::vector &yk ) +State::featuresAsMeasurements ( vector &yk ) #else -State::featuresAsMeasurements ( std::vector &yk, +State::featuresAsMeasurements ( vector &yk, const Quaterniond &q) #endif { @@ -1065,10 +1068,11 @@ State::featuresAsMeasurements ( std::vector &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 s; @@ -1081,3 +1085,39 @@ State::featuresAsMeasurements ( std::vector &yk, return ; } /* ----- end of method State::featuresAsMeasurements ----- */ +void +#if STATESIZE==13 +State::doMeasurements ( vector &z, Vision &viz, Camera &cam ) +#else +State::doMeasurements ( vector &z, Vision &viz, + const Quaterniond &q, Camera &cam) +#endif +{ + measurement_t hgt = z[0]; + assert (hgt.z_type==HEIGHT); + double hgtmeas = hgt.height; + vector newFeatures; + if (features.size() currentFeatures, featureZ; +#if STATESIZE==13 + featuresAsMeasurements(currentFeatures); +#else + featuresAsMeasurements(currentFeatures,q); +#endif + viz.measurements(cam, currentFeatures, featureZ); + + // Initialize new features + if (features.size()