From 2d378e90706e2bb1bef319f7e436a043a75e2118 Mon Sep 17 00:00:00 2001
From: Francesco Gatti <gattifrancesco@hotmail.it>
Date: Mon, 9 Mar 2020 22:05:34 +0100
Subject: [PATCH] imuodom fix

---
 include/tkDNN/ImuOdom.h   | 25 ++++++++++++++++++++++---
 tests/imuodom/imuodom.cpp | 19 ++++++++++++-------
 2 files changed, 34 insertions(+), 10 deletions(-)

diff --git a/include/tkDNN/ImuOdom.h b/include/tkDNN/ImuOdom.h
index e3ade02..a6b449c 100644
--- a/include/tkDNN/ImuOdom.h
+++ b/include/tkDNN/ImuOdom.h
@@ -35,6 +35,9 @@ class ImuOdom {
         // output eigen CPU
         Eigen::MatrixXf deltaP, deltaQ;
 
+        Eigen::MatrixXd odomPOS, odomROT;
+        Eigen::Isometry3f tf = Eigen::Isometry3f::Identity();
+
         ImuOdom() {}
 
         virtual ~ImuOdom() {}
@@ -101,8 +104,11 @@ class ImuOdom {
             odim0 = d0->output_dim;
             odim1 = d1->output_dim;
 
-            deltaP.resize(1, odim0.tot());
-            deltaQ.resize(1, odim1.tot());
+            deltaP.resize(odim0.tot(), 1);
+            deltaQ.resize(odim1.tot(), 1);
+
+            odomPOS = Eigen::MatrixXd::Zero(3, 1); 
+            odomROT = Eigen::MatrixXd::Identity(3, 3);
         }
 
         void update(dnnType *x0, dnnType *x1, dnnType *x2) {
@@ -116,7 +122,20 @@ class ImuOdom {
             net->infer(dim, nullptr);
 
             checkCuda( cudaMemcpy(deltaP.data(), o0_d, odim0.tot()*sizeof(dnnType), cudaMemcpyDeviceToHost) );
-            checkCuda( cudaMemcpy(deltaQ.data(), o1_d, odim1.tot()*sizeof(dnnType), cudaMemcpyDeviceToHost) );            
+            checkCuda( cudaMemcpy(deltaQ.data(), o1_d, odim1.tot()*sizeof(dnnType), cudaMemcpyDeviceToHost) ); 
+
+            // compute odom
+            Eigen::Quaterniond q;
+            q.w() = deltaQ(0);
+            q.x() = deltaQ(1);
+            q.y() = deltaQ(2);
+            q.z() = deltaQ(3);    
+            odomPOS = odomPOS + odomROT*deltaP.cast<double>();
+            odomROT = odomROT * q.normalized().toRotationMatrix();
+
+            // compose tf
+            tf.matrix().block(0, 0, 3, 3) = odomROT.cast<float>();
+            tf.matrix().block(0, 3, 3, 1) = odomPOS.cast<float>();
         }
 
 };
diff --git a/tests/imuodom/imuodom.cpp b/tests/imuodom/imuodom.cpp
index e9e98b8..6be35dd 100644
--- a/tests/imuodom/imuodom.cpp
+++ b/tests/imuodom/imuodom.cpp
@@ -47,21 +47,26 @@ int main() {
     readBinaryFile(o0_bin, ImuNet.odim0.tot()*N, &out0_h, &out0);
     readBinaryFile(o1_bin, ImuNet.odim1.tot()*N, &out1_h, &out1);
 
-    for(int i=0; i<N; i++) {
-        TIMER_START
+    
+    std::ofstream path("path.txt");
 
+    for(int i=0; i<N; i++) {
+        std::cout<<"i: "<<i<<"\n";
+        //TIMER_START
         // Inference
         ImuNet.update(i0_h, i1_h, i2_h);
+        //TIMER_STOP
 
-        TIMER_STOP
+        // log path
+        path<<ImuNet.odomPOS(0)<<" "<<ImuNet.odomPOS(1)<<" "<< ImuNet.odomPOS(2)<<"\n";
+        path.flush();
 
         // Print real test
-        printCenteredTitle( (std::string(" CHECK RESULT ") + std::to_string(i) + " ").c_str() , '=');
-
+        //printCenteredTitle( (std::string(" CHECK RESULT ") + std::to_string(i) + " ").c_str() , '=');
         //ImuNet.odim0.print();
-        checkResult(ImuNet.odim0.tot(), out0, ImuNet.o0_d);
+        //checkResult(ImuNet.odim0.tot(), out0, ImuNet.o0_d);
         //ImuNet.odim1.print();
-        checkResult(ImuNet.odim0.tot(), out1, ImuNet.o1_d);
+        //checkResult(ImuNet.odim0.tot(), out1, ImuNet.o1_d);
 
         i0_h += ImuNet.dim0.tot();
         i1_h += ImuNet.dim1.tot();