From 7f94f49668bfdffc514c7dd2c278ffc421adac50 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 17 Jun 2020 19:15:34 +0200
Subject: [PATCH] Tetsts for odometry: init and integrate
---
test/gtest_processor_motion.cpp | 31 +++++++++++++++++++++++++++++++
1 file changed, 31 insertions(+)
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 527aba103..a977672d0 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -423,6 +423,37 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior)
C_source->getBuffer().print(1,1,1,0);
}
+TEST_F(ProcessorMotion_test, initOdometry)
+{
+ auto odometry = processor->getOdometry();
+
+ ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(0,0), 1e-20);
+ ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0 ), 1e-20);
+}
+
+TEST_F(ProcessorMotion_test, integrateOdometry)
+{
+ auto odometry = processor->getOdometry();
+
+ ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(0,0), 1e-20);
+ ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0 ), 1e-20);
+
+ data << 1,0;
+ capture->setData(data);
+
+ capture->setTimeStamp(capture->getTimeStamp() + 1.0);
+ capture->process();
+ odometry = processor->getOdometry();
+ ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(1,0), 1e-20);
+ ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0 ), 1e-20);
+
+ capture->setTimeStamp(capture->getTimeStamp() + 1.0);
+ capture->process();
+ odometry = processor->getOdometry();
+ ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(2,0), 1e-20);
+ ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0 ), 1e-20);
+}
+
int main(int argc, char **argv)
{
--
GitLab