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/feature.cpp | 16 ++++++++++++++--
 1 file changed, 14 insertions(+), 2 deletions(-)

(limited to 'src/feature.cpp')

diff --git a/src/feature.cpp b/src/feature.cpp
index 22bf776..66b0677 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -26,7 +26,7 @@
  *--------------------------------------------------------------------------------------
  */
 Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xr, 
-        const Vector3d &xbw, const Quaterniond &q, double z)
+        const Vector3d &xbw, const Quaterniond &q, double z, Mat &p)
 {
     _id = id;
     Vector3d xib;
@@ -35,6 +35,7 @@ Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xr,
     X0 = X;
     xb0w = xbw;
     q0 = q;
+    _patch = p;
 }  /* -----  end of method Feature::Feature  (constructor)  ----- */
 
 /*
@@ -45,7 +46,7 @@ Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xr,
  *--------------------------------------------------------------------------------------
  */
 Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xbw,
-        const Quaterniond &q)
+        const Quaterniond &q, Mat &p)
 {
     _id = id;
     Vector3d xib, pib;
@@ -59,6 +60,7 @@ Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xbw,
     X0 = X;
     xb0w = xbw;
     q0 = q;
+    _patch = p;
 }
 
 
@@ -953,11 +955,15 @@ Feature::P0 ( measurement_type t )
     double p0 = FEATURECOVX;
     double p1 = FEATURECOVY;
     double p2; 
+    if (t==REFLECTION) {
 #ifdef INITDEPTH
         p2 = FEATURECOVRHO; 
 #else
         p2 = FEATURECOVRHO_MONO;
 #endif
+    } else {
+        p2 = FEATURECOVRHO_MONO;
+    }
     P << p0, 0., 0., 
          0., p1, 0.,
          0., 0., p2;
@@ -1107,3 +1113,9 @@ Feature::asVector ( const Vector3d &m )
     return ;
 }		/* -----  end of method Feature::asVector  ----- */
 
+Mat
+Feature::patch ( )
+{
+    return _patch ;
+}		/* -----  end of method Feature::patch  ----- */
+
-- 
cgit v1.1