diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index cd1d7972d274dc6bafafa3227af7b27c11f76ebd..81df227434c641cfa120855a242a98b918805e50 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -402,39 +402,39 @@ 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 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..33833bbee6ac5240295c50fabcde3746fba43845 100644 --- a/include/core/processor/processor_odom_2D.h +++ b/include/core/processor/processor_odom_2D.h @@ -72,10 +72,10 @@ 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 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..36125c4ea6b2ed64c3fffaf2baf6f4b4c832ab32 100644 --- a/include/core/processor/processor_odom_3D.h +++ b/include/core/processor/processor_odom_3D.h @@ -91,13 +91,13 @@ 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; +// 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..06d9d37f2b51e47472acd44bd6641105d30e438e 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -452,138 +452,138 @@ 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; - // } - -} +//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 { diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp index 96241bd069a64221ecce0ce86b5481f7e675ae1f..6c2a3f4192ac41cf0543b1d1a9b952351e78c572 100644 --- a/src/processor/processor_odom_2D.cpp +++ b/src/processor/processor_odom_2D.cpp @@ -92,10 +92,10 @@ 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); -} +//Motion ProcessorOdom2D::interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second) +//{ +// return ProcessorMotion::interpolate(_ref1, _ref2, _ts, _second); +//} bool ProcessorOdom2D::voteForKeyFrame() { diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp index 204ecd9d54d4dbd4f359e51b93553cf95baa7436..7a481e995c244180e586a52a08a84cea2fd767d5 100644 --- a/src/processor/processor_odom_3D.cpp +++ b/src/processor/processor_odom_3D.cpp @@ -147,190 +147,190 @@ 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()); +//Motion ProcessorOdom3D::interpolate(const Motion& _motion_ref, +// Motion& _motion_second, +// TimeStamp& _ts) +//{ // -// 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; -} +//// 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; +//} diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp index e508069f55ffeeffcc7ab28f00af7341f2dd9c9b..7e27fb1758188d4b9038d94c733788f94b92c3a6 100644 --- a/test/gtest_processor_motion.cpp +++ b/test/gtest_processor_motion.cpp @@ -55,15 +55,15 @@ 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); - } +// 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,92 +108,92 @@ TEST_F(ProcessorMotion_test, IntegrateCircle) ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3s()<<0,0,0).finished(), 1e-8); } -TEST_F(ProcessorMotion_test, Interpolate) -{ - 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->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); -} - -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); +//TEST_F(ProcessorMotion_test, Interpolate) +//{ +// 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->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); - - 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); +// +// 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); - - 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); -} +//} +// +//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..df554557cc80013231a7e88cc31c22cfe403cccf 100644 --- a/test/gtest_processor_odom_3D.cpp +++ b/test/gtest_processor_odom_3D.cpp @@ -151,423 +151,423 @@ 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); - -} +//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); +// +//}