diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp index 527aba1030ecc18a034b888d1ca02d75ad1408aa..a977672d0230aff9786590d10724d544036f7224 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) {