Fix Jacobian of motion model.

This fixes a bug that was present in the pyslam code. The Jacobian of the motion
model needs to be replaced with:
F = I + F*dt

1 files changed, 3 insertions, 1 deletions

diff --git a/src/feature.cpp b/src/feature.cpp
index 99ea448..2afd5a0 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -225,7 +225,7 @@ Feature::Fx ( )
  *--------------------------------------------------------------------------------------
  */
 Matrix<double,3,3>
-Feature::Fy ( const Vector3d &v, const Vector3d &w )
+Feature::Fy ( const Vector3d &v, const Vector3d &w, double dt )
 {
     double y0,y1,y2;
     double v0,v1,v2;
@@ -243,6 +243,8 @@ Feature::Fy ( const Vector3d &v, const Vector3d &w )
     F << v0*y2-2*w2*y0+y1*w1, w0+y0*w1, -v1+y0*v0,
          w0-y1*w2, v0*y2+2*w1*y1-y0*w2, -v2+y1*v0,
          -w2*y2, w1*y2, -w2*y0+w1*y1+2*v0*y2;
+    F *= dt;
+    F += Matrix<double,3,3>::Identity();
     return F;
 }		/* -----  end of method Feature::Fy  ----- */