Move feature depth calculation to Feature class.

And also fixed F computation for features.

I realized that the features were transformed from image to body frame
prior to being passed to State. This makes sense because the State
really doesn't need to know about Camera objects. Since the camera was
no longer necessary inside of State it made sense to move the depth
computation from Camera::ref2body() into a method in the Feature class
that does not rely on camera parameters.

Additionally, the depth solver was changed from a simple matrix
inversion to a least squares calculation.

Fx in the feature class was fixed to take into account dt.

Updating Pkk1 is in the process of being modified to handle features.

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  ----- */