From 3f6087172aeb07fc49aa8d4ed323befb0186463f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 23 Dec 2020 12:34:03 +0100
Subject: [PATCH] Make the bias test a true gtest by adding asserts

---
 ...est_processor_motion_intrinsics_update.cpp | 24 +++++++++++++++++++
 1 file changed, 24 insertions(+)

diff --git a/test/gtest_processor_motion_intrinsics_update.cpp b/test/gtest_processor_motion_intrinsics_update.cpp
index 210297368..4dbac9621 100644
--- a/test/gtest_processor_motion_intrinsics_update.cpp
+++ b/test/gtest_processor_motion_intrinsics_update.cpp
@@ -1,6 +1,8 @@
 #include <fstream>
 
+#include "imu/internal/config.h"
 #include "core/utils/utils_gtest.h"
+#include "core/utils/logging.h"
 
 #include "core/common/wolf.h"
 #include "core/problem/problem.h"
@@ -227,6 +229,28 @@ TEST_F(ProcessorImuTest, getState)
 #if WRITE_CSV_FILE
     file_est.close();
 #endif
+
+    /** assert final results
+     *
+     * t            0   .9  1   1.9 2   3
+     * BIAS         .4  .4  0   0   0   0
+     * BIAS PREINT  .4  .4  .4  .4  0   0
+     * X            0   !0  0   0   0   0
+     * V            0   !0  0   0   0   0
+     *
+     * though we only test at KFs so t = { 0, 1, 2, 3 }
+     */
+
+    for (auto F : *problem_->getTrajectory() )
+    {
+        auto ts = F->getTimeStamp();
+
+        double bias = (t == 0 ? 0.42 : 0.0);
+        ASSERT_NEAR(F->getCaptureOf(sensor_)->getSensorIntrinsic()->getState()(0), bias, 1e-6);
+        ASSERT_NEAR(F->getP()->getState()(0),0.0,1e-6);
+        ASSERT_NEAR(F->getV()->getState()(0),0.0,1e-6);
+    }
+
 }
 
 
-- 
GitLab