imuodom fix

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());
+            deltaP.resize(odim0.tot(), 1);
-            deltaQ.resize(1, odim1.tot());
+            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) {
@@ -117,6 +123,19 @@ class ImuOdom {
 
             checkCuda( cudaMemcpy(deltaP.data(), o0_d, odim0.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>();
         }
 
 };

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();