From ca7631250c87802768834b908b9fe9fc871e802a Mon Sep 17 00:00:00 2001
From: Francesco Gatti <gattifrancesco@hotmail.it>
Date: Sun, 3 May 2020 19:12:10 +0200
Subject: [PATCH] filter yaw ok

---
 include/tkDNN/ImuOdom.h   | 3 ++-
 tests/imuodom/imuodom.cpp | 6 +++++-
 2 files changed, 7 insertions(+), 2 deletions(-)

diff --git a/include/tkDNN/ImuOdom.h b/include/tkDNN/ImuOdom.h
index 8d21951..58def96 100644
--- a/include/tkDNN/ImuOdom.h
+++ b/include/tkDNN/ImuOdom.h
@@ -137,7 +137,8 @@ class ImuOdom {
             q.x() = deltaQ(1);
             q.y() = deltaQ(2);
             q.z() = deltaQ(3);    
-            odomPOS = odomPOS + deltaP.cast<double>();
+            odomPOS = odomPOS + odomROT*deltaP.cast<double>(); // V1
+            //odomPOS = odomPOS + deltaP.cast<double>(); // V2
             odomROT = odomROT * q.normalized().toRotationMatrix();
             
             // compute euler
diff --git a/tests/imuodom/imuodom.cpp b/tests/imuodom/imuodom.cpp
index bcf131e..24be585 100644
--- a/tests/imuodom/imuodom.cpp
+++ b/tests/imuodom/imuodom.cpp
@@ -9,7 +9,11 @@ const char *o1_bin   = "imuodom/layers/output1.bin";
 
 int main() {
 
-    downloadWeightsifDoNotExist(i0_bin, "imuodom", "https://cloud.hipert.unimore.it/s/BBSEbEbQbPKxp4s/download");
+    // V1 
+    downloadWeightsifDoNotExist(i0_bin, "imuodom", "https://cloud.hipert.unimore.it/s/ZAy34K5w2ixED6x/download");
+    
+    // V2
+    //downloadWeightsifDoNotExist(i0_bin, "imuodom", "https://cloud.hipert.unimore.it/s/BBSEbEbQbPKxp4s/download");
     
     tk::dnn::ImuOdom ImuNet;
     ImuNet.init("imuodom/layers/");