Tetsts for odometry: init and integrate

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)
 {