From 0f25fa35e063d3df3c652153b2d6f1b9f94ce82e Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Mon, 20 Mar 2017 11:57:09 -0500
Subject: 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
---
 src/feature.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

(limited to 'src/feature.cpp')

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  ----- */
 
-- 
cgit v1.1