Skip to content
Snippets Groups Projects
Commit 46db3bfd authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Comment out all interpolate()

parent 71b3335d
No related branches found
No related tags found
1 merge request!315Resolve "Remove all methods `ProcessorMotion::interpolate(...)`"
Pipeline #4231 passed
......@@ -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
......
......@@ -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,
......
......@@ -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,
......
......@@ -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
{
......
......@@ -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()
{
......
......@@ -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;
//}
......
......@@ -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)
{
......
......@@ -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);
//
//}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment