flop

3 files changed, 44 insertions, 2 deletions

diff --git a/src/feature.cpp b/src/feature.cpp
index 2c5dbb0..4db6a29 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -65,6 +65,19 @@ Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xbw,
     lastseen = 0;
 }
 
+void
+Feature::reinitialize(const Vector3d &xs, const Vector3d& xr,
+        const Vector3d &xbw, const Quaterniond &q, Mat &p)
+{
+    Vector3d xib;
+    xib = findDepth(q,xbw[2],xs,xr);
+    X = x2p(xib);
+    X0 = X;
+    xb0w = xbw;
+    q0 = q;
+    _patch = p;
+    lastseen = 0;
+}
 
 /*
  *--------------------------------------------------------------------------------------
@@ -956,7 +969,7 @@ Feature::P0 ( measurement_type t )
     double p0 = FEATURECOVX;
     double p1 = FEATURECOVY;
     double p2; 
-    if (t==REFLECTION) {
+    if (t==BOTH) {
 #ifdef INITDEPTH
         p2 = FEATURECOVRHO; 
 #else
diff --git a/src/feature.h b/src/feature.h
index d582250..eade6e7 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -63,6 +63,8 @@ class Feature
         void dx( const Vector3d &del);
         void seen();
         bool since(int N) const;
+        void reinitialize(const Vector3d &xs, const Vector3d& xr,
+                const Vector3d &xbw, const Quaterniond &q, Mat &p);
 
         /* ====================  OPERATORS     ======================================= */
         bool sane();
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