diff --git a/src/IMU_tools.h b/src/IMU_tools.h
index 4f6ecda67b6695723b11ddecc56403da57a931a0..6d231223554ec4ea5e196cb7220a40e0eb42c001 100644
--- a/src/IMU_tools.h
+++ b/src/IMU_tools.h
@@ -34,10 +34,10 @@
* - between: Db = D2 (-) D1, so that D2 = D1 (+) Db
* - composeOverState: x2 = x1 (+) D
* - betweenStates: D = x2 (-) x1, so that x2 = x1 (+) D
- * - lift: got from Delta manifold to tangent space (equivalent to log() in rotations)
- * - retract: go from tangent space to delta manifold (equivalent to exp() in rotations)
- * - plus: D2 = D1 (+) retract(d)
- * - diff: d = lift( D2 (-) D1 )
+ * - log: got from Delta manifold to tangent space (equivalent to log() in rotations)
+ * - exp_IMU: go from tangent space to delta manifold (equivalent to exp() in rotations)
+ * - plus: D2 = D1 (+) exp_IMU(d)
+ * - diff: d = log_IMU( D2 (-) D1 )
* - body2delta: construct a delta from body magnitudes of linAcc and angVel
*/
@@ -345,7 +345,7 @@ inline Matrix<typename D1::Scalar, 10, 1> betweenStates(const MatrixBase<D1>& x1
}
template<typename Derived>
-Matrix<typename Derived::Scalar, 9, 1> lift(const MatrixBase<Derived>& delta_in)
+Matrix<typename Derived::Scalar, 9, 1> log_IMU(const MatrixBase<Derived>& delta_in)
{
MatrixSizeCheck<10, 1>::check(delta_in);
@@ -366,7 +366,7 @@ Matrix<typename Derived::Scalar, 9, 1> lift(const MatrixBase<Derived>& delta_in)
}
template<typename Derived>
-Matrix<typename Derived::Scalar, 10, 1> retract(const MatrixBase<Derived>& d_in)
+Matrix<typename Derived::Scalar, 10, 1> exp_IMU(const MatrixBase<Derived>& d_in)
{
MatrixSizeCheck<9, 1>::check(d_in);
diff --git a/src/three_D_tools.h b/src/SE3.h
similarity index 70%
rename from src/three_D_tools.h
rename to src/SE3.h
index 73ec2d413475ccd8ca82a4b9db14f84787d38509..cecd71757360a0bee2d36d50bbf225eda914e606 100644
--- a/src/three_D_tools.h
+++ b/src/SE3.h
@@ -1,12 +1,12 @@
/*
- * three_D_tools.h
+ * SE3.h
*
* Created on: Mar 15, 2018
* Author: jsola
*/
-#ifndef THREE_D_TOOLS_H_
-#define THREE_D_TOOLS_H_
+#ifndef SE3_H_
+#define SE3_H_
#include "wolf.h"
@@ -27,10 +27,11 @@
* - identity: I = Delta at the origin, with Dp = [0,0,0]; Dq = [0,0,0,1], so that D (+) I = I (+) D = D
* - inverse: so that D (+) D.inv = D.inv (+) D = I
* - between: Db = D2 (-) D1 := D1.inv (+) D2, so that D2 = D1 (+) Db
- * - lift: go from Delta manifold to tangent space (equivalent to log() in rotations)
- * - retract: go from tangent space to delta manifold (equivalent to exp() in rotations)
- * - plus: D2 = D1 (+) retract(d)
- * - diff: d = lift( D2 (-) D1 )
+ * - log_SE3: go from Delta manifold to tangent space (equivalent to log() in rotations)
+ * - exp_SE3: go from tangent space to delta manifold (equivalent to exp() in rotations)
+ * - plus: D2 = D1 * exp_SE3(d)
+ * - minus: d = log_SE3( D1.inv() * D2 )
+ * - interpolate: dd = D1 * exp ( log( D1.inv() * D2 ) * t ) = D1 (+) ( (D2 (-) D1) * t)
*/
@@ -220,7 +221,7 @@ inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1,
}
template<typename Derived>
-Matrix<typename Derived::Scalar, 6, 1> lift(const MatrixBase<Derived>& delta_in)
+Matrix<typename Derived::Scalar, 6, 1> log_SE3(const MatrixBase<Derived>& delta_in)
{
MatrixSizeCheck<7, 1>::check(delta_in);
@@ -231,25 +232,32 @@ Matrix<typename Derived::Scalar, 6, 1> lift(const MatrixBase<Derived>& delta_in)
Map<Matrix<typename Derived::Scalar, 3, 1> > dp_ret ( & ret(0) );
Map<Matrix<typename Derived::Scalar, 3, 1> > do_ret ( & ret(3) );
- dp_ret = dp_in;
- do_ret = log_q(dq_in);
+ Matrix<typename Derived::Scalar, 3, 3> V_inv;
+
+ do_ret = log_q(dq_in);
+ V_inv = jac_SO3_left_inv(do_ret);
+ dp_ret = V_inv * dp_in;
return ret;
}
template<typename Derived>
-Matrix<typename Derived::Scalar, 7, 1> retract(const MatrixBase<Derived>& d_in)
+Matrix<typename Derived::Scalar, 7, 1> exp_SE3(const MatrixBase<Derived>& d_in)
{
MatrixSizeCheck<6, 1>::check(d_in);
Matrix<typename Derived::Scalar, 7, 1> ret;
+ Matrix<typename Derived::Scalar, 3, 3> V;
+
+ V = jac_SO3_left(d_in.template tail<3>());
+
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<Matrix<typename Derived::Scalar, 3, 1> > dp ( & ret(0) );
Map<Quaternion<typename Derived::Scalar> > dq ( & ret(3) );
- dp = dp_in;
+ dp = V * dp_in;
dq = exp_q(do_in);
return ret;
@@ -292,21 +300,21 @@ inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1,
}
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
- MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o )
+inline void minus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
+ const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
+ MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o )
{
- diff_p = dp2 - dp1;
- diff_o = log_q(dq1.conjugate() * dq2);
+ diff_p = dp2 - dp1;
+ diff_o = log_q(dq1.conjugate() * dq2);
}
template<typename D1, typename D2, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
-inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
- MatrixBase<D6>& diff_p, MatrixBase<D7>& diff_o,
- MatrixBase<D8>& J_do_dq1, MatrixBase<D9>& J_do_dq2)
+inline void minus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
+ const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
+ MatrixBase<D6>& diff_p, MatrixBase<D7>& diff_o,
+ MatrixBase<D8>& J_do_dq1, MatrixBase<D9>& J_do_dq2)
{
- diff(dp1, dq1, dp2, dq2, diff_p, diff_o);
+ minus(dp1, dq1, dp2, dq2, diff_p, diff_o);
J_do_dq1 = - jac_SO3_left_inv(diff_o);
J_do_dq2 = jac_SO3_right_inv(diff_o);
@@ -314,9 +322,9 @@ inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
template<typename D1, typename D2, typename D3>
-inline void diff(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& err)
+inline void minus(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ MatrixBase<D3>& err)
{
Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
@@ -325,15 +333,15 @@ inline void diff(const MatrixBase<D1>& d1,
Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & err(0) );
Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & err(3) );
- diff(dp1, dq1, dp2, dq2, diff_p, diff_o);
+ minus(dp1, dq1, dp2, dq2, diff_p, diff_o);
}
template<typename D1, typename D2, typename D3, typename D4, typename D5>
-inline void diff(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& dif,
- MatrixBase<D4>& J_diff_d1,
- MatrixBase<D5>& J_diff_d2)
+inline void minus(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ MatrixBase<D3>& dif,
+ MatrixBase<D4>& J_diff_d1,
+ MatrixBase<D5>& J_diff_d2)
{
Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
@@ -347,9 +355,9 @@ inline void diff(const MatrixBase<D1>& d1,
Matrix<typename D4::Scalar, 3, 3> J_do_dq1, J_do_dq2;
- diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2);
+ minus(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2);
- /* d = diff(d1, d2) is
+ /* d = minus(d1, d2) is
* dp = dp2 - dp1
* do = Log(dq1.conj * dq2)
* dv = dv2 - dv1
@@ -367,17 +375,111 @@ inline void diff(const MatrixBase<D1>& d1,
}
template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 6, 1> diff(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2)
+inline Matrix<typename D1::Scalar, 6, 1> minus(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2)
{
Matrix<typename D1::Scalar, 6, 1> ret;
- diff(d1, d2, ret);
+ minus(d1, d2, ret);
return ret;
}
+template<typename D1, typename D2, typename D3>
+inline void interpolate(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ const typename D1::Scalar& t,
+ MatrixBase<D3>& ret)
+{
+ Matrix<typename D1::Scalar, 7, 1> dd = between(d1, d2);
+
+ Matrix<typename D1::Scalar, 6, 1> tau = t * log_SE3(dd);
+
+ ret = compose(d1, exp_SE3(tau));
+}
+
+template<typename D1, typename D2>
+inline void toSE3(const MatrixBase<D1>& pose,
+ MatrixBase<D2>& SE3)
+{
+ MatrixSizeCheck<4,4>::check(SE3);
+
+ typedef typename D1::Scalar T;
+
+ SE3.template block<3,1>(0,3) = pose.template head<3>();
+ SE3.template block<3,3>(0,0) = q2R(pose.template tail<4>());
+ SE3.template block<1,3>(3,0).setZero();
+ SE3(3,3) = (T)1.0;
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 6, 6> Q_helper(const MatrixBase<D1>& v, const MatrixBase<D1>& w)
+{
+ typedef typename D1::Scalar T;
+
+ Matrix<T, 3, 3> V = skew(v);
+ Matrix<T, 3, 3> W = skew(w);
+ Matrix<T, 3, 3> VW = V * W;
+ Matrix<T, 3, 3> WV = VW.transpose(); // Note on this change wrt. Barfoot: it happens that V*W = (W*V).transpose() !!!
+ Matrix<T, 3, 3> WVW = WV * W;
+ Matrix<T, 3, 3> VWW = VW * W;
+
+ T th_2 = w.squaredNorm();
+
+ T A(T(0.5)), B, C, D;
+
+ // Small angle approximation
+ if (th_2 <= T(1e-8))
+ {
+ B = Scalar(1./6.) + Scalar(1./120.) * th_2;
+ C = -Scalar(1./24.) + Scalar(1./720.) * th_2;
+ D = -Scalar(1./60.);
+ }
+ else
+ {
+ T th = sqrt(th_2);
+ T th_3 = th_2*th;
+ T th_4 = th_2*th_2;
+ T th_5 = th_3*th_2;
+ T sin_th = sin(th);
+ T cos_th = cos(th);
+ B = (th-sin_th)/th_3;
+ C = (T(1.0) - th_2/T(2.0) - cos_th)/th_4;
+ D = (th - sin_th - th_3/T(6.0))/th_5;
+ }
+ Matrix<T, 3, 3> Q;
+ Q.noalias() =
+ + A * V
+ + B * (WV + VW + WVW)
+ - C * (VWW - VWW.transpose() - Scalar(3) * WVW) // Note on this change wrt. Barfoot: it happens that V*W*W = -(W*W*V).transpose() !!!
+ - D * WVW * W; // Note on this change wrt. Barfoot: it happens that W*V*W*W = W*W*V*W !!!
+
+ return Q;
+}
+
+template<typename D1>
+inline Matrix<typename D1::Scalar, 6, 6> jac_SE3_left(const MatrixBase<D1>& tangent)
+{
+ typedef typename D1::Scalar T;
+ Map<Matrix<T, 3, 1>> v(tangent.data() + 0); // linear
+ Map<Matrix<T, 3, 1>> w(tangent.data() + 3); // angular
+
+ Matrix<T, 3, 3> Jl(jac_SO3_left(w));
+ Matrix<T, 3, 3> Q = Q_helper(v,w);
+
+ Matrix<T, 6, 6> Jl_SE3;
+ Jl_SE3.topLeftCorner(3,3) = Jl;
+ Jl_SE3.bottomRightCorner(3,3) = Jl;
+ Jl_SE3.topRightCorner(3,3) = Q;
+ Jl_SE3.bottomLeftCorner(3,3) .setZero();
+}
+
+template<typename D1>
+inline Matrix<typename D1::Scalar, 6, 6> jac_SE3_right(const MatrixBase<D1>& tangent)
+{
+ return jac_SE3_left(-tangent);
+}
} // namespace three_d
} // namespace wolf
-#endif /* THREE_D_TOOLS_H_ */
+#endif /* SE3_H_ */
diff --git a/src/ceres_wrapper/ceres_manager.h b/src/ceres_wrapper/ceres_manager.h
index efd9861bf380710ea027a06570f08910107a397a..8852bc06d3a0fe24fa2b760aa3766cb2e24cbf66 100644
--- a/src/ceres_wrapper/ceres_manager.h
+++ b/src/ceres_wrapper/ceres_manager.h
@@ -53,13 +53,13 @@ class CeresManager : public SolverManager
virtual void computeCovariances(const StateBlockList& st_list) override;
- virtual bool hasConverged();
+ virtual bool hasConverged() override;
- virtual SizeStd iterations();
+ virtual SizeStd iterations() override;
- virtual Scalar initialCost();
+ virtual Scalar initialCost() override;
- virtual Scalar finalCost();
+ virtual Scalar finalCost() override;
ceres::Solver::Options& getSolverOptions();
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index 08deb800317224f415bbb0b589f24967c4ad2716..0abab214309a12779c0d916ac559ca4afbbd1339 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -26,7 +26,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
delta_cov_(_delta_cov_size, _delta_cov_size),
delta_integrated_(_delta_size),
delta_integrated_cov_(_delta_cov_size, _delta_cov_size),
- calib_(_calib_size),
+ calib_preint_(_calib_size),
jacobian_delta_preint_(delta_cov_size_, delta_cov_size_),
jacobian_delta_(delta_cov_size_, delta_cov_size_),
jacobian_calib_(delta_size_, calib_size_)
@@ -248,7 +248,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
getProblem()->keyFrameCallback(key_frame_ptr, shared_from_this(), params_motion_->time_tolerance);
}
- resetDerived(); // TODO see where to put this
+ resetDerived();
// clear incoming just in case
incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
@@ -274,7 +274,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
CaptureBasePtr cap_orig = capture_motion->getOriginFramePtr()->getCaptureOf(getSensorPtr());
VectorXs calib = cap_orig->getCalibration();
- // Get delta and correct it with new bias
+ // Get delta and correct it with new calibration params
VectorXs calib_preint = capture_motion->getBuffer().getCalibrationPreint();
Motion motion = capture_motion->getBuffer().getMotion(_ts);
@@ -296,35 +296,6 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
return true;
}
-//CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
-//{
-// //std::cout << "ProcessorMotion::findCaptureContainingTimeStamp: ts = " << _ts.getSeconds() << "." << _ts.getNanoSeconds() << std::endl;
-// CaptureMotionPtr capture_ptr = last_ptr_;
-// while (capture_ptr != nullptr)
-// {
-// // capture containing motion previous than the ts found:
-// if (capture_ptr->getBuffer().get().front().ts_ < _ts)
-// return capture_ptr;
-// else
-// {
-// // go to the previous motion capture
-// if (capture_ptr == last_ptr_)
-// capture_ptr = origin_ptr_;
-// else if (capture_ptr->getOriginFramePtr() == nullptr)
-// return nullptr;
-// else
-// {
-// CaptureBasePtr capture_base_ptr = capture_ptr->getOriginFramePtr()->getCaptureOf(getSensorPtr());
-// if (capture_base_ptr == nullptr)
-// return nullptr;
-// else
-// capture_ptr = std::static_pointer_cast<CaptureMotion>(capture_base_ptr);
-// }
-// }
-// }
-// return capture_ptr;
-//}
-
FrameBasePtr ProcessorMotion::setOrigin(const Eigen::VectorXs& _x_origin, const TimeStamp& _ts_origin)
{
FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(KEY_FRAME, _x_origin, _ts_origin);
@@ -387,12 +358,12 @@ void ProcessorMotion::integrateOneStep()
assert(dt_ >= 0 && "Time interval _dt is negative!");
// get vector of parameters to calibrate
- calib_ = getBuffer().getCalibrationPreint();
+ calib_preint_ = getBuffer().getCalibrationPreint();
// get data and convert it to delta, and obtain also the delta covariance
computeCurrentDelta(incoming_ptr_->getData(),
incoming_ptr_->getDataCovariance(),
- calib_,
+ calib_preint_,
dt_,
delta_,
delta_cov_,
@@ -408,11 +379,14 @@ void ProcessorMotion::integrateOneStep()
// integrate Jacobian wrt calib
if ( hasCalibration() )
- jacobian_calib_ = jacobian_delta_preint_ * getBuffer().get().back().jacobian_calib_ + jacobian_delta_ * jacobian_delta_calib_;
+ jacobian_calib_.noalias()
+ = jacobian_delta_preint_ * getBuffer().get().back().jacobian_calib_
+ + jacobian_delta_ * jacobian_delta_calib_;
// Integrate covariance
- delta_integrated_cov_ = jacobian_delta_preint_ * getBuffer().get().back().delta_integr_cov_ * jacobian_delta_preint_.transpose()
- + jacobian_delta_ * delta_cov_ * jacobian_delta_.transpose();
+ delta_integrated_cov_.noalias()
+ = jacobian_delta_preint_ * getBuffer().get().back().delta_integr_cov_ * jacobian_delta_preint_.transpose()
+ + jacobian_delta_ * delta_cov_ * jacobian_delta_.transpose();
// push all into buffer
getBuffer().get().emplace_back(incoming_ptr_->getTimeStamp(),
@@ -501,8 +475,53 @@ Motion ProcessorMotion::interpolate(const Motion& _ref, Motion& _second, TimeSta
else
{
// _ts is closest to _second
- Motion interpolated ( _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_);
+
+ if (tau < 0.5)
+ {
+ // _ts is closest to _ref1
+ Motion interpolated ( _ref1 );
+ interpolated.ts_ = _ts;
+ interpolated.data_ . setZero();
+ interpolated.data_cov_ . setZero();
+ interpolated.delta_ = deltaZero();
+ interpolated.delta_cov_ . setZero();
+ interpolated.jacobian_delta_integr_ . setIdentity();
+ interpolated.jacobian_delta_ . setZero();
+
+ _second = _ref2;
+
+ return interpolated;
+ }
+ else
+ {
+ // _ts is closest to _ref2
+ Motion interpolated ( _ref2 );
interpolated.ts_ = _ts;
+
+ _second = _ref2;
_second.data_ . setZero();
_second.data_cov_ . setZero();
_second.delta_ = deltaZero();
@@ -518,7 +537,7 @@ Motion ProcessorMotion::interpolate(const Motion& _ref, Motion& _second, TimeSta
CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
{
// We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
- // Note: since the buffer goes from a FK through the past until the previous KF, we need to:
+ // Note: since the buffer goes from a KF in the past until the next KF, we need to:
// 1. See that the KF contains a CaptureMotion
// 2. See that the TS is smaller than the KF's TS
// 3. See that the TS is bigger than the KF's first Motion in the CaptureMotion's buffer
@@ -533,7 +552,7 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
capture = frame->getCaptureOf(getSensorPtr());
if (capture != nullptr)
{
- // We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
+ // Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
capture_motion = std::static_pointer_cast<CaptureMotion>(capture);
if (_ts >= capture_motion->getBuffer().get().front().ts_)
// Found time stamp satisfying rule 3 above !! ==> break for loop
diff --git a/src/processor_motion.h b/src/processor_motion.h
index 4e20cab461bfb9cb5555f3091b1abd5d086d6573..e399bddcce6562e265d540c9e5699cd8e7f64193 100644
--- a/src/processor_motion.h
+++ b/src/processor_motion.h
@@ -372,6 +372,24 @@ class ProcessorMotion : public ProcessorBase
*/
virtual Motion interpolate(const Motion& _ref, Motion& _second, TimeStamp& _ts);
+ /** \brief Interpolate motion to an intermediate time-stamp
+ *
+ * @param _ref1 The first motion reference
+ * @param _ref2 The second motion reference.
+ * @param _ts The intermediate time stamp: it must be bounded by `_ref.ts_ <= _ts <= _second.ts_`.
+ * @param _second The second part motion after interpolation, so that return (+) second = ref2.
+ * @return The interpolated motion (see documentation below).
+ *
+ * This function interpolates a motion between two existing motions.
+ *
+ * In this base implementation, we just provide the closest motion provided (ref1 or ref2),
+ * the second motion being the complementary part,
+ * so really no interpolation takes place and just the current data and delta are updated.
+ *
+ * Should you require finer interpolation, you must overload this method in your derived class.
+ */
+ virtual Motion interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second);
+
/** \brief create a CaptureMotion and add it to a Frame
* \param _ts time stamp
* \param _sensor Sensor that produced the Capture
@@ -399,6 +417,10 @@ class ProcessorMotion : public ProcessorBase
* \param _capture_motion: the parent capture
*/
FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own);
+
+ /** \brief create a feature corresponding to given capture
+ * \param _capture_motion: the parent capture
+ */
virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_own) = 0;
/** \brief create a constraint and link it in the wolf tree
@@ -435,7 +457,7 @@ class ProcessorMotion : public ProcessorBase
SizeEigen data_size_; ///< the size of the incoming data
SizeEigen delta_size_; ///< the size of the deltas
SizeEigen delta_cov_size_; ///< the size of the delta covariances matrix
- SizeEigen calib_size_; ///< size of the extra parameters (TBD in derived classes)
+ SizeEigen calib_size_; ///< the size of the calibration parameters (TBD in derived classes)
CaptureMotionPtr origin_ptr_;
CaptureMotionPtr last_ptr_;
CaptureMotionPtr incoming_ptr_;
@@ -449,7 +471,7 @@ class ProcessorMotion : public ProcessorBase
Eigen::MatrixXs delta_cov_; ///< current delta covariance
Eigen::VectorXs delta_integrated_; ///< integrated delta
Eigen::MatrixXs delta_integrated_cov_; ///< integrated delta covariance
- Eigen::VectorXs calib_; ///< calibration vector
+ Eigen::VectorXs calib_preint_; ///< calibration vector used during pre-integration
Eigen::MatrixXs jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated
Eigen::MatrixXs jacobian_delta_; ///< jacobian of delta composition w.r.t current delta
Eigen::MatrixXs jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params
diff --git a/src/processor_odom_2D.cpp b/src/processor_odom_2D.cpp
index fb1f567761e1a8d35b15aa019a78524fd47bfd16..19818dffcc1aee098c316f1b13bf603703333c39 100644
--- a/src/processor_odom_2D.cpp
+++ b/src/processor_odom_2D.cpp
@@ -5,10 +5,6 @@ namespace wolf
ProcessorOdom2D::ProcessorOdom2D(ProcessorParamsOdom2DPtr _params) :
ProcessorMotion("ODOM 2D", 3, 3, 3, 2, 0, _params),
params_odom_2D_(_params)
-// dist_traveled_th_(_params.dist_traveled_th_),
-// theta_traveled_th_(_params.theta_traveled_th_),
-// cov_det_th_(_params->cov_det_th)//,
-// elapsed_time_th_(_params.elapsed_time_th_)
{
unmeasured_perturbation_cov_ = _params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std * Matrix3s::Identity();
}
@@ -21,16 +17,17 @@ void ProcessorOdom2D::computeCurrentDelta(const Eigen::VectorXs& _data, const Ei
const Eigen::VectorXs& _calib, const Scalar _dt, Eigen::VectorXs& _delta,
Eigen::MatrixXs& _delta_cov, Eigen::MatrixXs& _jacobian_calib)
{
- //std::cout << "ProcessorOdom2d::data2delta" << std::endl;
assert(_data.size() == data_size_ && "Wrong _data vector size");
assert(_data_cov.rows() == data_size_ && "Wrong _data_cov size");
assert(_data_cov.cols() == data_size_ && "Wrong _data_cov size");
+
// data is [dtheta, dr]
// delta is [dx, dy, dtheta]
// motion model is 1/2 turn + straight + 1/2 turn
_delta(0) = cos(_data(1) / 2) * _data(0);
_delta(1) = sin(_data(1) / 2) * _data(0);
_delta(2) = _data(1);
+
// Fill delta covariance
Eigen::MatrixXs J(delta_cov_size_, data_size_);
J(0, 0) = cos(_data(1) / 2);
@@ -39,35 +36,29 @@ void ProcessorOdom2D::computeCurrentDelta(const Eigen::VectorXs& _data, const Ei
J(0, 1) = -_data(0) * sin(_data(1) / 2) / 2;
J(1, 1) = _data(0) * cos(_data(1) / 2) / 2;
J(2, 1) = 1;
+
// Since input data is size 2, and delta is size 3, the noise model must be given by:
// 1. Covariance of the input data: J*Q*J.tr
- // 2. Fix variance term to be added: var*Id
+ // 2. Fixed variance term to be added: var*Id
_delta_cov = J * _data_cov * J.transpose() + unmeasured_perturbation_cov_;
- //std::cout << "data :" << _data.transpose() << std::endl;
- //std::cout << "data cov :" << std::endl << _data_cov << std::endl;
- //std::cout << "delta :" << delta_.transpose() << std::endl;
- //std::cout << "delta cov :" << std::endl << delta_cov_ << std::endl;
- // jacobian_delta_calib_ not used in this class yet. In any case, set to zero with:
- // jacobian_delta_calib_.setZero();
}
void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, const Scalar _Dt2,
Eigen::VectorXs& _delta1_plus_delta2)
{
- // This is just a frame composition in 2D
- //std::cout << "ProcessorOdom2d::deltaPlusDelta" << std::endl;
assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
assert(_delta1_plus_delta2.size() == delta_size_ && "Wrong _delta1_plus_delta2 vector size");
- _delta1_plus_delta2.head<2>() = _delta1.head<2>() + Eigen::Rotation2Ds(_delta1(2)).matrix() * _delta2.head<2>();
- _delta1_plus_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
+
+ // This is just a frame composition in 2D
+ _delta1_plus_delta2.head<2>() = _delta1.head<2>() + Eigen::Rotation2Ds(_delta1(2)).matrix() * _delta2.head<2>();
+ _delta1_plus_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
}
void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, const Scalar _Dt2,
Eigen::VectorXs& _delta1_plus_delta2, Eigen::MatrixXs& _jacobian1,
Eigen::MatrixXs& _jacobian2)
{
- //std::cout << "ProcessorOdom2d::deltaPlusDelta jacobians" << std::endl;
assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
assert(_delta1_plus_delta2.size() == delta_size_ && "Wrong _delta1_plus_delta2 vector size");
@@ -75,12 +66,16 @@ void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen
assert(_jacobian1.cols() == delta_cov_size_ && "Wrong _jacobian1 size");
assert(_jacobian2.rows() == delta_cov_size_ && "Wrong _jacobian2 size");
assert(_jacobian2.cols() == delta_cov_size_ && "Wrong _jacobian2 size");
+
+ // This is just a frame composition in 2D
_delta1_plus_delta2.head<2>() = _delta1.head<2>() + Eigen::Rotation2Ds(_delta1(2)).matrix() * _delta2.head<2>();
_delta1_plus_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
+
// Jac wrt delta_integrated
_jacobian1 = Eigen::MatrixXs::Identity(delta_cov_size_, delta_cov_size_);
_jacobian1(0, 2) = -sin(_delta1(2)) * _delta2(0) - cos(_delta1(2)) * _delta2(1);
_jacobian1(1, 2) = cos(_delta1(2)) * _delta2(0) - sin(_delta1(2)) * _delta2(1);
+
// jac wrt delta
_jacobian2 = Eigen::MatrixXs::Identity(delta_cov_size_, delta_cov_size_);
_jacobian2.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(_delta1(2)).matrix();
@@ -89,10 +84,10 @@ void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen
void ProcessorOdom2D::statePlusDelta(const Eigen::VectorXs& _x, const Eigen::VectorXs& _delta, const Scalar _Dt,
Eigen::VectorXs& _x_plus_delta)
{
- // This is just a frame composition in 2D
- //std::cout << "ProcessorOdom2d::statePlusDelta" << std::endl;
assert(_x.size() == x_size_ && "Wrong _x vector size");
assert(_x_plus_delta.size() == x_size_ && "Wrong _x_plus_delta vector size");
+
+ // This is just a frame composition in 2D
_x_plus_delta.head<2>() = _x.head<2>() + Eigen::Rotation2Ds(_x(2)).matrix() * _delta.head<2>();
_x_plus_delta(2) = pi2pi(_x(2) + _delta(2));
}
diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index bac392115b91c812e8b079493ebe85b95ae3a8d2..cd99f0476dfe9eacbf0b7b63566f9306def8e554 100644
--- a/src/test/CMakeLists.txt
+++ b/src/test/CMakeLists.txt
@@ -94,7 +94,9 @@ target_link_libraries(gtest_processor_motion ${PROJECT_NAME})
# Rotation test
wolf_add_gtest(gtest_rotation gtest_rotation.cpp)
-#target_link_libraries(gtest_rotation ${PROJECT_NAME})
+
+# SE3 test
+wolf_add_gtest(gtest_SE3 gtest_SE3.cpp)
# SensorBase test
wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
diff --git a/src/test/gtest_IMU_tools.cpp b/src/test/gtest_IMU_tools.cpp
index 14957c9791a4754c254319b90f1d93c4e7763225..9f0f8c398e1f7b51690228f5ff3f75be4e66b01d 100644
--- a/src/test/gtest_IMU_tools.cpp
+++ b/src/test/gtest_IMU_tools.cpp
@@ -129,7 +129,7 @@ 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 = retract(d_min);
+ VectorXs delta = exp_IMU(d_min);
// expected delta
Vector3s dp = d_min.head(3);
@@ -139,11 +139,11 @@ TEST(IMU_tools, lift_retract)
ASSERT_MATRIX_APPROX(delta, delta_true, 1e-10);
// transform to a new tangent -- should be the original tangent
- VectorXs d_from_delta = lift(delta);
+ VectorXs d_from_delta = log_IMU(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 = retract(d_from_delta);
+ VectorXs delta_from_d = exp_IMU(d_from_delta);
ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10);
}
diff --git a/src/test/gtest_SE3.cpp b/src/test/gtest_SE3.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b734385b4a997e78d8b0de73573bbbe331009715
--- /dev/null
+++ b/src/test/gtest_SE3.cpp
@@ -0,0 +1,236 @@
+/**
+ * \file gtest_SE3.cpp
+ *
+ * Created on: Feb 2, 2019
+ * \author: jsola
+ */
+
+
+#include "../SE3.h"
+#include "utils_gtest.h"
+
+
+
+using namespace Eigen;
+using namespace wolf;
+using namespace three_D;
+
+TEST(SE3, exp_0)
+{
+ Vector6s tau = Vector6s::Zero();
+ Vector7s pose; pose << 0,0,0, 0,0,0,1;
+
+ ASSERT_MATRIX_APPROX(pose, exp_SE3(tau), 1e-8);
+}
+
+TEST(SE3, log_I)
+{
+ Vector7s pose; pose << 0,0,0, 0,0,0,1;
+ Vector6s tau = Vector6s::Zero();
+
+ ASSERT_MATRIX_APPROX(tau, log_SE3(pose), 1e-8);
+}
+
+TEST(SE3, exp_log)
+{
+ Vector6s tau = Vector6s::Random() / 5.0;
+ Vector7s pose = exp_SE3(tau);
+
+ ASSERT_MATRIX_APPROX(tau, log_SE3(exp_SE3(tau)), 1e-8);
+ ASSERT_MATRIX_APPROX(pose, exp_SE3(log_SE3(pose)), 1e-8);
+}
+
+TEST(SE3, exp_of_multiple)
+{
+ Vector6s tau, tau2, tau3;
+ Vector7s pose, pose2, pose3;
+ tau = Vector6s::Random() / 5;
+ pose << exp_SE3(tau);
+ tau2 = 2*tau;
+ tau3 = 3*tau;
+ pose2 = compose(pose, pose);
+ pose3 = compose(pose2, pose);
+
+ // 1
+ ASSERT_MATRIX_APPROX(exp_SE3(tau), pose, 1e-8);
+
+ // 2
+ ASSERT_MATRIX_APPROX(exp_SE3(tau2), pose2, 1e-8);
+
+ // 3
+ ASSERT_MATRIX_APPROX(exp_SE3(tau3), pose3, 1e-8);
+}
+
+TEST(SE3, log_of_power)
+{
+ Vector6s tau, tau2, tau3;
+ Vector7s pose, pose2, pose3;
+ tau = Vector6s::Random() / 5;
+ pose << exp_SE3(tau);
+ tau2 = 2*tau;
+ tau3 = 3*tau;
+ pose2 = compose(pose, pose);
+ pose3 = compose(pose2, pose);
+
+ // 1
+ ASSERT_MATRIX_APPROX(tau, log_SE3(pose), 1e-8);
+
+ // 2
+ ASSERT_MATRIX_APPROX(tau2, log_SE3(pose2), 1e-8);
+
+ // 3
+ ASSERT_MATRIX_APPROX(tau3, log_SE3(pose3), 1e-8);
+}
+
+TEST(SE3, interpolate_I_xyz)
+{
+ Vector7s p1, p2, p_pre;
+
+ p1 << 0,0,0, 0,0,0,1;
+ p2 << 1,2,3, 0,0,0,1;
+ Scalar t = 0.2;
+
+ interpolate(p1, p2, t, p_pre);
+
+ Vector7s p_gt; p_gt << 0.2,0.4,0.6, 0,0,0,1;
+
+ ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, interpolate_xyz_xyz)
+{
+ Vector7s p1, p2, p_pre;
+
+ p1 << 1,2,3, 0,0,0,1;
+ p2 << 2,4,6, 0,0,0,1;
+ Scalar t = 0.2;
+
+ interpolate(p1, p2, t, p_pre);
+
+ Vector7s p_gt; p_gt << 1.2,2.4,3.6, 0,0,0,1;
+
+ ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, interpolate_I_qx)
+{
+ Vector7s p1, p2, p_pre;
+
+ p1 << 0,0,0, 0,0,0,1;
+ p2 << 0,0,0, 1,0,0,0;
+ Scalar t = 0.5;
+
+ interpolate(p1, p2, t, p_pre);
+
+ Vector7s p_gt; p_gt << 0,0,0, sqrt(2)/2,0,0,sqrt(2)/2;
+
+ ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, interpolate_I_qy)
+{
+ Vector7s p1, p2, p_pre;
+
+ p1 << 0,0,0, 0,0,0,1;
+ p2 << 0,0,0, 0,1,0,0;
+ Scalar t = 0.5;
+
+ interpolate(p1, p2, t, p_pre);
+
+ Vector7s p_gt; p_gt << 0,0,0, 0,sqrt(2)/2,0,sqrt(2)/2;
+
+ ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, interpolate_I_qz)
+{
+ Vector7s p1, p2, p_pre;
+
+ p1 << 0,0,0, 0,0,0,1;
+ p2 << 0,0,0, 0,0,1,0;
+ Scalar t = 0.5;
+
+ interpolate(p1, p2, t, p_pre);
+
+ Vector7s p_gt; p_gt << 0,0,0, 0,0,sqrt(2)/2,sqrt(2)/2;
+
+ ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, interpolate_I_qxyz)
+{
+ Vector7s p1, p2, dp, p_pre, p_gt;
+ Vector3s da;
+
+ da.setRandom(); da /= 5;
+
+ p1 << 0,0,0, 0,0,0,1;
+ dp << 0,0,0, exp_q(da).coeffs();
+ p_gt = compose(p1, dp);
+ p2 = compose(p_gt, dp);
+ Scalar t = 0.5;
+
+ interpolate(p1, p2, t, p_pre);
+
+ ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, interpolate_half)
+{
+ Vector7s p1, p2, dp, p_pre, p_gt;
+ Vector6s da;
+
+ p1.setRandom(); p1.tail(4).normalize();
+
+ da.setRandom(); da /= 5; // small rotation
+ dp << exp_SE3(da);
+
+ // compose double, then interpolate 1/2
+
+ p_gt = compose(p1, dp);
+ p2 = compose(p_gt, dp);
+
+ Scalar t = 0.5;
+ interpolate(p1, p2, t, p_pre);
+
+ ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, interpolate_third)
+{
+ Vector7s p1, p2, dp, dp2, dp3, p_pre, p_gt;
+ Vector6s da;
+
+ p1.setRandom(); p1.tail(4).normalize();
+
+ da.setRandom(); da /= 5; // small rotation
+ dp << exp_SE3(da);
+ dp2 = compose(dp, dp);
+ dp3 = compose(dp2, dp);
+
+ // compose triple, then interpolate 1/3
+
+ p_gt = compose(p1, dp);
+ p2 = compose(p1, dp3);
+
+ Scalar t = 1.0/3.0;
+ interpolate(p1, p2, t, p_pre);
+
+ ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, toSE3_I)
+{
+ Vector7s pose; pose << 0,0,0, 0,0,0,1;
+ Matrix4s R;
+ toSE3(pose, R);
+
+ ASSERT_MATRIX_APPROX(R, Matrix4s::Identity(), 1e-8);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/src/test/gtest_odom_2D.cpp b/src/test/gtest_odom_2D.cpp
index da1e5ca48bf5fd9f0e96217426cbb31f73be5772..d8e10b949a137e718d1ca51502e94671d8379e09 100644
--- a/src/test/gtest_odom_2D.cpp
+++ b/src/test/gtest_odom_2D.cpp
@@ -16,6 +16,7 @@
#include "../state_block.h"
#include "../wolf.h"
#include "../ceres_wrapper/ceres_manager.h"
+#include "../capture_pose.h"
// STL includes
#include <map>
@@ -26,7 +27,6 @@
// General includes
#include <iostream>
#include <iomanip> // std::setprecision
-#include "../capture_pose.h"
using namespace wolf;
using namespace Eigen;
@@ -124,7 +124,7 @@ TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D)
Matrix3s delta_cov; delta_cov << .02, 0, 0, 0, .025, .02, 0, .02, .02;
ProblemPtr Pr = Problem::create("PO 2D");
- CeresManager ceres_manager(Pr);
+ CeresManager ceres_manager(Pr);
// KF0 and absolute prior
FrameBasePtr F0 = Pr->setPrior(x0, P0,t0, dt/2);
diff --git a/src/test/gtest_processor_motion.cpp b/src/test/gtest_processor_motion.cpp
index 77d2a55186ae6084f54e6832c71bc88067edc081..94a2f0c15f61439d8f5d800d893d6ae95f45016a 100644
--- a/src/test/gtest_processor_motion.cpp
+++ b/src/test/gtest_processor_motion.cpp
@@ -29,13 +29,17 @@ class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{
Vector2s data;
Matrix2s data_cov;
- ProcessorMotion_test() : ProcessorOdom2D(std::make_shared<ProcessorParamsOdom2D>()) { }
+ ProcessorMotion_test() :
+ ProcessorOdom2D(std::make_shared<ProcessorParamsOdom2D>()),
+ dt(0)
+ { }
virtual void SetUp()
{
- dt = 1.0;
+ dt = 1.0;
problem = Problem::create("PO 2D");
ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
+ params->time_tolerance = 0.25;
params->dist_traveled = 100;
params->angle_turned = 6.28;
params->max_time_span = 2.5*dt; // force KF at every third process()
@@ -52,19 +56,6 @@ class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{
virtual void TearDown(){}
- virtual Motion interpolate(const Motion& _ref,
- Motion& _second,
- TimeStamp& _ts)
- {
- return ProcessorMotion::interpolate(_ref, _second, _ts);
- }
-
- virtual Motion motionZero(TimeStamp& t)
- {
- return ProcessorOdom2D::motionZero(t);
- }
-
-
};
@@ -93,7 +84,7 @@ TEST_F(ProcessorMotion_test, IntegrateCircle)
data_cov.setIdentity();
TimeStamp t(0.0);
- for (int i = 0; i<10; i++) // one full turn
+ for (int i = 0; i<10; i++) // one full turn exactly
{
t += dt;
capture->setTimeStamp(t);
@@ -108,13 +99,13 @@ TEST_F(ProcessorMotion_test, IntegrateCircle)
TEST_F(ProcessorMotion_test, Interpolate)
{
- data << 1, 2*M_PI/10; // advance straight
+ 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++)
+ for (int i = 0; i<10; i++) // one full turn exactly
{
t += dt;
capture->setTimeStamp(t);