diff options
Diffstat (limited to 'src/state.cpp')
| -rw-r--r-- | src/state.cpp | 29 |
1 files changed, 28 insertions, 1 deletions
diff --git a/src/state.cpp b/src/state.cpp index b521eff..80cfc59 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -548,7 +548,7 @@ State::addFeatures(vector<measurement_t> &F, const Quaterniond &q, double z) void State::initializePi ( int i, const Matrix<double,3,3> &Pi ) { - int pt=STATESIZE+3*(i-1); + int pt=STATESIZE+3*(i); 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; @@ -1146,6 +1146,33 @@ State::doMeasurements ( vector<measurement_t> &z, Vision &viz, viz.acquireFeatures(cam, newFeatures,currentFeatures); } viz.measurements(cam, currentFeatures, z); + // Check for flipped correspondence pairs and flop them + for (auto i=z.begin(); i!=z.end(); ++i) { + if (i->z_type==BOTH && i->source[2]>i->reflection[2]) { + // Flop the measurements + Vector3d tmp; + tmp = i->source; + i->source = i->reflection; + i->reflection = tmp; + + // Get a new patch + Vector3d pi; + cv::Mat patch; + pi = cam.body2img(i->source); + viz.getTemplate(patch, pi); + + // Reinitialize feature +#if STATESIZE==13 + Quaterniond q = body->qhat(); +#endif + Feature *ft = featureById(i->id); + ft->reinitialize(i->source,i->reflection,body->ned(),q,patch); + + // reinitialize the covariance + int j = iById(i->id); + initializePi(j,ft->P0(BOTH)); + } + } #ifdef DORANSAC #if STATESIZE==13 |