From e5c3b5117c2f3936967319223e353ac6c7a0b1ca Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Sun, 9 Apr 2017 13:25:13 -0500
Subject: warp source and reflection patches

---
 src/state.cpp | 48 ++++++++++++++++++++++++++++++++++++++++--------
 1 file changed, 40 insertions(+), 8 deletions(-)

(limited to 'src/state.cpp')

diff --git a/src/state.cpp b/src/state.cpp
index d43be62..5a334c5 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -1049,9 +1049,9 @@ State::quaternion_covariance ( const Matrix<double,4,4> &covq )
  */
 void
 #if STATESIZE==13
-State::featuresAsMeasurements ( vector<measurement_t> &yk )
+State::featuresAsMeasurements ( vector<measurement_t> &yk, const Camera &cam )
 #else
-State::featuresAsMeasurements ( vector<measurement_t> &yk,
+State::featuresAsMeasurements ( vector<measurement_t> &yk, const Camera &cam,
         const Quaterniond &q)
 #endif
 {
@@ -1071,8 +1071,9 @@ State::featuresAsMeasurements ( vector<measurement_t> &yk,
         z.id = (*i)->id();
         z.source = (*i)->p2x(pbs);
         z.reflection = (*i)->p2x(pbr);
-        z.z_type = REFLECTION;
-        z.patch = (*i)->patch();
+        z.z_type = MONO;
+        z.patch = (*i)->warpedPatch(cam, q);
+        z.refpatch = (*i)->reflectedPatch(cam, q);
 
         // Get ellipse
         Matrix<double,6,6> s;
@@ -1101,13 +1102,35 @@ State::doMeasurements ( vector<measurement_t> &z, Vision &viz,
         viz.acquireFeatures(cam, newFeatures);
     }
     // Measure existing features
-    vector<measurement_t> currentFeatures, featureZ;
+    vector<measurement_t> currentFeatures;
 #if STATESIZE==13
-    featuresAsMeasurements(currentFeatures);
+    featuresAsMeasurements(currentFeatures,cam);
 #else
-    featuresAsMeasurements(currentFeatures,q);
+    featuresAsMeasurements(currentFeatures,cam,q);
+#endif
+    viz.measurements(cam, currentFeatures, z);
+
+#ifdef DORANSAC
+#if STATESIZE==13
+    ransacUpdate(z);
+#else
+    ransacUpdate(z,q);
+#endif
+#else      /* -----  not DORANSAC  ----- */
+        MatrixXd h;
+#if STATESIZE==13
+        h = H(z);
+#else
+        h = H(q,z);
+#endif
+        MatrixXd S;
+        S = h*P*h.transpose() + R(z);
+#if STATESIZE==13
+        kalmanUpdate(h,S,z);
+#else
+        kalmanUpdate(h,S,z,q);
+#endif
 #endif
-    viz.measurements(cam, currentFeatures, featureZ);
 
     // Initialize new features
     if (features.size()<MINFEATURES) {
@@ -1117,6 +1140,15 @@ State::doMeasurements ( vector<measurement_t> &z, Vision &viz,
         addFeatures(newFeatures, q, hgtmeas);
 #endif
     }
+    vector<measurement_t> updatedFeatures;
+#if STATESIZE==13
+    featuresAsMeasurements(updatedFeatures, cam);
+#else
+    featuresAsMeasurements(updatedFeatures, cam, q);
+#endif
+    viz.drawMeasurements(z, cam, cv::Scalar(0,0,100), cv::Scalar(0,0,0), false);
+    viz.drawMeasurements(updatedFeatures, cam, cv::Scalar(0,255,0), cv::Scalar(0,0,0), true);
+    viz.show();
 
     return ;
 }		/* -----  end of method State::doMeasurements  ----- */
-- 
cgit v1.1