update

1 files changed, 13 insertions, 6 deletions

diff --git a/src/feature.cpp b/src/feature.cpp
index 5963ae5..ffce145 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -26,13 +26,20 @@
  *--------------------------------------------------------------------------------------
  */
 Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xr, 
-        const Vector3d &xbw, const Quaterniond &q)
+        const Vector3d &xbw, const Quaterniond &q, double z)
 {
-    double z = xbw[2];
     _id = id;
     Vector3d xib;
     xib = findDepth(q,z,xs,xr);
     X = x2p(xib);
+    /*
+    Vector3d pib;
+    xib = xs/xs[0];
+    pib[0] = xib[1];
+    pib[1] = xib[2];
+    pib[2] = .1;
+    X = pib;
+    */
     X0 = X;
     xb0w = xbw;
     q0 = q;
@@ -125,7 +132,7 @@ Feature::Q ( const double dt )
 {
     Matrix<double,3,3> Q;
     Q = Matrix<double,3,3>::Identity();
-    Q *= dt*dt*1e-3;
+    Q *= dt*dt;//*1e-1;
     return Q;
 }		/* -----  end of method Feature::q  ----- */
 
@@ -134,7 +141,7 @@ Feature::R (  )
 {
     Matrix<double,6,6> R;
     R = Matrix<double,6,6>::Identity();
-    R *= 1e-3;
+    R *= 1e-4;
     return R;
 }		/* -----  end of method Feature::R  ----- */
 
@@ -485,8 +492,8 @@ Matrix<double,3,3>
 Feature::P0 ( )
 {
     Matrix<double,3,3> P;
-    double p0 = 1e-2;
-    double p1 = 1e-1;
+    double p0 = 1e-4;
+    double p1 = 1e-4;
     P << p0, 0., 0., 
          0., p0, 0.,
          0., 0., p1;