2 files changed, 70 insertions, 2 deletions

diff --git a/src/feature.cpp b/src/feature.cpp
index ad554a1..45e24e7 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -19,12 +19,78 @@
 #include "feature.h"
 
 Matrix<double,6,1>
-Feature::h ( )
+Feature::h ( const Vector3d &x, const Quaterniond &q )
 {
     Matrix<double,6,1> h;
+    Vector3d xib, xib0;
+    Vector3d pib0;
+
+    xib = p2x(X);
+
+    // Predict initial view
+    xib0 = q0._transformVector(q._transformVector(xib) + x - xb0w);
+    pib0 = x2p(xib0);
+
+    // Predict reflection view
+    Vector3d xpb, ppb;
+    Quaterniond qxpb, qrb;
+    const Quaterniond qrw(0,0,0,-1); // the reflecting plane in world frame
+    Quaterniond qxib(0,xib[0],xib[1],xib[2]);
+
+    // Compute reflection plane in body frame
+    qrb = q.conjugate()*qrw;
+    // Reflect xib about qrb
+    qxpb = qrb*qxib*qrb;
+    xpb = qxpb.vec();
+    ppb = x2p(xpb);
+
+    h[0] = X[0];
+    h[1] = X[1];
+    h[2] = pib0[0];
+    h[3] = pib0[1];
+    h[4] = ppb[0];
+    h[5] = ppb[1];
+
     return h;
 }		/* -----  end of method Feature::h  ----- */
 
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Feature
+ *      Method:  Feature :: p2x
+ * Description:  Returns x, the point p in inverse depth frame in the Cartesian
+ * frame.
+ *--------------------------------------------------------------------------------------
+ */
+Vector3d
+Feature::p2x ( const Vector3d &p )
+{
+    Vector3d x;
+    x[0] = 1./p[2];
+    x[1] = p[0]/p[2];
+    x[2] = p[1]/p[2];
+    return x;
+}		/* -----  end of method Feature::p2x  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Feature
+ *      Method:  Feature :: x2p
+ * Description:  Returns p, the point x in the Cartesian body frame in the
+ * inverse depth frame.
+ *--------------------------------------------------------------------------------------
+ */
+Vector3d
+Feature::x2p ( const Vector3d &x )
+{
+    Vector3d p;
+    p[0] = x[1]/x[0];
+    p[1] = x[2]/x[0];
+    p[2] = 1./x[0];
+    return p;
+}		/* -----  end of method Feature::x2p  ----- */
+
+
 
 Matrix<double,3,9>
 Feature::Fx ( )
diff --git a/src/feature.h b/src/feature.h
index 077a617..2982276 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -28,7 +28,7 @@ class Feature
         Matrix<double,3,3> Fy( const Vector3d &vel, const Vector3d &ang);
         Matrix<double,6,9> Hx( const Vector3d &pos, const Quaterniond &q);
         Matrix<double,6,3> Hy( const Vector3d &pos, const Quaterniond &q);
-        Matrix<double,6,1> h();
+        Matrix<double,6,1> h( const Vector3d &x, const Quaterniond &q);
         /*
         void motionModel ( const Vector3d &acc, const Vector3d &ang, 
                 const Quaterniond &q, const double dt);
@@ -46,6 +46,8 @@ class Feature
 
     private:
         /* ====================  METHODS       ======================================= */
+        Vector3d p2x(const Vector3d &p);
+        Vector3d x2p(const Vector3d &x);
 
         /* ====================  DATA MEMBERS  ======================================= */
         Vector3d X;