diff --git a/src/imu_tools.h b/src/imu_tools.h
new file mode 100644
index 0000000000000000000000000000000000000000..5c63986221b845d6d128355515686b9ae1b734f9
--- /dev/null
+++ b/src/imu_tools.h
@@ -0,0 +1,242 @@
+/*
+ * imu_tools.h
+ *
+ * Created on: Jul 29, 2017
+ * Author: jsola
+ */
+
+#ifndef IMU_TOOLS_H_
+#define IMU_TOOLS_H_
+
+
+#include "wolf.h"
+#include "rotations.h"
+
+namespace wolf
+{
+namespace imu {
+using namespace Eigen;
+
+template<typename T = wolf::Scalar>
+inline Matrix<T, 10, 1> identity()
+{
+ Matrix<T, 10, 1> ret;
+ ret.setZero();
+ ret(6) = 1.0;
+ return ret;
+}
+
+template<typename D1, typename D2>
+inline void inverse(const MatrixBase<D1>& d,
+ typename D1::Scalar dt,
+ MatrixBase<D2>& id)
+{
+ MatrixSizeCheck<10, 1>::check(d);
+ MatrixSizeCheck<10, 1>::check(id);
+
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dp ( &d( 0 ) );
+ Map<const Quaternion<typename D1::Scalar> > dq ( &d( 3 ) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dv ( &d( 7 ) );
+ Map<Matrix<typename D2::Scalar, 3, 1> > idp ( &id( 0 ));
+ Map<Quaternion<typename D2::Scalar> > idq ( &id( 3 ));
+ Map<Matrix<typename D2::Scalar, 3, 1> > idv ( &id( 7 ));
+
+ idp = - ( dq.conjugate() * (dp - dv * dt) );
+ idv = - ( dq.conjugate() * dv );
+ idq = dq.conjugate();
+}
+
+template<typename D>
+inline Matrix<typename D::Scalar, 10, 1> inverse(const MatrixBase<D>& d,
+ typename D::Scalar dt)
+{
+ Matrix<typename D::Scalar, 10, 1> id;
+
+ inverse(d, dt, id);
+
+ return id;
+}
+
+
+template<typename D1, typename D2, typename D3>
+inline void compose(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ Scalar dt,
+ MatrixBase<D3>& sum)
+{
+ MatrixSizeCheck<10, 1>::check(d1);
+ MatrixSizeCheck<10, 1>::check(d2);
+ MatrixSizeCheck<10, 1>::check(sum);
+
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( &d1( 0 ) );
+ Map<const Quaternion<typename D1::Scalar> > dq1 ( &d1( 3 ) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( &d1( 7 ) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( &d2( 0 ) );
+ Map<const Quaternion<typename D2::Scalar> > dq2 ( &d2( 3 ) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( &d2( 7 ) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > sum_p ( &sum( 0 ));
+ Map<Quaternion<typename D3::Scalar> > sum_q ( &sum( 3 ));
+ Map<Matrix<typename D3::Scalar, 3, 1> > sum_v ( &sum( 7 ));
+
+ sum_p = dp1 + dv1*dt + dq1*dp2;
+ sum_v = dv1 + dq1*dv2;
+ sum_q = dq1*dq2; // dq here to avoid possible aliasing between d1 and sum
+}
+
+template<typename D1, typename D2>
+inline void a() {}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 10, 1> compose(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ Scalar dt)
+{
+ Matrix<typename D1::Scalar, 10, 1> ret;
+ compose(d1, d2, dt, ret);
+ return ret;
+}
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5>
+inline void compose(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ Scalar dt,
+ MatrixBase<D3>& sum,
+ MatrixBase<D4>& J_sum_d1,
+ MatrixBase<D5>& J_sum_d2)
+{
+ MatrixSizeCheck<10, 1>::check(d1);
+ MatrixSizeCheck<10, 1>::check(d2);
+ MatrixSizeCheck<10, 1>::check(sum);
+ MatrixSizeCheck< 9, 1>::check(J_sum_d1);
+ MatrixSizeCheck< 9, 1>::check(J_sum_d2);
+
+ // Maps over provided data
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( &d1( 0 ) );
+ Map<const Quaternion<typename D1::Scalar> > dq1 ( &d1( 3 ) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( &d1( 7 ) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( &d2( 0 ) );
+ Map<const Quaternion<typename D2::Scalar> > dq2 ( &d2( 3 ) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( &d2( 7 ) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > sum_p ( &sum( 0 ));
+ Map<Quaternion<typename D3::Scalar> > sum_q ( &sum( 3 ));
+ Map<Matrix<typename D3::Scalar, 3, 1> > sum_v ( &sum( 7 ));
+
+ // Some useful temporaries
+ Matrix<typename D1::Scalar, 3, 3> dR1 = dq1.matrix(); // First Delta, DR
+ Matrix<typename D2::Scalar, 3, 3> dR2 = dq2.matrix(); // Second delta, dR
+
+ // Jac wrt first delta
+ J_sum_d1.setIdentity(); // dDp'/dDp = dDv'/dDv = I
+ J_sum_d1.block(0,3,3,3).noalias() = - dR1 * skew(dp2) ; // dDp'/dDo
+ J_sum_d1.block(0,6,3,3) = Matrix3s::Identity() * dt; // dDp'/dDv = I*dt
+ J_sum_d1.block(3,3,3,3) = dR2.transpose(); // dDo'/dDo
+ J_sum_d1.block(6,3,3,3).noalias() = - dR1 * skew(dv2) ; // dDv'/dDo
+
+ // Jac wrt second delta
+ J_sum_d2.setIdentity(); //
+ J_sum_d2.block(0,0,3,3) = dR1; // dDp'/ddp
+ J_sum_d2.block(6,6,3,3) = dR1; // dDv'/ddv
+ // J_sum_d2.block(3,3,3,3) = Matrix3s::Identity(); // dDo'/ddo = I
+
+ // compose deltas -- done here to avoid aliasing when calling with `d1` and `sum` pointing to the same variable
+ compose(d1, d2, dt, sum);
+}
+
+template<typename D1, typename D2, typename D3>
+inline void between(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ Scalar dt,
+ MatrixBase<D3>& d2_minus_d1)
+{
+ MatrixSizeCheck<10, 1>::check(d1);
+ MatrixSizeCheck<10, 1>::check(d2);
+ MatrixSizeCheck<10, 1>::check(d2_minus_d1);
+
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( &d1(0) );
+ Map<const Quaternion<typename D1::Scalar> > dq1 ( &d1(3) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( &d1(7) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( &d2(0) );
+ Map<const Quaternion<typename D2::Scalar> > dq2 ( &d2(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( &d2(7) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_p (&d2_minus_d1(0));
+ Map<Quaternion<typename D3::Scalar> > diff_q (&d2_minus_d1(3));
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_v (&d2_minus_d1(7));
+
+ diff_p = dq1.conjugate() * ( dp2 - dp1 - dv1*dt );
+ diff_q = dq1.conjugate() * dq2;
+ diff_v = dq1.conjugate() * ( dv2 - dv1 );
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 10, 1> between(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ Scalar dt)
+{
+ Matrix<typename D1::Scalar, 10, 1> d_bet;
+ between(d1, d2, dt, d_bet);
+ return d_bet;
+}
+
+template<typename Derived>
+Matrix<typename Derived::Scalar, 9, 1> lift(const MatrixBase<Derived>& delta_in)
+{
+ MatrixSizeCheck<10, 1>::check(delta_in);
+
+ Matrix<typename Derived::Scalar, 9, 1> ret;
+
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( &delta_in(0) );
+ Map<const Quaternion<typename Derived::Scalar> > dq_in ( &delta_in(3) );
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > dv_in ( &delta_in(7) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > dp_ret ( & ret(0) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > do_ret ( & ret(3) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > dv_ret ( & ret(6) );
+
+ dp_ret = dp_in;
+ do_ret = log_q(dq_in);
+ dv_ret = dv_in;
+
+ return ret;
+}
+
+template<typename Derived>
+Matrix<typename Derived::Scalar, 10, 1> retract(const MatrixBase<Derived>& d_in)
+{
+ MatrixSizeCheck<9, 1>::check(d_in);
+
+ Matrix<typename Derived::Scalar, 10, 1> ret;
+
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( &d_in(0) );
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > do_in ( &d_in(3) );
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > dv_in ( &d_in(6) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > dp ( & ret(0) );
+ Map<Quaternion<typename Derived::Scalar> > dq ( & ret(3) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > dv ( & ret(7) );
+
+ dp = dp_in;
+ dq = exp_q(do_in);
+ dv = dv_in;
+
+ return ret;
+}
+
+template<typename D1, typename D2, typename D3>
+inline void diff(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ MatrixBase<D3>& err)
+{
+ err = lift( between(d1, d2, 0.0) );
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 9, 1> diff(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2)
+{
+
+ return lift( between(d1, d2, 0.0) );
+}
+
+
+} // namespace imu
+} // namespace wolf
+
+#endif /* IMU_TOOLS_H_ */
diff --git a/src/processor_imu.cpp b/src/processor_imu.cpp
index 0231fa66c13356b290b687610c5641c852b8307f..d4d9b78ee70ee2d0038046e35fa56feed40844ea 100644
--- a/src/processor_imu.cpp
+++ b/src/processor_imu.cpp
@@ -108,6 +108,125 @@ bool ProcessorIMU::voteForKeyFrame()
return false;
}
+Motion ProcessorIMU::interpolate(const Motion& _motion_ref, Motion& _motion_second, TimeStamp& _ts)
+{
+ /* Note: See extensive documentation in ProcessorMotion::interpolate().
+ *
+ * Interpolate between motion_ref and motion, as in:
+ *
+ * motion_ref ------ ts_ ------ motion_sec
+ * return
+ *
+ * and return the motion at the given time_stamp ts_.
+ *
+ * DATA:
+ * Data receives no change
+ *
+ * DELTA:
+ * The delta's position and velocity receive linear interpolation:
+ * p_ret = (ts - t_ref) / dt * (p - p_ref)
+ * v_ret = (ts - t_ref) / dt * (v - v_ref)
+ *
+ * The delta's quaternion receives a slerp interpolation
+ * q_ret = q_ref.slerp( (ts - t_ref) / dt , q);
+ *
+ * DELTA_INTEGR:
+ * The interpolated delta integral is just the reference integral plus the interpolated delta
+ *
+ * The second integral does not change
+ *
+ * Covariances receive linear interpolation
+ * Q_ret = (ts - t_ref) / dt * Q_sec
+ */
+ // 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.data_ = _motion_second.data_;
+ motion_int.data_cov_ = _motion_second.data_cov_;
+ 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");
+
+ // reference
+ TimeStamp t_ref = _motion_ref.ts_;
+
+ // second
+ TimeStamp t_sec = _motion_second.ts_;
+ Map<VectorXs> motion_sec_dp (_motion_second.delta_.data() + 0, 3);
+ Map<Quaternions> motion_sec_dq (_motion_second.delta_.data() + 3 );
+ Map<VectorXs> motion_sec_dv (_motion_second.delta_.data() + 7, 3);
+
+ // interpolated
+ Motion motion_int = motionZero(_ts);
+
+ // Jacobians for covariance propagation
+ MatrixXs J_ref(delta_cov_size_, delta_cov_size_);
+ MatrixXs J_int(delta_cov_size_, delta_cov_size_);
+
+ // interpolation factor
+ Scalar dt1 = _ts - t_ref;
+ Scalar dt2 = t_sec - _ts;
+ Scalar tau = dt1 / (t_sec - t_ref); // interpolation factor (0 to 1)
+ Scalar tau_sq = tau * tau;
+
+ // copy data
+ motion_int.data_ = _motion_second.data_;
+ motion_int.data_cov_ = _motion_second.data_cov_;
+
+ // interpolate delta
+ motion_int.ts_ = _ts;
+ Map<VectorXs> motion_int_dp (motion_int.delta_.data() + 0, 3);
+ Map<Quaternions> motion_int_dq (motion_int.delta_.data() + 3 );
+ Map<VectorXs> motion_int_dv (motion_int.delta_.data() + 7, 3);
+ motion_int_dp = tau_sq * motion_sec_dp; // FIXME: delta_p not correctly interpolated
+ motion_int_dv = tau * motion_sec_dv;
+ motion_int_dq = Quaternions::Identity().slerp(tau, motion_sec_dq);
+ motion_int.delta_cov_ = tau * _motion_second.delta_cov_;
+
+ // integrate
+ deltaPlusDelta(_motion_ref.delta_integr_, motion_int.delta_, dt1, motion_int.delta_integr_, J_ref, J_int);
+ 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 )
+ motion_sec_dp = motion_int_dq.conjugate() * (motion_sec_dp - motion_int_dp - motion_int_dv * dt2);
+ motion_sec_dv = motion_int_dq.conjugate() * (motion_sec_dv - motion_int_dv);
+ motion_sec_dq = motion_int_dq.conjugate() * motion_sec_dq;
+ _motion_second.delta_cov_ *= (1 - tau); // 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;
+}
+
} // namespace wolf
diff --git a/src/processor_imu.h b/src/processor_imu.h
index be263aaa86c3afd8c25ba0f62fb7832705c40d9f..55109586c593c607c7d8fcafc699f58f05a5306d 100644
--- a/src/processor_imu.h
+++ b/src/processor_imu.h
@@ -406,102 +406,6 @@ inline Eigen::VectorXs ProcessorIMU::deltaZero() const
return (Eigen::VectorXs(10) << 0,0,0, 0,0,0,1, 0,0,0 ).finished(); // p, q, v
}
-inline Motion ProcessorIMU::interpolate(const Motion& _motion_ref,
- Motion& _motion_second,
- TimeStamp& _ts)
-{
- // 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");
-
- // 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 and velocity receive linear interpolation:
- // p_ret = (ts - t_ref) / dt * (p - p_ref)
- // v_ret = (ts - t_ref) / dt * (v - v_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);
- Map<VectorXs> dv_sec(_motion_second.delta_.data() + 7, 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);
- Map<VectorXs> dv_int(motion_int.delta_.data() + 7, 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);
- dv_int = tau * dv_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;
- dv_sec = dq_int.conjugate() * ((1 - tau) * dv_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;
-}
-
-
inline void ProcessorIMU::resetDerived()
{
// Cast a pointer to origin IMU frame
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index 095e5e65c238f577a779acc05d04afc681324e8f..e17c407c9f6a127d2f2ad73dd8d3e7751a8f918a 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -318,7 +318,7 @@ void ProcessorMotion::integrateOneStep()
+ jacobian_delta_ * delta_cov_ * jacobian_delta_.transpose();
// push all into buffer
- getBuffer().get().push_back(Motion(incoming_ptr_->getTimeStamp(),
+ getBuffer().get().emplace_back(incoming_ptr_->getTimeStamp(),
incoming_ptr_->getData(),
incoming_ptr_->getDataCovariance(),
delta_,
@@ -327,7 +327,7 @@ void ProcessorMotion::integrateOneStep()
delta_integrated_cov_,
jacobian_delta_,
jacobian_delta_preint_,
- jacobian_calib_));
+ jacobian_calib_);
}
void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
@@ -345,6 +345,7 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
const Scalar dt = motion_it->ts_ - prev_motion_it->ts_;
// re-convert data to delta with the new calibration parameters
+ // FIXME: Get calibration params from Capture or capture->Sensor
VectorXs calib = getCalibration();
data2delta(motion_it->data_, motion_it->data_cov_, dt, motion_it->delta_, motion_it->delta_cov_, calib, jacobian_delta_calib_);
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index 8ebe7bc40edfa84212d80720387393a8dd6df258..7862208791bea5f7cd86d3594da70b3458e60424 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -6,14 +6,20 @@ namespace wolf {
unsigned int SensorBase::sensor_id_count_ = 0;
-SensorBase::SensorBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr,
- const unsigned int _noise_size, const bool _extr_dyn) :
+SensorBase::SensorBase(const std::string& _type,
+ StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr,
+ StateBlockPtr _intr_ptr,
+ const unsigned int _noise_size,
+ const bool _extr_dyn,
+ const bool _intr_dyn) :
NodeBase("SENSOR", _type),
hardware_ptr_(),
state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
is_removing_(false),
sensor_id_(++sensor_id_count_), // simple ID factory
extrinsic_dynamic_(_extr_dyn),
+ intrinsic_dynamic_(_intr_dyn),
noise_std_(_noise_size),
noise_cov_(_noise_size, _noise_size)
{
@@ -24,14 +30,20 @@ SensorBase::SensorBase(const std::string& _type, StateBlockPtr _p_ptr, StateBloc
//
}
-SensorBase::SensorBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr,
- const Eigen::VectorXs & _noise_std, const bool _extr_dyn) :
+SensorBase::SensorBase(const std::string& _type,
+ StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr,
+ StateBlockPtr _intr_ptr,
+ const Eigen::VectorXs & _noise_std,
+ const bool _extr_dyn,
+ const bool _intr_dyn) :
NodeBase("SENSOR", _type),
hardware_ptr_(),
state_block_vec_(6), // allow for 3 state blocks by default. Resize in derived constructors if needed.
is_removing_(false),
sensor_id_(++sensor_id_count_), // simple ID factory
extrinsic_dynamic_(_extr_dyn),
+ intrinsic_dynamic_(_intr_dyn),
noise_std_(_noise_std),
noise_cov_(_noise_std.size(), _noise_std.size())
{
diff --git a/src/sensor_base.h b/src/sensor_base.h
index 71c38883217b46077f894a3d4b00d2c4ea459f45..9b8bd4e6fbd02a545b66fc4868cdc85ed995465f 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -41,6 +41,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
unsigned int sensor_id_; // sensor ID
bool extrinsic_dynamic_;// extrinsic parameters vary with time? If so, they will be taken from the Capture nodes. TODO: Not Yet Implemented.
+ bool intrinsic_dynamic_;// intrinsic parameters vary with time? If so, they will be taken from the Capture nodes. TODO: Not Yet Implemented.
Eigen::VectorXs noise_std_; // std of sensor noise
Eigen::MatrixXs noise_cov_; // cov matrix of noise
@@ -57,7 +58,13 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
* \param _extr_dyn Flag indicating if extrinsics are dynamic (moving) or static (not moving)
*
**/
- SensorBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr, const unsigned int _noise_size, const bool _extr_dyn = false);
+ SensorBase(const std::string& _type,
+ StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr,
+ StateBlockPtr _intr_ptr,
+ const unsigned int _noise_size,
+ const bool _extr_dyn = false,
+ const bool _intr_dyn = false);
/** \brief Constructor with noise std vector
*
@@ -70,7 +77,14 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
* \param _extr_dyn Flag indicating if extrinsics are dynamic (moving) or static (not moving)
*
**/
- SensorBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr, const Eigen::VectorXs & _noise_std, const bool _extr_dyn = false);
+ SensorBase(const std::string& _type,
+ StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr,
+ StateBlockPtr _intr_ptr,
+ const Eigen::VectorXs & _noise_std,
+ const bool _extr_dyn = false,
+ const bool _intr_dyn = false);
+
virtual ~SensorBase();
void remove();
diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index 1a857125ddc92d8044dc2cd3fbb06473bb3bf6a3..0b01f569fcfc9c6640ee5f81e6fe6a8d844f7c98 100644
--- a/src/test/CMakeLists.txt
+++ b/src/test/CMakeLists.txt
@@ -56,9 +56,9 @@ target_link_libraries(gtest_feature_base ${PROJECT_NAME})
wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp)
target_link_libraries(gtest_frame_base ${PROJECT_NAME})
-# FrameIMU class test
-wolf_add_gtest(gtest_frame_imu gtest_frame_imu.cpp)
-target_link_libraries(gtest_frame_imu ${PROJECT_NAME})
+# IMU tools test
+wolf_add_gtest(gtest_imu_tools gtest_imu_tools.cpp)
+# target_link_libraries(gtest_imu_tools ${PROJECT_NAME}) // WOLF library not needed
# LocalParametrizationXxx classes test
wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
@@ -106,6 +106,10 @@ target_link_libraries(gtest_constraint_odom_3D ${PROJECT_NAME})
wolf_add_gtest(gtest_feature_imu gtest_feature_imu.cpp)
target_link_libraries(gtest_feature_imu ${PROJECT_NAME})
+# FrameIMU class test
+wolf_add_gtest(gtest_frame_imu gtest_frame_imu.cpp)
+target_link_libraries(gtest_frame_imu ${PROJECT_NAME})
+
# IMU Bias + estimation tests
#wolf_add_gtest(gtest_imuBias gtest_imuBias.cpp)
#target_link_libraries(gtest_imuBias ${PROJECT_NAME})
diff --git a/src/test/gtest_imu_tools.cpp b/src/test/gtest_imu_tools.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a26cac216d9ac58c7c3d1c566f06507044daf868
--- /dev/null
+++ b/src/test/gtest_imu_tools.cpp
@@ -0,0 +1,123 @@
+/*
+ * gtest_imu_tools.cpp
+ *
+ * Created on: Jul 29, 2017
+ * Author: jsola
+ */
+
+#include "imu_tools.h"
+
+#include "utils_gtest.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+
+TEST(IMU_tools, identity)
+{
+ VectorXs id_true;
+ id_true.setZero(10);
+ id_true(6) = 1.0;
+
+ VectorXs id = imu::identity<Scalar>();
+ ASSERT_MATRIX_APPROX(id, id_true, 1e-10);
+}
+
+TEST(IMU_tools, inverse)
+{
+ VectorXs d(10), id(10), iid(10), iiid(10), I(10);
+ Vector4s qv;
+ Scalar dt = 0.1;
+
+ qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+ d << 0, 1, 2, qv, 7, 8, 9;
+
+ id = imu::inverse(d, dt);
+
+ imu::compose(id, d, dt, I);
+ ASSERT_MATRIX_APPROX(I, imu::identity(), 1e-10);
+ imu::compose(d, id, -dt, I); // Observe -dt is reversed !!
+ ASSERT_MATRIX_APPROX(I, imu::identity(), 1e-10);
+
+ imu::inverse(id, -dt, iid); // Observe -dt is reversed !!
+ ASSERT_MATRIX_APPROX( d, iid, 1e-10);
+ iiid = imu::inverse(iid, dt);
+ ASSERT_MATRIX_APPROX(id, iiid, 1e-10);
+}
+
+TEST(IMU_tools, compose_between)
+{
+ VectorXs dx1(10), dx2(10), dx3(10);
+ Matrix<Scalar, 10, 1> d1, d2, d3;
+ Vector4s qv;
+ Scalar dt = 0.1;
+
+ qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+ dx1 << 0, 1, 2, qv, 7, 8, 9;
+ d1 = dx1;
+ qv = (Vector4s() << 6, 5, 4, 3).finished().normalized();
+ dx2 << 9, 8, 7, qv, 2, 1, 0;
+ d2 = dx2;
+
+ // combinations of templates for sum()
+ imu::compose(dx1, dx2, dt, dx3);
+ imu::compose(d1, d2, dt, d3);
+ ASSERT_MATRIX_APPROX(d3, dx3, 1e-10);
+
+ imu::compose(d1, dx2, dt, dx3);
+ d3 = imu::compose(dx1, d2, dt);
+ ASSERT_MATRIX_APPROX(d3, dx3, 1e-10);
+
+ // minus(d1, d3) should recover delta_2
+ VectorXs diffx(10);
+ Matrix<Scalar,10,1> diff;
+ imu::between(d1, d3, dt, diff);
+ ASSERT_MATRIX_APPROX(diff, d2, 1e-10);
+
+ // minus(d3, d1, -dt) should recover inverse(d2, dt)
+ diff = imu::between(d3, d1, -dt);
+ ASSERT_MATRIX_APPROX(diff, imu::inverse(d2, dt), 1e-10);
+}
+
+TEST(IMU_tools, lift_retract)
+{
+ VectorXs d_min(9); d_min << 0, 1, 2, .3, .4, .5, 6, 7, 8; // use angles in the ball theta < pi
+
+ // transform to delta
+ VectorXs delta = imu::retract(d_min);
+
+ // expected delta
+ Vector3s dp = d_min.head(3);
+ Quaternions dq = v2q(d_min.segment(3,3));
+ Vector3s dv = d_min.tail(3);
+ VectorXs delta_true(10); delta_true << dp, dq.coeffs(), dv;
+ ASSERT_MATRIX_APPROX(delta, delta_true, 1e-10);
+
+ // transform to a new tangent -- should be the original tangent
+ VectorXs d_from_delta = imu::lift(delta);
+ ASSERT_MATRIX_APPROX(d_from_delta, d_min, 1e-10);
+
+ // transform to a new delta -- should be the previous delta
+ VectorXs delta_from_d = imu::retract(d_from_delta);
+ ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10);
+}
+
+TEST(IMU_tools, diff)
+{
+ VectorXs d1(10), d2(10);
+ Vector4s qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+ d1 << 0, 1, 2, qv, 7, 8, 9;
+ d2 = d1;
+
+ VectorXs err(9);
+ imu::diff(d1, d2, err);
+ ASSERT_MATRIX_APPROX(err, VectorXs::Zero(9), 1e-10);
+ ASSERT_MATRIX_APPROX(imu::diff(d1, d2), VectorXs::Zero(9), 1e-10);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/src/test/gtest_processor_imu.cpp b/src/test/gtest_processor_imu.cpp
index 0532a63e8839ca362a6b60e23794af77d81bd02a..c81c2e63088444b40917276da8550de664a02120 100644
--- a/src/test/gtest_processor_imu.cpp
+++ b/src/test/gtest_processor_imu.cpp
@@ -206,6 +206,54 @@ TEST(ProcessorIMU, voteForKeyFrame)
}
//replace TEST by TEST_F if SetUp() needed
+TEST_F(ProcessorIMUt, interpolate)
+{
+ using namespace wolf;
+
+ t.set(0);
+ x0 << 0,0,0, 0,0,0,1, 0,0,0, 0,0,0, 0,0,0; // Try some non-zero biases
+
+ problem->getProcessorMotionPtr()->setOrigin(x0, t);
+
+ data << 2, 0, 0, 0, 0, 0; // only acc_x
+ cap_imu_ptr->setData(data);
+
+ // make one step to depart from origin
+ cap_imu_ptr->setTimeStamp(0.05);
+ sensor_ptr->process(cap_imu_ptr);
+ Motion mot_ref = problem->getProcessorMotionPtr()->getMotion();
+
+ // make two steps, then pretend it's just one
+ cap_imu_ptr->setTimeStamp(0.10);
+ sensor_ptr->process(cap_imu_ptr);
+ Motion mot_int_gt = problem->getProcessorMotionPtr()->getMotion();
+
+ cap_imu_ptr->setTimeStamp(0.15);
+ sensor_ptr->process(cap_imu_ptr);
+ Motion mot_final = problem->getProcessorMotionPtr()->getMotion();
+ mot_final.delta_ = mot_final.delta_integr_;
+ Motion mot_sec = mot_final;
+
+ problem->getProcessorMotionPtr()->getBuffer().print(0,1,1,0);
+
+class P : wolf::ProcessorIMU
+{
+ public:
+ Motion interp(const Motion& ref, Motion& sec, TimeStamp ts)
+ {
+ return ProcessorIMU::interpolate(ref, sec, ts);
+ }
+} imu;
+
+Motion mot_int = imu.interp(mot_ref, mot_sec, TimeStamp(0.10));
+
+ASSERT_MATRIX_APPROX(mot_int.data_, mot_int_gt.data_, 1e-6);
+//ASSERT_MATRIX_APPROX(mot_int.delta_, mot_int_gt.delta_, 1e-6); // FIXME: delta_p not correctly interpolated
+ASSERT_MATRIX_APPROX(mot_final.delta_integr_, mot_sec.delta_integr_, 1e-6);
+
+
+}
+
TEST_F(ProcessorIMUt, acc_x)
{
t.set(0); // clock in 0,1 ms ticks