1 files changed, 67 insertions, 5 deletions

diff --git a/src/feature.cpp b/src/feature.cpp
index fd4a85f..aa0490e 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -25,18 +25,79 @@
  * Description:  constructor
  *--------------------------------------------------------------------------------------
  */
-Feature::Feature ( int id, const Vector3d &xib, const Vector3d &xpb,
-        const Vector3d &xbw, const Quaterniond &q )  /* constructor */
+Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xr, 
+        const Vector3d &xbw, const Quaterniond &q, double z )                             /* constructor */
 {
     _id = id;
+    Vector3d xib;
+    xib = findDepth(q,z,xs,xr);
     X = x2p(xib);
-    X[2] = 1./20.;
     xb0w = xbw;
     q0 = q;
 }  /* -----  end of method Feature::Feature  (constructor)  ----- */
 
-Feature::Feature () {;}
 
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Feature
+ *      Method:  Feature :: findDepth
+ * Description:  Compute the feature depth given the camera pose and the source
+ * and reflection measurements. The measurements xs and xr should be given in
+ * the body frame (i.e. transformed from the image frame into the camera frame
+ * according to K and rotated from the camera frame into the body frame with
+ * Rcb. The feature is then returned in body coordinates.
+ *--------------------------------------------------------------------------------------
+ */
+Vector3d
+Feature::findDepth ( const Quaterniond &qbw, double z, const Vector3d &xs,
+        const Vector3d &xr )
+{
+    // The output vector
+    Vector3d xb;
+    Vector4d xb4;
+
+    // Projection matrices
+    Matrix<double,4,4> P1,P2;
+    P1 = Matrix<double,4,4>::Identity();
+
+    // Rotation from body to world
+    Matrix<double,3,3> Rbw;
+    Matrix<double,4,4> Rbw4,Rwb4;
+    Rbw = qbw.toRotationMatrix();
+    Rbw4 = Matrix<double,4,4>::Identity();
+    Rbw4.block<3,3>(0,0) = Rbw;
+    Rwb4 = Rbw4.transpose();
+
+    Matrix<double,4,4> J; // Reflection matrix
+    J = Matrix<double,4,4>::Identity();
+    J(2,2) = -1;
+    J(2,3) = -2*z;
+
+    P2 = Rwb4*J*Rbw4;
+
+    Matrix<double,6,4> P;
+    P.block<3,4>(0,0) = P1.block<3,4>(0,0);
+    P.block<3,4>(3,0) = P2.block<3,4>(0,0);
+
+    //Vector4d b;
+    Matrix<double,6,1> b;
+    b.segment<3>(0) = xs;
+    //b[3] = 1;
+    //b.segment<3>(4) = xr;
+    //b[7] = 1;
+    b[3] = xr[0];
+    b[4] = xr[1];
+    b[5] = xr[2];
+
+
+    xb4 = P.colPivHouseholderQr().solve(b);
+    //xb4 = P.inverse()*b;
+    xb4 /= xb4[3];
+    xb = xb4.segment<3>(0);
+    cout << xb.transpose() << endl;
+
+    return xb ;
+}		/* -----  end of method Feature::findDepth  ----- */
 
 /*
  *--------------------------------------------------------------------------------------
@@ -208,7 +269,7 @@ Feature::x2p ( const Vector3d &x )
  *--------------------------------------------------------------------------------------
  */
 Matrix<double,3,9>
-Feature::Fx ( )
+Feature::Fx ( double dt )
 {
     double y0,y1,y2;
     y0 = X[0];
@@ -218,6 +279,7 @@ Feature::Fx ( )
     F << 0., 0., 0., y0*y2,-y2, 0.,0,0,0,
          0.,0.,0.,   y1*y2, 0.,-y2, 0,0,0,
          0,0,0,   y2*y2,    0,   0, 0,0,0;
+    F *= dt;
     return F;
 }		/* -----  end of method Feature::Fx  ----- */