From 6c6d5428d98d0d7259ed4ad35da7ecc148036338 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 6 Feb 2019 10:33:54 +0100
Subject: [PATCH] Add another interpolate() prototype to API
To avoid confusion, this version has a separate entry for the second reference and the second part of the interpolation.
---
src/processor_motion.cpp | 45 ++++++++++++++++++++++++++++++++++++++++
src/processor_motion.h | 20 +++++++++++++++++-
2 files changed, 64 insertions(+), 1 deletion(-)
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index cfca3dd80..5457b42b9 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -515,6 +515,51 @@ Motion ProcessorMotion::interpolate(const Motion& _ref, Motion& _second, TimeSta
}
+Motion ProcessorMotion::interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second)
+{
+ // Check time bounds
+ assert(_ref1.ts_ <= _ref2.ts_ && "Interpolation bounds not causal.");
+ assert(_ts >= _ref1.ts_ && "Interpolation time is before the _ref1 motion.");
+ assert(_ts <= _ref2.ts_ && "Interpolation time is after the _ref2 motion.");
+
+ // Fraction of the time interval
+ Scalar tau = (_ts - _ref1.ts_) / (_ref2.ts_ - _ref1.ts_);
+
+ if (tau < 0.5)
+ {
+ // _ts is closest to _ref1
+ Motion interpolated ( _ref1 );
+ interpolated.ts_ = _ts;
+ interpolated.data_ . setZero();
+ interpolated.data_cov_ . setZero();
+ interpolated.delta_ = deltaZero();
+ interpolated.delta_cov_ . setZero();
+ interpolated.jacobian_delta_integr_ . setIdentity();
+ interpolated.jacobian_delta_ . setZero();
+
+ _second = _ref2;
+
+ return interpolated;
+ }
+ else
+ {
+ // _ts is closest to _ref2
+ Motion interpolated ( _ref2 );
+ interpolated.ts_ = _ts;
+
+ _second = _ref2;
+ _second.data_ . setZero();
+ _second.data_cov_ . setZero();
+ _second.delta_ = deltaZero();
+ _second.delta_cov_ . setZero();
+ _second.jacobian_delta_integr_ . setIdentity();
+ _second.jacobian_delta_ . setZero();
+
+ return interpolated;
+ }
+
+}
+
CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
{
// We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
diff --git a/src/processor_motion.h b/src/processor_motion.h
index 8e9f4bf83..a9e9b9771 100644
--- a/src/processor_motion.h
+++ b/src/processor_motion.h
@@ -372,6 +372,24 @@ class ProcessorMotion : public ProcessorBase
*/
virtual Motion interpolate(const Motion& _ref, Motion& _second, TimeStamp& _ts);
+ /** \brief Interpolate motion to an intermediate time-stamp
+ *
+ * @param _ref1 The first motion reference
+ * @param _ref2 The second motion reference.
+ * @param _ts The intermediate time stamp: it must be bounded by `_ref.ts_ <= _ts <= _second.ts_`.
+ * @param _second The second part motion after interpolation, so that return (+) second = ref2.
+ * @return The interpolated motion (see documentation below).
+ *
+ * This function interpolates a motion between two existing motions.
+ *
+ * In this base implementation, we just provide the closest motion provided (ref1 or ref2),
+ * the second motion being the complementary part,
+ * so really no interpolation takes place and just the current data and delta are updated.
+ *
+ * Should you require finer interpolation, you must overload this method in your derived class.
+ */
+ virtual Motion interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second);
+
/** \brief create a CaptureMotion and add it to a Frame
* \param _ts time stamp
* \param _sensor Sensor that produced the Capture
@@ -439,7 +457,7 @@ class ProcessorMotion : public ProcessorBase
SizeEigen data_size_; ///< the size of the incoming data
SizeEigen delta_size_; ///< the size of the deltas
SizeEigen delta_cov_size_; ///< the size of the delta covariances matrix
- SizeEigen calib_size_; ///< size of the extra parameters (TBD in derived classes)
+ SizeEigen calib_size_; ///< the size of the calibration parameters (TBD in derived classes)
CaptureMotionPtr origin_ptr_;
CaptureMotionPtr last_ptr_;
CaptureMotionPtr incoming_ptr_;
--
GitLab