From 62eb85dfc0cd4b970b53b1903d06c6329df556a7 Mon Sep 17 00:00:00 2001 From: Joan Sola <jsola@iri.upc.edu> Date: Wed, 6 Feb 2019 17:39:22 +0100 Subject: [PATCH] Use the new interpolate() with two outputs --- src/processor_odom_2D.cpp | 5 +++ src/processor_odom_2D.h | 4 ++ src/test/gtest_processor_motion.cpp | 60 +++++++++++++++++++++++++++++ 3 files changed, 69 insertions(+) diff --git a/src/processor_odom_2D.cpp b/src/processor_odom_2D.cpp index 5041ae985..20ec36f74 100644 --- a/src/processor_odom_2D.cpp +++ b/src/processor_odom_2D.cpp @@ -114,6 +114,11 @@ Motion ProcessorOdom2D::interpolate(const Motion& _ref, Motion& _second, TimeSta } +Motion ProcessorOdom2D::interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second) +{ + return ProcessorMotion::interpolate(_ref1, _ref2, _ts, _second); +} + bool ProcessorOdom2D::voteForKeyFrame() { // Distance criterion diff --git a/src/processor_odom_2D.h b/src/processor_odom_2D.h index bf797ca8b..69ac46686 100644 --- a/src/processor_odom_2D.h +++ b/src/processor_odom_2D.h @@ -60,6 +60,10 @@ class ProcessorOdom2D : public ProcessorMotion virtual Motion interpolate(const Motion& _ref, Motion& _second, TimeStamp& _ts) override; + virtual Motion interpolate(const Motion& _ref1, + const Motion& _ref2, + const TimeStamp& _ts, + Motion& _second); virtual CaptureMotionPtr createCapture(const TimeStamp& _ts, const SensorBasePtr& _sensor, diff --git a/src/test/gtest_processor_motion.cpp b/src/test/gtest_processor_motion.cpp index 94a2f0c15..a1ece3992 100644 --- a/src/test/gtest_processor_motion.cpp +++ b/src/test/gtest_processor_motion.cpp @@ -54,6 +54,17 @@ class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{ problem->setPrior(x0, P0, 0.0, 0.01); } + Motion interpolate(const Motion& _ref, Motion& _second, TimeStamp& _ts) + { + return ProcessorMotion::interpolate(_ref, _second, _ts); + } + + Motion interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second) + { + return ProcessorMotion::interpolate(_ref1, _ref2, _ts, _second); + } + + virtual void TearDown(){} }; @@ -135,6 +146,55 @@ TEST_F(ProcessorMotion_test, Interpolate) ASSERT_MATRIX_APPROX(interp.delta_integr_ , motions[3].delta_integr_ , 1e-8); } +TEST_F(ProcessorMotion_test, Interpolate_alternative) +{ + data << 1, 2*M_PI/10; // advance in turn + data_cov.setIdentity(); + TimeStamp t(0.0); + std::vector<Motion> motions; + motions.push_back(motionZero(t)); + + for (int i = 0; i<10; i++) // one full turn exactly + { + t += dt; + capture->setTimeStamp(t); + capture->setData(data); + capture->setDataCovariance(data_cov); + processor->process(capture); + motions.push_back(processor->getMotion(t)); + WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose()); + } + + TimeStamp tt = 2.2; + Motion ref1 = motions[2]; + Motion ref2 = motions[3]; + Motion second(0.0, 2, 3, 3, 0); + Motion interp = interpolate(ref1, ref2, tt, second); + + ASSERT_NEAR( interp.ts_.get() , 2.2 , 1e-8); + ASSERT_MATRIX_APPROX(interp.data_ , VectorXs::Zero(2) , 1e-8); + ASSERT_MATRIX_APPROX(interp.delta_ , VectorXs::Zero(3) , 1e-8); + ASSERT_MATRIX_APPROX(interp.delta_integr_ , motions[2].delta_integr_ , 1e-8); + + ASSERT_NEAR( second.ts_.get() , 3.0 , 1e-8); + ASSERT_MATRIX_APPROX(second.data_ , motions[3].data_ , 1e-8); + ASSERT_MATRIX_APPROX(second.delta_ , motions[3].delta_ , 1e-8); + ASSERT_MATRIX_APPROX(second.delta_integr_ , motions[3].delta_integr_ , 1e-8); + + tt = 2.6; + interp = interpolate(ref1, ref2, tt, second); + + ASSERT_NEAR( interp.ts_.get() , 2.6 , 1e-8); + ASSERT_MATRIX_APPROX(interp.data_ , motions[3].data_ , 1e-8); + ASSERT_MATRIX_APPROX(interp.delta_ , motions[3].delta_ , 1e-8); + ASSERT_MATRIX_APPROX(interp.delta_integr_ , motions[3].delta_integr_ , 1e-8); + + ASSERT_NEAR( second.ts_.get() , 3.0 , 1e-8); + ASSERT_MATRIX_APPROX(second.data_ , VectorXs::Zero(2) , 1e-8); + ASSERT_MATRIX_APPROX(second.delta_ , VectorXs::Zero(3) , 1e-8); + ASSERT_MATRIX_APPROX(second.delta_integr_ , motions[3].delta_integr_ , 1e-8); +} + int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); -- GitLab