diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index cd1d7972d274dc6bafafa3227af7b27c11f76ebd..2eb19c69cd77ebb4b249dad0cda83d9a388c450e 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -402,39 +402,6 @@ class ProcessorMotion : public ProcessorBase
*/
virtual Eigen::VectorXs deltaZero() const = 0;
- /** \brief Interpolate motion to an intermediate time-stamp
- *
- * @param _ref The motion reference
- * @param _second The second motion. It is modified by the function (see documentation below).
- * @param _ts The intermediate time stamp: it must be bounded by `_ref.ts_ <= _ts <= _second.ts_`.
- * @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 (ref or second),
- * 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& _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 emplace a CaptureMotion
* \param _ts time stamp
diff --git a/include/core/processor/processor_odom_2D.h b/include/core/processor/processor_odom_2D.h
index a55b2e1c0b8610ccc17da17878b9936d64cfc407..8fca11b3e5913e683a9cfde766ad1e072ef5c32e 100644
--- a/include/core/processor/processor_odom_2D.h
+++ b/include/core/processor/processor_odom_2D.h
@@ -72,10 +72,6 @@ class ProcessorOdom2D : public ProcessorMotion
const Scalar _Dt,
Eigen::VectorXs& _x_plus_delta) override;
virtual Eigen::VectorXs deltaZero() const override;
- virtual Motion interpolate(const Motion& _ref1,
- const Motion& _ref2,
- const TimeStamp& _ts,
- Motion& _second) override;
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
diff --git a/include/core/processor/processor_odom_3D.h b/include/core/processor/processor_odom_3D.h
index 47334e580d8c9f1b50e8e99e05f4f76e5585180f..bd1e3cf013135ac90bfcb9dec749b9b64a63ce07 100644
--- a/include/core/processor/processor_odom_3D.h
+++ b/include/core/processor/processor_odom_3D.h
@@ -91,13 +91,6 @@ class ProcessorOdom3D : public ProcessorMotion
const Scalar _Dt,
Eigen::VectorXs& _x_plus_delta) override;
Eigen::VectorXs deltaZero() const override;
- Motion interpolate(const Motion& _motion_ref,
- Motion& _motion,
- TimeStamp& _ts) override;
- virtual Motion interpolate(const Motion& _ref1,
- const Motion& _ref2,
- const TimeStamp& _ts,
- Motion& _second) override;
bool voteForKeyFrame() override;
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 3c80ad1050683825586155488563993cc21023ca..633a98022487b5f54cfc93d27e2ac3018ecaea6d 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -29,7 +29,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
calib_preint_(_calib_size),
jacobian_delta_preint_(delta_cov_size_, delta_cov_size_),
jacobian_delta_(delta_cov_size_, delta_cov_size_),
- jacobian_calib_(delta_size_, calib_size_)
+ jacobian_calib_(delta_cov_size_, calib_size_)
{
//
}
@@ -48,19 +48,9 @@ void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
// and give the part of the buffer before the new keyframe to the capture for the KF callback
_capture_source->getBuffer().split(_ts_split, _capture_target->getBuffer());
-// // interpolate individual delta which has been cut by the split timestamp
-// if (!_capture_source->getBuffer().get().empty()
-// && _capture_target->getBuffer().get().back().ts_ != _ts_split)
-// {
-// // interpolate Motion at the new time stamp
-// Motion motion_interpolated = interpolate(_capture_target->getBuffer().get().back(), // last Motion of old buffer
-// _capture_source->getBuffer().get().front(), // first motion of new buffer
-// _ts_split,
-// _capture_source->getBuffer().get().front());
-//
-// // add to old buffer
-// _capture_target->getBuffer().get().push_back(motion_interpolated);
-// }
+ // start with empty motion
+ TimeStamp t_zero = _capture_target->getBuffer().get().back().ts_; // last tick of previous buffer is zero tick of current buffer
+ _capture_source->getBuffer().get().push_front(motionZero(t_zero));
// Update the existing capture
_capture_source->setOriginFrame(_keyframe_target);
@@ -407,13 +397,20 @@ void ProcessorMotion::integrateOneStep()
void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
{
- // start with empty motion
- _capture_ptr->getBuffer().get().push_front(motionZero(_capture_ptr->getOriginFrame()->getTimeStamp()));
+ VectorXs calib = _capture_ptr->getCalibrationPreint();
- VectorXs calib = _capture_ptr->getCalibrationPreint();
+ // some temporaries for integration
+ delta_integrated_ =deltaZero();
+ delta_integrated_cov_.setZero();
+ jacobian_calib_ .setZero();
+
+ // check that first motion in buffer is zero!
+ assert(_capture_ptr->getBuffer().get().front().delta_integr_ == delta_integrated_ && "Buffer's first motion is not zero!");
+ assert(_capture_ptr->getBuffer().get().front().delta_integr_cov_ == delta_integrated_cov_ && "Buffer's first motion is not zero!");
+ assert(_capture_ptr->getBuffer().get().front().jacobian_calib_ == jacobian_calib_ && "Buffer's first motion is not zero!");
// Iterate all the buffer
- auto motion_it = _capture_ptr->getBuffer().get().begin();
+ auto motion_it = _capture_ptr->getBuffer().get().begin();
auto prev_motion_it = motion_it;
motion_it++;
while (motion_it != _capture_ptr->getBuffer().get().end())
@@ -431,7 +428,7 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
jacobian_delta_calib_);
// integrate delta into delta_integr, and rewrite the buffer
- deltaPlusDelta(prev_motion_it->delta_integr_,
+ deltaPlusDelta(delta_integrated_,
motion_it->delta_,
dt,
motion_it->delta_integr_,
@@ -440,11 +437,17 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
// integrate Jacobian wrt calib
if ( hasCalibration() )
- motion_it->jacobian_calib_ = motion_it->jacobian_delta_integr_ * prev_motion_it->jacobian_calib_ + motion_it->jacobian_delta_ * jacobian_delta_calib_;
+ motion_it->jacobian_calib_ = motion_it->jacobian_delta_integr_ * jacobian_calib_
+ + motion_it->jacobian_delta_ * jacobian_delta_calib_;
// Integrate covariance
- motion_it->delta_integr_cov_ = motion_it->jacobian_delta_integr_ * prev_motion_it->delta_integr_cov_ * motion_it->jacobian_delta_integr_.transpose()
- + motion_it->jacobian_delta_ * motion_it->delta_cov_ * motion_it->jacobian_delta_.transpose();
+ motion_it->delta_integr_cov_ = motion_it->jacobian_delta_integr_ * delta_integrated_cov_ * motion_it->jacobian_delta_integr_.transpose()
+ + motion_it->jacobian_delta_ * motion_it->delta_cov_ * motion_it->jacobian_delta_.transpose();
+
+ // update temporaries
+ delta_integrated_ = motion_it->delta_integr_;
+ delta_integrated_cov_ = motion_it->delta_integr_cov_;
+ jacobian_calib_ = motion_it->jacobian_calib_;
// advance in buffer
motion_it++;
@@ -452,146 +455,13 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
}
}
-Motion ProcessorMotion::interpolate(const Motion& _ref, Motion& _second, TimeStamp& _ts)
-{
- // Check time bounds
- assert(_ref.ts_ <= _second.ts_ && "Interpolation bounds not causal.");
- assert(_ts >= _ref.ts_ && "Interpolation time is before the _ref motion.");
- assert(_ts <= _second.ts_ && "Interpolation time is after the _second motion.");
-
- // Fraction of the time interval
- Scalar tau = (_ts - _ref.ts_) / (_second.ts_ - _ref.ts_);
-
- if (tau < 0.5)
- {
- // _ts is closest to _ref
- Motion interpolated ( _ref );
- 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();
-
- return interpolated;
- }
- else
- {
- // _ts is closest to _second
- Motion interpolated ( _second );
- interpolated.ts_ = _ts;
- _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;
- }
-
-}
-
-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_);
-
-
-
-
- Motion interpolated(_ref1);
- interpolated.ts_ = _ts;
- interpolated.data_ = (1-tau)*_ref1.data_ + tau*_ref2.data_;
- interpolated.data_cov_ = (1-tau)*_ref1.data_cov_ + tau*_ref2.data_cov_; // bof
- computeCurrentDelta(interpolated.data_,
- interpolated.data_cov_,
- calib_preint_,
- _ts.get() - _ref1.ts_.get(),
- interpolated.delta_,
- interpolated.delta_cov_,
- interpolated.jacobian_calib_);
- deltaPlusDelta(_ref1.delta_integr_,
- interpolated.delta_,
- _ts.get() - _ref1.ts_.get(),
- interpolated.delta_integr_,
- interpolated.jacobian_delta_integr_,
- interpolated.jacobian_delta_);
-
- _second = _ref2;
- _second.data_ = tau*_ref1.data_ + (1-tau)*_ref2.data_;
- _second.data_cov_ = tau*_ref1.data_cov_ + (1-tau)*_ref2.data_cov_; // bof
- computeCurrentDelta(_second.data_,
- _second.data_cov_,
- calib_preint_,
- _ref2.ts_.get() - _ts.get(),
- _second.delta_,
- _second.delta_cov_,
- _second.jacobian_calib_);
-
-// deltaPlusDelta(_second.delta_integr_,
-// _second.delta_,
-// _second.ts_.get() - _ref1.ts_.get(),
-// _second.delta_integr_,
-// _second.jacobian_delta_integr_,
-// _second.jacobian_delta_);
-
- return interpolated;
-
-
-
-
- // if (tau < 0.5)
- // {
- // // _ts is closest to _ref1
- // Motion interpolated ( _ref1 );
- // // interpolated.ts_ = _ref1.ts_;
- // // interpolated.data_ = _ref1.data_;
- // // interpolated.data_cov_ = _ref1.data_cov_;
- // interpolated.delta_ = deltaZero();
- // interpolated.delta_cov_ . setZero();
- // // interpolated.delta_integr_ = _ref1.delta_integr_;
- // // interpolated.delta_integr_cov_ = _ref1.delta_integr_cov_;
- // interpolated.jacobian_delta_integr_ . setIdentity();
- // interpolated.jacobian_delta_ . setZero();
-
- // _second = _ref2;
-
- // return interpolated;
- // }
- // else
- // {
- // // _ts is closest to _ref2
- // Motion interpolated ( _ref2 );
-
- // _second = _ref2;
- // // _second.data_ = _ref2.data_;
- // // _second.data_cov_ = _ref2.data_cov_;
- // _second.delta_ = deltaZero();
- // _second.delta_cov_ . setZero();
- // // _second.delta_integr_ = _ref2.delta_integr_;
- // // _second.delta_integr_cov_ = _ref2.delta_integr_cov_;
- // _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
// Note: since the buffer goes from a KF in the past until the next KF, we need to:
// 1. See that the KF contains a CaptureMotion
// 2. See that the TS is smaller than the KF's TS
- // 3. See that the TS is bigger than the KF's first Motion in the CaptureMotion's buffer
+ // 3. See that the TS is bigger than the first Motion in the CaptureMotion's buffer
FrameBasePtr frame = nullptr;
CaptureBasePtr capture = nullptr;
CaptureMotionPtr capture_motion = nullptr;
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index 96241bd069a64221ecce0ce86b5481f7e675ae1f..dda3af4ddaa964537b95d85eac1a8cccf1fb7167 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2D.cpp
@@ -92,11 +92,6 @@ void ProcessorOdom2D::statePlusDelta(const Eigen::VectorXs& _x, const Eigen::Vec
_x_plus_delta(2) = pi2pi(_x(2) + _delta(2));
}
-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/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp
index 204ecd9d54d4dbd4f359e51b93553cf95baa7436..6c20d3e167f66449ccab2f1e041febaacce4ce5e 100644
--- a/src/processor/processor_odom_3D.cpp
+++ b/src/processor/processor_odom_3D.cpp
@@ -147,191 +147,6 @@ void ProcessorOdom3D::statePlusDelta(const Eigen::VectorXs& _x, const Eigen::Vec
q_out_ = q1_ * q2_;
}
-Motion ProcessorOdom3D::interpolate(const Motion& _motion_ref,
- Motion& _motion_second,
- TimeStamp& _ts)
-{
-
-// WOLF_TRACE("motion ref ts: ", _motion_ref.ts_.get());
-// WOLF_TRACE("interp ts : ", _ts.get());
-// WOLF_TRACE("motion ts : ", _motion_second.ts_.get());
-//
-// WOLF_TRACE("ref delta size: ", _motion_ref.delta_.size(), " , cov size: ", _motion_ref.delta_cov_.size());
-// WOLF_TRACE("ref Delta size: ", _motion_ref.delta_integr_.size(), " , cov size: ", _motion_ref.delta_integr_cov_.size());
-// WOLF_TRACE("sec delta size: ", _motion_second.delta_.size(), " , cov size: ", _motion_second.delta_cov_.size());
-// WOLF_TRACE("sec Delta size: ", _motion_second.delta_integr_.size(), " , cov size: ", _motion_second.delta_integr_cov_.size());
-
- // resolve out-of-bounds time stamp as if the time stamp was exactly on the bounds
- if (_ts <= _motion_ref.ts_) // behave as if _ts == _motion_ref.ts_
- { // return null motion. Second stays the same.
- Motion motion_int(_motion_ref);
- motion_int.delta_ = deltaZero();
- motion_int.delta_cov_.setZero();
- return motion_int;
- }
- if (_motion_second.ts_ <= _ts) // behave as if _ts == _motion_second.ts_
- { // return original second motion. Second motion becomes null motion
- Motion motion_int(_motion_second);
- _motion_second.delta_ = deltaZero();
- _motion_second.delta_cov_.setZero();
- return motion_int;
- }
-
- assert(_motion_ref.ts_ <= _ts && "Interpolation time stamp out of bounds");
- assert(_ts <= _motion_second.ts_ && "Interpolation time stamp out of bounds");
-
- assert(_motion_ref.delta_.size() == delta_size_ && "Wrong delta size");
- assert(_motion_ref.delta_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_ref.delta_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_ref.delta_integr_.size() == delta_size_ && "Wrong delta size");
-// assert(_motion_ref.delta_integr_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
-// assert(_motion_ref.delta_integr_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_second.delta_.size() == delta_size_ && "Wrong delta size");
- assert(_motion_second.delta_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_second.delta_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_second.delta_integr_.size() == delta_size_ && "Wrong delta size");
-// assert(_motion_second.delta_integr_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
-// assert(_motion_second.delta_integr_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
-
- using namespace Eigen;
- // Interpolate between motion_ref and motion, as in:
- //
- // motion_ref ------ ts_ ------ motion
- // return
- //
- // and return the value at the given time_stamp ts_.
- //
- // The position receives linear interpolation:
- // p_ret = (ts - t_ref) / dt * (p - p_ref)
- //
- // the quaternion receives a slerp interpolation
- // q_ret = q_ref.slerp( (ts - t_ref) / dt , q);
- //
- // See extensive documentation in ProcessorMotion::interpolate().
-
- // reference
- TimeStamp t_ref = _motion_ref.ts_;
-
- // final
- TimeStamp t_sec = _motion_second.ts_;
- Map<VectorXs> dp_sec(_motion_second.delta_.data(), 3);
- Map<Quaternions> dq_sec(_motion_second.delta_.data() + 3);
-
- // interpolated
- Motion motion_int = motionZero(_ts);
- Map<VectorXs> dp_int(motion_int.delta_.data(), 3);
- Map<Quaternions> dq_int(motion_int.delta_.data() + 3);
-
- // Jacobians for covariance propagation
- MatrixXs J_ref(delta_cov_size_, delta_cov_size_);
- MatrixXs J_int(delta_cov_size_, delta_cov_size_);
-
- // interpolate delta
- Scalar tau = (_ts - t_ref) / (t_sec - t_ref); // interpolation factor (0 to 1)
- motion_int.ts_ = _ts;
- dp_int = tau * dp_sec;
- dq_int = Quaternions::Identity().slerp(tau, dq_sec);
- deltaPlusDelta(_motion_ref.delta_integr_, motion_int.delta_, (t_sec - t_ref), motion_int.delta_integr_, J_ref, J_int);
-
- // interpolate covariances
- motion_int.delta_cov_ = tau * _motion_second.delta_cov_;
-// motion_int.delta_integr_cov_ = J_ref * _motion_ref.delta_integr_cov_ * J_ref.transpose() + J_int * _motion_second.delta_cov_ * J_int.transpose();
-
- // update second delta ( in place update )
- dp_sec = dq_int.conjugate() * ((1 - tau) * dp_sec);
- dq_sec = dq_int.conjugate() * dq_sec;
- _motion_second.delta_cov_ = (1 - tau) * _motion_second.delta_cov_; // easy interpolation // TODO check for correctness
- //Dp = Dp; // trivial, just leave the code commented
- //Dq = Dq; // trivial, just leave the code commented
- //_motion.delta_integr_cov_ = _motion.delta_integr_cov_; // trivial, just leave the code commented
-
- return motion_int;
-}
-
-Motion ProcessorOdom3D::interpolate(const Motion& _ref1,
- const Motion& _ref2,
- const TimeStamp& _ts,
- Motion& _second)
-{
- // resolve out-of-bounds time stamp as if the time stamp was exactly on the bounds
- if (_ts <= _ref1.ts_ || _ref2.ts_ <= _ts)
- return ProcessorMotion::interpolate(_ref1, _ref2, _ts, _second);
-
- assert(_ref1.ts_ <= _ts && "Interpolation time stamp out of bounds");
- assert(_ts <= _ref2.ts_ && "Interpolation time stamp out of bounds");
-
- assert(_ref1.delta_.size() == delta_size_ && "Wrong delta size");
- assert(_ref1.delta_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- assert(_ref1.delta_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
- assert(_ref1.delta_integr_.size() == delta_size_ && "Wrong delta size");
- // assert(_motion_ref.delta_integr_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- // assert(_motion_ref.delta_integr_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
- assert(_ref2.delta_.size() == delta_size_ && "Wrong delta size");
- assert(_ref2.delta_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- assert(_ref2.delta_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
- assert(_ref2.delta_integr_.size() == delta_size_ && "Wrong delta size");
- // assert(_motion_second.delta_integr_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- // assert(_motion_second.delta_integr_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
-
-
- using namespace Eigen;
- // Interpolate between ref1 and ref2, as in:
- //
- // ref1 ------ ts ------ ref2
- // return second
- //
- // and return the value at the given time_stamp ts_, and the second motion.
- //
- // The position receives linear interpolation:
- // p_ret = (ts - t_ref) / dt * (p - p_ref)
- //
- // the quaternion receives a slerp interpolation
- // q_ret = q_ref.slerp( (ts - t_ref) / dt , q);
- //
- // See extensive documentation in ProcessorMotion::interpolate().
-
- // reference
- TimeStamp t1 = _ref1.ts_;
-
- // final
- TimeStamp t2 = _ref2.ts_;
- Map<const VectorXs> dp2(_ref2.delta_.data(), 3);
- Map<const Quaternions> dq2(_ref2.delta_.data() + 3);
-
- // interpolated
- Motion motion_int = motionZero(_ts);
- Map<VectorXs> dp_int(motion_int.delta_.data(), 3);
- Map<Quaternions> dq_int(motion_int.delta_.data() + 3);
-
- // Jacobians for covariance propagation
- MatrixXs J_ref(delta_cov_size_, delta_cov_size_);
- MatrixXs J_int(delta_cov_size_, delta_cov_size_);
-
- // interpolate delta
- Scalar tau = (_ts - t1) / (t2 - t1); // interpolation factor (0 to 1)
- motion_int.ts_ = _ts;
- dp_int = tau * dp2;
- dq_int = Quaternions::Identity().slerp(tau, dq2);
- deltaPlusDelta(_ref1.delta_integr_, motion_int.delta_, (t2 - t1), motion_int.delta_integr_, J_ref, J_int);
-
- // interpolate covariances
- motion_int.delta_cov_ = tau * _ref2.delta_cov_;
- // motion_int.delta_integr_cov_ = J_ref * _motion_ref.delta_integr_cov_ * J_ref.transpose() + J_int * _motion_second.delta_cov_ * J_int.transpose();
-
- // update second delta ( in place update )
- _second = _ref2;
- Map<VectorXs> dp_sec(_second.delta_.data(), 3);
- Map<Quaternions> dq_sec(_second.delta_.data() + 3);
- dp_sec = dq_int.conjugate() * ((1 - tau) * dp2);
- dq_sec = dq_int.conjugate() * dq2;
- _second.delta_cov_ = (1 - tau) * _ref2.delta_cov_; // easy interpolation // TODO check for correctness
- //Dp = Dp; // trivial, just leave the code commented
- //Dq = Dq; // trivial, just leave the code commented
- //_motion.delta_integr_cov_ = _motion.delta_integr_cov_; // trivial, just leave the code commented
-
- return motion_int;
-}
-
bool ProcessorOdom3D::voteForKeyFrame()
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index 4970ddc97e0e8f63079c9b55b82e2d75ec243c25..83e9ea62baf912954f94bb135408969398a37201 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -476,15 +476,16 @@ TEST(Odom2D, KF_callback)
for (int n=1; n<=N; n++)
{
t += dt;
- // WOLF_DEBUG(" estimated(", t, ") = ", problem->getState(t).transpose());
- // WOLF_DEBUG("ground truth(", t, ") = ", integrated_pose_vector[n].transpose());
- EXPECT_POSE2D_APPROX(problem->getState(t), integrated_pose_vector[n], 1e-6);
+ WOLF_DEBUG(" estimated(", t, ") = ", problem->getState(t).transpose());
+ WOLF_DEBUG("ground truth(", t, ") = ", integrated_pose_vector[n].transpose());
+ ASSERT_POSE2D_APPROX(problem->getState(t), integrated_pose_vector[n], 1e-6);
}
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
+ testing::GTEST_FLAG(filter) = "Odom2D.KF_callback";
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index e508069f55ffeeffcc7ab28f00af7341f2dd9c9b..93bf18911e2daadcabdd5ad7df66a9161fdb679c 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -18,20 +18,43 @@ using namespace Eigen;
using namespace wolf;
using std::static_pointer_cast;
-class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{
+class ProcessorOdom2DPublic : public ProcessorOdom2D
+{
+ public:
+ ProcessorOdom2DPublic(ProcessorParamsOdom2DPtr params) : ProcessorOdom2D(params)
+ {
+ //
+ }
+ virtual ~ProcessorOdom2DPublic(){}
+
+ void splitBuffer(const wolf::CaptureMotionPtr& capture_source,
+ TimeStamp ts_split,
+ const FrameBasePtr& keyframe_target,
+ const wolf::CaptureMotionPtr& capture_target)
+ {
+ ProcessorOdom2D::splitBuffer(capture_source,
+ ts_split,
+ keyframe_target,
+ capture_target);
+ }
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorOdom2DPublic);
+
+class ProcessorMotion_test : public testing::Test{
public:
- Scalar dt;
- ProblemPtr problem;
- SensorOdom2DPtr sensor;
- ProcessorOdom2DPtr processor;
- CaptureMotionPtr capture;
- Vector2s data;
- Matrix2s data_cov;
-
- ProcessorMotion_test() :
- ProcessorOdom2D(std::make_shared<ProcessorParamsOdom2D>()),
- dt(0)
- { }
+ Scalar dt;
+ ProblemPtr problem;
+ SensorOdom2DPtr sensor;
+ ProcessorOdom2DPublicPtr processor;
+ CaptureMotionPtr capture;
+ Vector2s data;
+ Matrix2s data_cov;
+
+// ProcessorMotion_test() :
+// ProcessorOdom2D(std::make_shared<ProcessorParamsOdom2D>()),
+// dt(0)
+// { }
virtual void SetUp()
{
@@ -39,6 +62,7 @@ class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{
dt = 1.0;
problem = Problem::create("PO", 2);
+ sensor = static_pointer_cast<SensorOdom2D>(problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml"));
ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
params->time_tolerance = 0.25;
params->dist_traveled = 100;
@@ -46,8 +70,7 @@ class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{
params->max_time_span = 2.5*dt; // force KF at every third process()
params->cov_det = 100;
params->unmeasured_perturbation_std = 0.001;
- sensor = static_pointer_cast<SensorOdom2D>(problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml"));
- processor = static_pointer_cast<ProcessorOdom2D>(problem->installProcessor("ODOM 2D", "odom", sensor, params));
+ processor = ProcessorBase::emplace<ProcessorOdom2DPublic>(sensor, params);
capture = std::make_shared<CaptureMotion>("ODOM 2D", 0.0, sensor, data, data_cov, 3, 3, nullptr);
Vector3s x0; x0 << 0, 0, 0;
@@ -55,17 +78,6 @@ 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(){}
};
@@ -108,13 +120,11 @@ TEST_F(ProcessorMotion_test, IntegrateCircle)
ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3s()<<0,0,0).finished(), 1e-8);
}
-TEST_F(ProcessorMotion_test, Interpolate)
+TEST_F(ProcessorMotion_test, splitBuffer)
{
- data << 1, 2*M_PI/10; // advance in turn
+ data << 1, 2*M_PI/10; // advance in circle
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
{
@@ -123,77 +133,34 @@ TEST_F(ProcessorMotion_test, Interpolate)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- motions.push_back(processor->getMotion(t));
WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
}
- TimeStamp tt = 2.2;
- Motion ref = motions[2];
- Motion second = motions[3];
- Motion interp = interpolate(ref, second, tt);
-
- 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);
-
- tt = 2.6;
- interp = interpolate(ref, second, tt);
-
- 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);
+ SensorBasePtr S = processor->getSensor();
+
+ TimeStamp t_target = 8.5;
+ FrameBasePtr F_target = problem->emplaceFrame(KEY, t_target);
+ CaptureBasePtr C_last = processor->getLast();
+ CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(C_last);
+ FrameBasePtr F_origin = C_source->getOriginFrame();
+ CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
+ "ODOM 2D",
+ t_target,
+ sensor,
+ data,
+ 3,
+ 3,
+ F_origin);
+
+ processor->splitBuffer(C_source,
+ t_target,
+ F_target,
+ C_target);
+
+ C_target->getBuffer().print(1,1,1,0);
+ C_source->getBuffer().print(1,1,1,0);
}
-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);
- capture->process();
- 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_ , data , 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_ , 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_ , 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_ , data , 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)
{
diff --git a/test/gtest_processor_odom_3D.cpp b/test/gtest_processor_odom_3D.cpp
index 3c13449cede9e839e14c50e47d7db78e2134dc1d..421efc97afa7227e15013bd4a97d8f6c8b409759 100644
--- a/test/gtest_processor_odom_3D.cpp
+++ b/test/gtest_processor_odom_3D.cpp
@@ -151,425 +151,6 @@ TEST(ProcessorOdom3D, deltaPlusDelta_Jac)
}
-TEST(ProcessorOdom3D, Interpolate0) // basic test
-{
- /* Conditions:
- * ref d = id
- * ref D = id
- * fin d = pos
- * fin D = id
- */
-
- ProcessorOdom3D prc(std::make_shared<ProcessorParamsOdom3D>());
-
- Motion ref(0.0,6,7,6,0), final(0.0,6,7,6,0), interpolated(0.0,6,7,6,0);
-
- // set ref
- ref.ts_ = 1;
- ref.delta_ << 0,0,0, 0,0,0,1;
- ref.delta_integr_ << 0,0,0, 0,0,0,1;
-
- WOLF_DEBUG("ref delta= ", ref.delta_.transpose());
- WOLF_DEBUG("ref Delta= ", ref.delta_integr_.transpose());
-
- // set final
- final.ts_ = 5;
- final.delta_ << 1,0,0, 0,0,0,1;
- final.delta_integr_ << 0,0,0, 0,0,0,1;
- prc.deltaPlusDelta(ref.delta_integr_, final.delta_, (final.ts_ - ref.ts_), final.delta_integr_);
-
- WOLF_DEBUG("final delta= ", final.delta_.transpose());
- WOLF_DEBUG("final Delta= ", final.delta_integr_.transpose());
-
- // interpolate!
- Motion second = final;
- TimeStamp t; t = 2;
- // +--+--------+---> time(s)
- // 1 2 3 4 5 // 2 = 25% into interpolated, 75% into second
- interpolated = prc.interpolate(ref, second, t);
-
- WOLF_DEBUG("interpolated delta= ", interpolated.delta_.transpose());
- WOLF_DEBUG("interpolated Delta= ", interpolated.delta_integr_.transpose());
-
- // delta
- ASSERT_MATRIX_APPROX(interpolated.delta_.head<3>() , 0.25 * final.delta_.head<3>(), Constants::EPS);
- ASSERT_MATRIX_APPROX(second.delta_.head<3>() , 0.75 * final.delta_.head<3>(), Constants::EPS);
-
-}
-
-TEST(ProcessorOdom3D, Interpolate1) // delta algebra test
-{
- ProcessorOdom3D prc(std::make_shared<ProcessorParamsOdom3D>());
-
- /*
- * We create several poses: origin, ref, int, second, final, as follows:
- *
- * +---+---+---+---->
- * o r i s,f
- *
- * We compute all deltas between them: d_or, d_ri, d_is, d_rf
- * We create the motions R, F
- * We interpolate, and get I, S
- */
-
- // absolute poses: origin, ref, interp, second=final
- Vector7s x_o, x_r, x_i, x_s, x_f;
- Map<Vector3s> p_o(x_o.data(), 3);
- Map<Quaternions> q_o(x_o.data() +3);
- Map<Vector3s> p_r(x_r.data(), 3);
- Map<Quaternions> q_r(x_r.data() +3);
- Map<Vector3s> p_i(x_i.data(), 3);
- Map<Quaternions> q_i(x_i.data() +3);
- Map<Vector3s> p_s(x_s.data(), 3);
- Map<Quaternions> q_s(x_s.data() +3);
- Map<Vector3s> p_f(x_f.data(), 3);
- Map<Quaternions> q_f(x_f.data() +3);
-
- // deltas -- referred to previous delta
- // o-r r-i i-s s-f
- Vector7s dx_or, dx_ri, dx_is, dx_rf;
- Map<Vector3s> dp_or(dx_or.data(), 3);
- Map<Quaternions> dq_or(dx_or.data() +3);
- Map<Vector3s> dp_ri(dx_ri.data(), 3);
- Map<Quaternions> dq_ri(dx_ri.data() +3);
- Map<Vector3s> dp_is(dx_is.data(), 3);
- Map<Quaternions> dq_is(dx_is.data() +3);
- Map<Vector3s> dp_rf(dx_rf.data(), 3);
- Map<Quaternions> dq_rf(dx_rf.data() +3);
- Map<Vector7s> dx_rs(dx_rf.data(), 7); // this ensures dx_rs = dx_rf
-
- // Deltas -- always referred to origin
- // o-r o-i o-s o-f
- Vector7s Dx_or, Dx_oi, Dx_os, Dx_of;
- Map<Vector3s> Dp_or(Dx_or.data(), 3);
- Map<Quaternions> Dq_or(Dx_or.data() +3);
- Map<Vector3s> Dp_oi(Dx_oi.data(), 3);
- Map<Quaternions> Dq_oi(Dx_oi.data() +3);
- Map<Vector3s> Dp_os(Dx_os.data(), 3);
- Map<Quaternions> Dq_os(Dx_os.data() +3);
- Map<Vector3s> Dp_of(Dx_of.data(), 3);
- Map<Quaternions> Dq_of(Dx_of.data() +3);
-
- // time stamps and intervals
- TimeStamp t_o(0), t_r(1), t_i(2.3), t_f(5); // t_i=2: 25% of motion; t_i=2.3: a general interpolation point
- Scalar dt_ri = t_i - t_r;
- Scalar dt_rf = t_f - t_r;
-
- WOLF_DEBUG("t_o: ", t_o.get(), "; t_r: ", t_r.get(), "; t_i: ", t_i.get(), "; t_f: ", t_f.get());
- WOLF_DEBUG("dt_ri: ", dt_ri, "; dt_rf ", dt_rf)
-
- // Constant velocity model
- Vector3s v;
- Vector3s w;
-
- // Motion structures
- Motion R(0.0,6,7,6,0), I(0.0,6,7,6,0), S(0.0,6,7,6,0), F(0.0,6,7,6,0);
-
- /////////// start experiment ///////////////
-
- // set origin and ref states
- x_o << 1,2,3, 4,5,6,7; q_o.normalize();
- x_r << 7,6,5, 4,3,2,1; q_r.normalize();
-
- // set constant velocity params
- v << 3,2,1; // linear velocity
- w << .1,.2,.3; // angular velocity
-
- // compute other poses from model
- p_i = p_r + v * dt_ri;
- q_i = q_r * v2q (w * dt_ri);
- p_f = p_r + v * dt_rf;
- q_f = q_r * v2q (w * dt_rf);
- x_s = x_f;
-
- WOLF_DEBUG("o = ", x_o.transpose());
- WOLF_DEBUG("r = ", x_r.transpose());
- WOLF_DEBUG("i = ", x_i.transpose());
- WOLF_DEBUG("s = ", x_s.transpose());
- WOLF_DEBUG("f = ", x_f.transpose());
-
- // deltas -- referred to previous delta
- dp_or = q_o.conjugate() * (p_r - p_o);
- dq_or = q_o.conjugate() * q_r;
- dp_ri = q_r.conjugate() * (p_i - p_r);
- dq_ri = q_r.conjugate() * q_i;
- dp_is = q_i.conjugate() * (p_s - p_i);
- dq_is = q_i.conjugate() * q_s;
- dp_rf = q_r.conjugate() * (p_f - p_r);
- dq_rf = q_r.conjugate() * q_f;
-
- // Deltas -- always referred to origin
- Dp_or = q_o.conjugate() * (p_r - p_o);
- Dq_or = q_o.conjugate() * q_r;
- Dp_oi = q_o.conjugate() * (p_i - p_o);
- Dq_oi = q_o.conjugate() * q_i;
- Dp_os = q_o.conjugate() * (p_s - p_o);
- Dq_os = q_o.conjugate() * q_s;
- Dp_of = q_o.conjugate() * (p_f - p_o);
- Dq_of = q_o.conjugate() * q_f;
-
- // set ref
- R.ts_ = t_r;
- R.delta_ = dx_or; // origin to ref
- R.delta_integr_ = Dx_or; // origin to ref
-
- WOLF_DEBUG("* R.d = ", R.delta_.transpose());
- WOLF_DEBUG(" or = ", dx_or.transpose());
- ASSERT_MATRIX_APPROX(R.delta_ , dx_or, Constants::EPS);
-
- WOLF_DEBUG(" R.D = ", R.delta_integr_.transpose());
- WOLF_DEBUG(" or = ", Dx_or.transpose());
- ASSERT_MATRIX_APPROX(R.delta_integr_ , Dx_or, Constants::EPS);
-
- // set final
- F.ts_ = t_f;
- F.delta_ = dx_rf; // ref to final
- F.delta_integr_ = Dx_of; // origin to final
-
- WOLF_DEBUG("* F.d = ", F.delta_.transpose());
- WOLF_DEBUG(" rf = ", dx_rf.transpose());
- ASSERT_MATRIX_APPROX(F.delta_ , dx_rf, Constants::EPS);
-
- WOLF_DEBUG(" F.D = ", F.delta_integr_.transpose());
- WOLF_DEBUG(" of = ", Dx_of.transpose());
- ASSERT_MATRIX_APPROX(F.delta_integr_ , Dx_of, Constants::EPS);
-
- S = F; // avoid overwriting final
- WOLF_DEBUG("* S.d = ", S.delta_.transpose());
- WOLF_DEBUG(" rs = ", dx_rs.transpose());
- ASSERT_MATRIX_APPROX(S.delta_ , dx_rs, Constants::EPS);
-
- WOLF_DEBUG(" S.D = ", S.delta_integr_.transpose());
- WOLF_DEBUG(" os = ", Dx_os.transpose());
- ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_os, Constants::EPS);
-
- // interpolate!
- WOLF_DEBUG("*** INTERPOLATE *** I has been computed; S has changed.");
- I = prc.interpolate(R, S, t_i);
-
- WOLF_DEBUG("* I.d = ", I.delta_.transpose());
- WOLF_DEBUG(" ri = ", dx_ri.transpose());
- ASSERT_MATRIX_APPROX(I.delta_ , dx_ri, Constants::EPS);
-
- WOLF_DEBUG(" I.D = ", I.delta_integr_.transpose());
- WOLF_DEBUG(" oi = ", Dx_oi.transpose());
- ASSERT_MATRIX_APPROX(I.delta_integr_ , Dx_oi, Constants::EPS);
-
- WOLF_DEBUG("* S.d = ", S.delta_.transpose());
- WOLF_DEBUG(" is = ", dx_is.transpose());
- ASSERT_MATRIX_APPROX(S.delta_ , dx_is, Constants::EPS);
-
- WOLF_DEBUG(" S.D = ", S.delta_integr_.transpose());
- WOLF_DEBUG(" os = ", Dx_os.transpose());
- ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_os, Constants::EPS);
-
-}
-
-TEST(ProcessorOdom3D, Interpolate2) // timestamp out of bounds test
-{
- ProcessorOdom3D prc(std::make_shared<ProcessorParamsOdom3D>());
-
- /*
- * We create several poses: origin, ref, int, second, final, as follows:
- *
- * +---+---+---+---->
- * o r i s,f
- *
- * We compute all deltas between them: d_or, d_ri, d_is, d_rf
- * We create the motions R, F
- * We interpolate, and get I, S
- */
-
- // deltas -- referred to previous delta
- // o-r r-i i-s s-f
- VectorXs dx_or(7), dx_ri(7), dx_is(7), dx_rf(7);
- Map<VectorXs> dx_rs(dx_rf.data(), 7); // this ensures dx_rs = dx_rf
-
- // Deltas -- always referred to origin
- // o-r o-i o-s o-f
- VectorXs Dx_or(7), Dx_oi(7), Dx_os(7), Dx_of(7);
-
- // time stamps and intervals
- TimeStamp t_o(0), t_r(1), t_i, t_f(5); // we'll set t_i later
-
- WOLF_DEBUG("t_o: ", t_o.get(), "; t_r: ", t_r.get(), "; t_f: ", t_f.get());
-
- // Motion structures
- Motion R(0.0,6,7,6,0), I(0.0,6,7,6,0), S(0.0,6,7,6,0), F(0.0,6,7,6,0);
-
- /////////// start experiment ///////////////
-
- // set final and ref deltas
- dx_or << 1,2,3, 4,5,6,7; dx_or.tail<4>().normalize();
- dx_rf << 7,6,5, 4,3,2,1; dx_rf.tail<4>().normalize();
- Dx_or = dx_or;
- prc.deltaPlusDelta(Dx_or, dx_rf, t_f - t_r, Dx_of);
- Dx_os = Dx_of;
-
- // set ref
- R.ts_ = t_r;
- R.delta_ = dx_or; // origin to ref
- R.delta_integr_ = Dx_or; // origin to ref
-
- WOLF_DEBUG("* R.d = ", R.delta_.transpose());
- WOLF_DEBUG(" or = ", dx_or.transpose());
- ASSERT_MATRIX_APPROX(R.delta_ , dx_or, Constants::EPS);
-
- WOLF_DEBUG(" R.D = ", R.delta_integr_.transpose());
- WOLF_DEBUG(" or = ", Dx_or.transpose());
- ASSERT_MATRIX_APPROX(R.delta_integr_ , Dx_or, Constants::EPS);
-
- // set final
- F.ts_ = t_f;
- F.delta_ = dx_rf; // ref to final
- F.delta_integr_ = Dx_of; // origin to final
-
- WOLF_DEBUG("* F.d = ", F.delta_.transpose());
- WOLF_DEBUG(" rf = ", dx_rf.transpose());
- ASSERT_MATRIX_APPROX(F.delta_ , dx_rf, Constants::EPS);
-
- WOLF_DEBUG(" F.D = ", F.delta_integr_.transpose());
- WOLF_DEBUG(" of = ", Dx_of.transpose());
- ASSERT_MATRIX_APPROX(F.delta_integr_ , Dx_of, Constants::EPS);
-
- S = F; // avoid overwriting final
- WOLF_DEBUG("* S.d = ", S.delta_.transpose());
- WOLF_DEBUG(" rs = ", dx_rs.transpose());
- ASSERT_MATRIX_APPROX(S.delta_ , dx_rs, Constants::EPS);
-
- WOLF_DEBUG(" S.D = ", S.delta_integr_.transpose());
- WOLF_DEBUG(" os = ", Dx_os.transpose());
- ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_os, Constants::EPS);
-
- // interpolate!
- t_i = 0.5; /// before ref!
- WOLF_DEBUG("*** INTERPOLATE *** I has been computed; S has changed.");
- I = prc.interpolate(R, S, t_i);
-
- WOLF_DEBUG("* I.d = ", I.delta_.transpose());
- WOLF_DEBUG(" ri = ", prc.deltaZero().transpose());
- ASSERT_MATRIX_APPROX(I.delta_ , prc.deltaZero(), Constants::EPS);
-
- WOLF_DEBUG(" I.D = ", I.delta_integr_.transpose());
- WOLF_DEBUG(" oi = ", Dx_or.transpose());
- ASSERT_MATRIX_APPROX(I.delta_integr_ , Dx_or, Constants::EPS);
-
- WOLF_DEBUG("* S.d = ", S.delta_.transpose());
- WOLF_DEBUG(" is = ", dx_rf.transpose());
- ASSERT_MATRIX_APPROX(S.delta_ , dx_rf, Constants::EPS);
-
- WOLF_DEBUG(" S.D = ", S.delta_integr_.transpose());
- WOLF_DEBUG(" os = ", Dx_of.transpose());
- ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_of, Constants::EPS);
-
- // interpolate!
- t_i = 5.5; /// after ref!
- S = F;
- WOLF_DEBUG("*** INTERPOLATE *** I has been computed; S has changed.");
- I = prc.interpolate(R, S, t_i);
-
- WOLF_DEBUG("* I.d = ", I.delta_.transpose());
- WOLF_DEBUG(" ri = ", dx_rf.transpose());
- ASSERT_MATRIX_APPROX(I.delta_ , dx_rf, Constants::EPS);
-
- WOLF_DEBUG(" I.D = ", I.delta_integr_.transpose());
- WOLF_DEBUG(" oi = ", Dx_of.transpose());
- ASSERT_MATRIX_APPROX(I.delta_integr_ , Dx_of, Constants::EPS);
-
- WOLF_DEBUG("* S.d = ", S.delta_.transpose());
- WOLF_DEBUG(" is = ", prc.deltaZero().transpose());
- ASSERT_MATRIX_APPROX(S.delta_ , prc.deltaZero(), Constants::EPS);
-
- WOLF_DEBUG(" S.D = ", S.delta_integr_.transpose());
- WOLF_DEBUG(" os = ", Dx_of.transpose());
- ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_of, Constants::EPS);
-
-}
-
-
-TEST(ProcessorOdom3D, Interpolate3) // timestamp out of bounds test
-{
- ProcessorOdom3D prc(std::make_shared<ProcessorParamsOdom3D>());
-
- /*
- * We create several poses: origin, ref, int, second, final, as follows:
- *
- * +---+---+---+---->
- * o r i s,f
- *
- * We compute all deltas between them: d_or, d_ri, d_is, d_rf
- * We create the motions R, F
- * We interpolate, and get I, S
- */
-
- // deltas -- referred to previous delta
- // o-r r-i i-s s-f
- VectorXs dx_or(7), dx_ri(7), dx_is(7), dx_if(7), dx_rf(7);
- Map<VectorXs> dx_rs(dx_rf.data(), 7); // this ensures dx_rs = dx_rf
-
- // Deltas -- always referred to origin
- // o-r o-i o-s o-f
- VectorXs Dx_or(7), Dx_oi(7), Dx_os(7), Dx_of(7);
-
- // time stamps and intervals
- TimeStamp t_o(0), t_r(1), t_i, t_f(5); // we'll set t_i later
-
- WOLF_DEBUG("t_o: ", t_o.get(), "; t_r: ", t_r.get(), "; t_f: ", t_f.get());
-
- // Motion structures
- Motion R(0.0,6,7,6,0), I(0.0,6,7,6,0), S(0.0,6,7,6,0), F(0.0,6,7,6,0);
-
-
- /////////// start experiment ///////////////
-
- // set ref and final deltas
- dx_or << 1,2,3, 4,5,6,7; dx_or.tail<4>().normalize();
- dx_rf << 7,6,5, 4,3,2,1; dx_rf.tail<4>().normalize();
- Dx_or = dx_or;
- prc.deltaPlusDelta(Dx_or, dx_rf, t_f - t_r, Dx_of);
- Dx_os = Dx_of;
-
- // set ref
- R.ts_ = t_r;
- R.delta_ = dx_or; // origin to ref
- R.delta_integr_ = Dx_or; // origin to ref
-
- WOLF_DEBUG("* R.d = ", R.delta_.transpose());
- WOLF_DEBUG(" or = ", dx_or.transpose());
- ASSERT_MATRIX_APPROX(R.delta_ , dx_or, Constants::EPS);
-
- WOLF_DEBUG(" R.D = ", R.delta_integr_.transpose());
- WOLF_DEBUG(" or = ", Dx_or.transpose());
- ASSERT_MATRIX_APPROX(R.delta_integr_ , Dx_or, Constants::EPS);
-
- // set final
- F.ts_ = t_f;
- F.delta_ = dx_rf; // ref to final
- F.delta_integr_ = Dx_of; // origin to final
-
- WOLF_DEBUG("* F.d = ", F.delta_.transpose());
- WOLF_DEBUG(" rf = ", dx_rf.transpose());
- ASSERT_MATRIX_APPROX(F.delta_ , dx_rf, Constants::EPS);
-
- WOLF_DEBUG(" F.D = ", F.delta_integr_.transpose());
- WOLF_DEBUG(" of = ", Dx_of.transpose());
- ASSERT_MATRIX_APPROX(F.delta_integr_ , Dx_of, Constants::EPS);
-
- // interpolate!
- t_i = 2; /// 25% between refs!
- WOLF_DEBUG("*** INTERPOLATE *** I and S have been computed.");
- I = prc.interpolate(R, F, t_i, S);
-
-
- prc.deltaPlusDelta(R.delta_integr_, I.delta_, t_i-t_r, Dx_oi);
- ASSERT_MATRIX_APPROX(I.delta_integr_ , Dx_oi, Constants::EPS);
-
- prc.deltaPlusDelta(I.delta_, S.delta_, t_f-t_i, dx_rf);
- ASSERT_MATRIX_APPROX(F.delta_ , dx_rf, Constants::EPS);
-
-}
-
-
int main(int argc, char **argv)