diff --git a/.gitignore b/.gitignore index 2e5e674704c29459650beb9667740661b78e88e9..ee22d7c8146da16da38f11e838fb6523e42dced7 100644 --- a/.gitignore +++ b/.gitignore @@ -4,6 +4,7 @@ README.txt bin/ build/ +build_release/ lib/ .idea/ ./Wolf.user diff --git a/CMakeLists.txt b/CMakeLists.txt index e6d799cbe84406acc5e4af82f79335f3464a0754..a7b87c5e9430892f56e7629dd8ab5fd4ada02a2a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -282,7 +282,7 @@ include/base/state_angle.h include/base/state_block.h include/base/state_homogeneous_3D.h include/base/state_quaternion.h -include/base/three_D_tools.h +include/base/SE3.h include/base/time_stamp.h include/base/track_matrix.h include/base/trajectory_base.h @@ -478,7 +478,7 @@ include/base/state_angle.h include/base/state_block.h include/base/state_homogeneous_3D.h include/base/state_quaternion.h -include/base/three_D_tools.h +include/base/SE3.h include/base/time_stamp.h include/base/track_matrix.h include/base/trajectory_base.h diff --git a/README.md b/README.md index d8291ed4041e6c171c853ce14c86e67187e5d374..fb08051ca2f57199c0d91870e30db96ebb381b43 100644 --- a/README.md +++ b/README.md @@ -139,7 +139,7 @@ We are shipping the CMAKE file `FindYamlCpp.cmake` together with Wolf. Find it a #### spdlog -Wolf used spdlog macros. To install it: +Wolf uses spdlog macros. Right now Wolf is only compatible with spdlog version 0.17.0. To install it: - Git clone the source: @@ -148,6 +148,7 @@ Wolf used spdlog macros. To install it: - Build and install with: $ cd spdlog + $ git checkout v0.17.0 $ mkdir build $ cd build $ cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" .. diff --git a/include/base/IMU_tools.h b/include/base/IMU_tools.h index 3159a1b9ca7b80b9c5ef018998f2f9e1fc119c49..eecad244c7a57ee2868bcf464e951a0a5108160d 100644 --- a/include/base/IMU_tools.h +++ b/include/base/IMU_tools.h @@ -33,10 +33,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 */ @@ -340,7 +340,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); @@ -361,7 +361,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/include/base/SE3.h b/include/base/SE3.h new file mode 100644 index 0000000000000000000000000000000000000000..cea35460771f0bf91d4f56520dd823ad3a1d505d --- /dev/null +++ b/include/base/SE3.h @@ -0,0 +1,479 @@ +/* + * SE3.h + * + * Created on: Mar 15, 2018 + * Author: jsola + */ + +#ifndef SE3_H_ +#define SE3_H_ + +#include "base/wolf.h" +#include "base/rotations.h" + +/* + * The functions in this file are related to manipulations of Delta motion magnitudes used in 3D motion. + * + * The Delta is defined as a simple 3D pose with the rotation expressed in quaternion form, + * Delta = [Dp, Dq] + * with + * Dp : position delta + * Dq : quaternion delta + * + * The functions are listed below: + * + * - compose: Dc = D1 (+) D2 + * - 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 + * - 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) + */ + +namespace wolf +{ +namespace three_D { +using namespace Eigen; + +template<typename D1, typename D2> +inline void identity(MatrixBase<D1>& p, QuaternionBase<D2>& q) +{ + MatrixSizeCheck<3, 1>::check(p); + p = MatrixBase<D1>::Zero(3,1); + q = QuaternionBase<D2>::Identity(); +} + +template<typename D1, typename D2> +inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q) +{ + MatrixSizeCheck<3, 1>::check(p); + MatrixSizeCheck<4, 1>::check(q); + typedef typename D1::Scalar T1; + typedef typename D2::Scalar T2; + p << T1(0), T1(0), T1(0); + q << T2(0), T2(0), T2(0), T2(1); +} + +template<typename T = Scalar> +inline Matrix<T, 7, 1> identity() +{ + Matrix<T, 7, 1> ret; + ret<< T(0), T(0), T(0), + T(0), T(0), T(0), T(1); + return ret; +} + +template<typename D1, typename D2, typename D4, typename D5> +inline void inverse(const MatrixBase<D1>& dp, const QuaternionBase<D2>& dq, + MatrixBase<D4>& idp, QuaternionBase<D5>& idq) +{ + MatrixSizeCheck<3, 1>::check(dp); + MatrixSizeCheck<3, 1>::check(idp); + + idp = - dq.conjugate() * dp ; + idq = dq.conjugate() ; +} + +template<typename D1, typename D2> +inline void inverse(const MatrixBase<D1>& d, + MatrixBase<D2>& id) +{ + MatrixSizeCheck<7, 1>::check(d); + MatrixSizeCheck<7, 1>::check(id); + + Map<const Matrix<typename D1::Scalar, 3, 1> > dp ( & d( 0 ) ); + Map<const Quaternion<typename D1::Scalar> > dq ( & d( 3 ) ); + Map<Matrix<typename D2::Scalar, 3, 1> > idp ( & id( 0 ) ); + Map<Quaternion<typename D2::Scalar> > idq ( & id( 3 ) ); + + inverse(dp, dq, idp, idq); +} + +template<typename D> +inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d) +{ + Matrix<typename D::Scalar, 7, 1> id; + inverse(d, id); + return id; +} + +template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8> +inline void compose(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, + const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, + MatrixBase<D7>& sum_p, QuaternionBase<D8>& sum_q ) +{ + MatrixSizeCheck<3, 1>::check(dp1); + MatrixSizeCheck<3, 1>::check(dp2); + MatrixSizeCheck<3, 1>::check(sum_p); + + sum_p = dp1 + dq1*dp2; + sum_q = dq1*dq2; // dq here to avoid possible aliasing between d1 and sum +} + +template<typename D1, typename D2, typename D3> +inline void compose(const MatrixBase<D1>& d1, + const MatrixBase<D2>& d2, + MatrixBase<D3>& sum) +{ + MatrixSizeCheck<7, 1>::check(d1); + MatrixSizeCheck<7, 1>::check(d2); + MatrixSizeCheck<7, 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 D2::Scalar, 3, 1> > dp2 ( & d2( 0 ) ); + Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2( 3 ) ); + Map<Matrix<typename D3::Scalar, 3, 1> > sum_p ( & sum( 0 ) ); + Map<Quaternion<typename D3::Scalar> > sum_q ( & sum( 3 ) ); + + compose(dp1, dq1, dp2, dq2, sum_p, sum_q); +} + +template<typename D1, typename D2> +inline Matrix<typename D1::Scalar, 7, 1> compose(const MatrixBase<D1>& d1, + const MatrixBase<D2>& d2 ) +{ + Matrix<typename D1::Scalar, 7, 1> ret; + compose(d1, d2, 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, + MatrixBase<D3>& sum, + MatrixBase<D4>& J_sum_d1, + MatrixBase<D5>& J_sum_d2) +{ + MatrixSizeCheck<7, 1>::check(d1); + MatrixSizeCheck<7, 1>::check(d2); + MatrixSizeCheck<7, 1>::check(sum); + MatrixSizeCheck< 6, 6>::check(J_sum_d1); + MatrixSizeCheck< 6, 6>::check(J_sum_d2); + + // Some useful temporaries + Matrix<typename D1::Scalar, 3, 3> dR1 = q2R(d1.segment(3,4)); //dq1.matrix(); // First Delta, DR + Matrix<typename D2::Scalar, 3, 3> dR2 = q2R(d2.segment(3,4)); //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(d2.head(3)) ; // dDp'/dDo + J_sum_d1.block(3,3,3,3) = dR2.transpose(); // dDo'/dDo + + // Jac wrt second delta + J_sum_d2.setIdentity(); // + J_sum_d2.block(0,0,3,3) = dR1; // dDp'/ddp + // J_sum_d2.block(3,3,3,3) = Matrix3s::Identity(); // dDo'/ddo = I + + // compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable + compose(d1, d2, sum); +} + +template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8> +inline void between(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, + const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, + MatrixBase<D7>& dp12, QuaternionBase<D8>& dq12) +{ + MatrixSizeCheck<3, 1>::check(dp1); + MatrixSizeCheck<3, 1>::check(dp2); + MatrixSizeCheck<3, 1>::check(dp12); + + dp12 = dq1.conjugate() * ( dp2 - dp1 ); + dq12 = dq1.conjugate() * dq2; +} + +template<typename D1, typename D2, typename D3> +inline void between(const MatrixBase<D1>& d1, + const MatrixBase<D2>& d2, + MatrixBase<D3>& d2_minus_d1) +{ + MatrixSizeCheck<7, 1>::check(d1); + MatrixSizeCheck<7, 1>::check(d2); + MatrixSizeCheck<7, 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 D2::Scalar, 3, 1> > dp2 ( & d2(0) ); + Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) ); + Map<Matrix<typename D3::Scalar, 3, 1> > dp12 ( & d2_minus_d1(0) ); + Map<Quaternion<typename D3::Scalar> > dq12 ( & d2_minus_d1(3) ); + + between(dp1, dq1, dp2, dq2, dp12, dq12); +} + +template<typename D1, typename D2> +inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1, + const MatrixBase<D2>& d2 ) +{ + MatrixSizeCheck<7, 1>::check(d1); + MatrixSizeCheck<7, 1>::check(d2); + Matrix<typename D1::Scalar, 7, 1> d12; + between(d1, d2, d12); + return d12; +} + +template<typename Derived> +Matrix<typename Derived::Scalar, 6, 1> log_SE3(const MatrixBase<Derived>& delta_in) +{ + MatrixSizeCheck<7, 1>::check(delta_in); + + Matrix<typename Derived::Scalar, 6, 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<Matrix<typename Derived::Scalar, 3, 1> > dp_ret ( & ret(0) ); + Map<Matrix<typename Derived::Scalar, 3, 1> > do_ret ( & ret(3) ); + + 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> 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 = V * dp_in; + dq = exp_q(do_in); + + return ret; +} + +template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8> +inline void plus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, + const MatrixBase<D4>& dp2, const MatrixBase<D5>& do2, + MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q) +{ + MatrixSizeCheck<3, 1>::check(dp1); + MatrixSizeCheck<3, 1>::check(dp2); + MatrixSizeCheck<3, 1>::check(plus_p); + plus_p = dp1 + dp2; + plus_q = dq1 * exp_q(do2); +} + +template<typename D1, typename D2, typename D3> +inline void plus(const MatrixBase<D1>& d1, + const MatrixBase<D2>& d2, + MatrixBase<D3>& d_plus) +{ + Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) ); + Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) ); + Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) ); + Map<const Matrix<typename D2::Scalar, 3, 1> > do2 ( & d2(3) ); + Map<Matrix<typename D3::Scalar, 3, 1> > dp_p ( & d_plus(0) ); + Map<Quaternion<typename D3::Scalar> > dq_p ( & d_plus(3) ); + + plus(dp1, dq1, dp2, do2, dp_p, dq_p); +} + +template<typename D1, typename D2> +inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1, + const MatrixBase<D2>& d2) +{ + Matrix<typename D1::Scalar, 7, 1> d_plus; + plus(d1, d2, d_plus); + return d_plus; +} + +template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8> +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); +} + +template<typename D1, typename D2, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9> +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) +{ + 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); +} + +template<typename D1, typename D2, typename D3> +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) ); + Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) ); + Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) ); + Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & err(0) ); + Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & err(3) ); + + minus(dp1, dq1, dp2, dq2, diff_p, diff_o); +} + +template<typename D1, typename D2, typename D3, typename D4, typename D5> +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) ); + 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 ( & dif(0) ); + Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & dif(3) ); + Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & dif(6) ); + + Matrix<typename D4::Scalar, 3, 3> 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 = minus(d1, d2) is + * dp = dp2 - dp1 + * do = Log(dq1.conj * dq2) + * dv = dv2 - dv1 + * + * With trivial Jacobians for dp and dv, and: + * J_do_dq1 = - J_l_inv(theta) + * J_do_dq2 = J_r_inv(theta) + */ + + J_diff_d1 = - Matrix<typename D4::Scalar, 6, 6>::Identity();// d(p2 - p1) / d(p1) = - Identity + J_diff_d1.block(3,3,3,3) = J_do_dq1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta) + + J_diff_d2.setIdentity(6,6); // d(R1.tr*R2) / d(R2) = Identity + J_diff_d2.block(3,3,3,3) = J_do_dq2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta) +} + +template<typename D1, typename D2> +inline Matrix<typename D1::Scalar, 6, 1> minus(const MatrixBase<D1>& d1, + const MatrixBase<D2>& d2) +{ + Matrix<typename D1::Scalar, 6, 1> 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 /* SE3_H_ */ diff --git a/include/base/capture/capture_base.h b/include/base/capture/capture_base.h index a3a94e6769a9517e02ff3e7b11395d84a8f3efb4..293ab9d0303aa493930ae11336b7abfe58d7d0f9 100644 --- a/include/base/capture/capture_base.h +++ b/include/base/capture/capture_base.h @@ -29,7 +29,6 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture SizeEigen calib_size_; ///< size of the calibration parameters (dynamic or static sensor params that are not fixed) static unsigned int capture_id_count_; - bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove(). protected: unsigned int capture_id_; @@ -45,11 +44,13 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture StateBlockPtr _intr_ptr = nullptr); virtual ~CaptureBase(); - void remove(); + virtual void remove(); // Type virtual bool isMotion() const { return false; } + bool process(); + unsigned int id(); TimeStamp getTimeStamp() const; void setTimeStamp(const TimeStamp& _ts); @@ -217,6 +218,14 @@ inline void CaptureBase::setTimeStampToNow() time_stamp_.setToNow(); } +inline bool CaptureBase::process() +{ + assert (getSensorPtr() != nullptr && "Attempting to process a capture with no associated sensor. Either set the capture's sensor or call sensor->process(capture) instead."); + + return getSensorPtr()->process(shared_from_this()); +} + + } // namespace wolf #endif diff --git a/include/base/ceres_wrapper/ceres_manager.h b/include/base/ceres_wrapper/ceres_manager.h index 7a0404cec9e50580b52710271e73bf284d38df2c..bd81359681008701c6568a3d5300f5f3ab54c612 100644 --- a/include/base/ceres_wrapper/ceres_manager.h +++ b/include/base/ceres_wrapper/ceres_manager.h @@ -55,6 +55,14 @@ class CeresManager : public SolverManager virtual void computeCovariances(const StateBlockList& st_list) override; + virtual bool hasConverged() override; + + virtual SizeStd iterations() override; + + virtual Scalar initialCost() override; + + virtual Scalar finalCost() override; + ceres::Solver::Options& getSolverOptions(); void check(); diff --git a/include/base/constraint/constraint_AHP.h b/include/base/constraint/constraint_AHP.h index 59705efbbafc267d708d60f6ba927e806c79a5bd..a0ab7db1fa497ac7f9a9f87db97ecb128650c306 100644 --- a/include/base/constraint/constraint_AHP.h +++ b/include/base/constraint/constraint_AHP.h @@ -184,18 +184,14 @@ inline bool ConstraintAHP::operator ()(const T* const _current_frame_p, } inline ConstraintAHPPtr ConstraintAHP::create(const FeatureBasePtr& _ftr_ptr, - const LandmarkAHPPtr& _lmk_ahp_ptr, - const ProcessorBasePtr& _processor_ptr, - bool _apply_loss_function, - ConstraintStatus _status) + const LandmarkAHPPtr& _lmk_ahp_ptr, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + ConstraintStatus _status) { // construct constraint ConstraintAHPPtr ctr_ahp = std::make_shared<ConstraintAHP>(_ftr_ptr, _lmk_ahp_ptr, _processor_ptr, _apply_loss_function, _status); - // link it to wolf tree <-- these pointers cannot be set at construction time - _lmk_ahp_ptr->getAnchorFrame()->addConstrainedBy(ctr_ahp); - _lmk_ahp_ptr->addConstrainedBy(ctr_ahp); - return ctr_ahp; } diff --git a/include/base/constraint/constraint_base.h b/include/base/constraint/constraint_base.h index 9932bad8d03cb757c87011c39deb30a61bcf1887..1a0b4f14f77f559f983b7dbce0b4028bea45baa9 100644 --- a/include/base/constraint/constraint_base.h +++ b/include/base/constraint/constraint_base.h @@ -42,7 +42,6 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons FeatureBaseWPtr feature_ptr_; ///< FeatureBase pointer (upper node) static unsigned int constraint_id_count_; - bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove(). protected: unsigned int constraint_id_; @@ -75,7 +74,7 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons virtual ~ConstraintBase() = default; - void remove(); + virtual void remove(); unsigned int id() const; diff --git a/include/base/feature/feature_base.h b/include/base/feature/feature_base.h index 88521e9a6e9c55ececea8fbf9c81cda2aa8edf13..1ab46cccceebfae070368341aa2edde0000e694a 100644 --- a/include/base/feature/feature_base.h +++ b/include/base/feature/feature_base.h @@ -24,7 +24,6 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature ConstraintBaseList constrained_by_list_; static unsigned int feature_id_count_; - bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove(). protected: unsigned int feature_id_; @@ -45,7 +44,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance); virtual ~FeatureBase(); - void remove(); + virtual void remove(); virtual void setProblem(ProblemPtr _problem) final; diff --git a/include/base/frame_base.h b/include/base/frame_base.h index b8430b41fe9624b80227aa39ba6088051393b760..16d13dbee1f0484dee1f911df14b07fd476a007d 100644 --- a/include/base/frame_base.h +++ b/include/base/frame_base.h @@ -36,7 +36,6 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order: Position, Orientation, Velocity. static unsigned int frame_id_count_; - bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove(). protected: unsigned int frame_id_; @@ -66,7 +65,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _v_ptr = nullptr); virtual ~FrameBase(); - void remove(); + virtual void remove(); // Frame properties ----------------------------------------------- public: diff --git a/include/base/landmark/landmark_base.h b/include/base/landmark/landmark_base.h index f0a61e47496fb8373f6e75ee9cbbb71138191111..8b365e9be506f9271571542970683032326819f9 100644 --- a/include/base/landmark/landmark_base.h +++ b/include/base/landmark/landmark_base.h @@ -28,7 +28,6 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order P, O. static unsigned int landmark_id_count_; - bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove(). protected: unsigned int landmark_id_; ///< landmark unique id @@ -47,7 +46,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma **/ LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr); virtual ~LandmarkBase(); - void remove(); + virtual void remove(); virtual YAML::Node saveToYaml() const; // Properties diff --git a/include/base/node_base.h b/include/base/node_base.h index 0c1d60d8603b56026867fd4be70441a42d38a383..2426f53a53357005f9e0fcab1ed0af72b7494b7a 100644 --- a/include/base/node_base.h +++ b/include/base/node_base.h @@ -66,6 +66,8 @@ class NodeBase std::string node_type_; ///< Text label identifying the type or subcategory of node ("Pin Hole", "Point 2D", etc) std::string node_name_; ///< Text label identifying each specific object ("left camera", "LIDAR 1", "PointGrey", "Andrew", etc) + bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove(). + public: NodeBase(const std::string& _category, const std::string& _type = "Undefined", const std::string& _name = ""); @@ -75,6 +77,7 @@ class NodeBase std::string getCategory() const; std::string getType() const; std::string getName() const; + bool isRemoving() const; void setType(const std::string& _type); void setName(const std::string& _name); @@ -94,7 +97,8 @@ inline NodeBase::NodeBase(const std::string& _category, const std::string& _type node_id_(++node_id_count_), node_category_(_category), node_type_(_type), - node_name_(_name) + node_name_(_name), + is_removing_(false) { // } @@ -119,6 +123,11 @@ inline std::string NodeBase::getName() const return node_name_; } +inline bool NodeBase::isRemoving() const +{ + return is_removing_; +} + inline void NodeBase::setType(const std::string& _type) { node_type_ = _type; diff --git a/include/base/problem.h b/include/base/problem.h index c6374623ba7ea4550c1d2dce053026bd9789716f..b15159163f4b13248bca7c1a23fa9ae90b30541c 100644 --- a/include/base/problem.h +++ b/include/base/problem.h @@ -43,7 +43,7 @@ class Problem : public std::enable_shared_from_this<Problem> ProcessorMotionPtr processor_motion_ptr_; StateBlockList state_block_list_; std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXs> covariances_; - SizeEigen state_size_, state_cov_size_; + SizeEigen state_size_, state_cov_size_, dim_; std::map<ConstraintBasePtr, Notification> constraint_notification_map_; std::map<StateBlockPtr, Notification> state_block_notification_map_; bool prior_is_set_; @@ -59,6 +59,7 @@ class Problem : public std::enable_shared_from_this<Problem> // Properties ----------------------------------------- SizeEigen getFrameStructureSize() const; void getFrameStructureSize(SizeEigen& _x_size, SizeEigen& _cov_size) const; + SizeEigen getDim() const; // Hardware branch ------------------------------------ HardwareBasePtr getHardwarePtr(); diff --git a/include/base/processor/processor_base.h b/include/base/processor/processor_base.h index 8537db78b20189aef1adeb3bdc2abdf7f41f3681..6a1d23cfb3fd19a625407c4de20eb6d17784e371 100644 --- a/include/base/processor/processor_base.h +++ b/include/base/processor/processor_base.h @@ -56,8 +56,8 @@ class PackKeyFrameBuffer PackKeyFramePtr selectPack(const TimeStamp& _time_stamp, const Scalar& _time_tolerance); PackKeyFramePtr selectPack(const CaptureBasePtr _capture, const Scalar& _time_tolerance); - PackKeyFramePtr selectPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance); - PackKeyFramePtr selectPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance); + PackKeyFramePtr selectFirstPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance); + PackKeyFramePtr selectFirstPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance); /**\brief Buffer size * @@ -145,15 +145,13 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce private: SensorBaseWPtr sensor_ptr_; - bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove(). - static unsigned int processor_id_count_; public: ProcessorBase(const std::string& _type, ProcessorParamsBasePtr _params); virtual ~ProcessorBase(); virtual void configure(SensorBasePtr _sensor) = 0; - void remove(); + virtual void remove(); unsigned int id(); diff --git a/include/base/processor/processor_motion.h b/include/base/processor/processor_motion.h index 5fc43d3887f9277512e67cca351e23edb2107771..0ce05f202e9eb68290871cbcd2319b8c12528a99 100644 --- a/include/base/processor/processor_motion.h +++ b/include/base/processor/processor_motion.h @@ -367,6 +367,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 @@ -394,6 +412,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 @@ -428,7 +450,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_; @@ -442,7 +464,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/include/base/processor/processor_tracker_feature.h b/include/base/processor/processor_tracker_feature.h index d118388717c322444cac0da6c26c95fc856edeba..52bd493f6870a1424621f5085ebed10b9558df60 100644 --- a/include/base/processor/processor_tracker_feature.h +++ b/include/base/processor/processor_tracker_feature.h @@ -114,11 +114,11 @@ class ProcessorTrackerFeature : public ProcessorTracker virtual unsigned int processKnown(); /** \brief Track provided features from \b last to \b incoming - * \param _feature_list_in input list of features in \b last to track - * \param _feature_list_out returned list of features found in \b incoming + * \param _features_last_in input list of features in \b last to track + * \param _features_incoming_out returned list of features found in \b incoming * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr */ - virtual unsigned int trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out, FeatureMatchMap& _feature_correspondences) = 0; + virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_correspondences) = 0; /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin. * \param _origin_feature input feature in origin capture tracked @@ -153,7 +153,7 @@ class ProcessorTrackerFeature : public ProcessorTracker * * The function sets the member new_features_last_, the list of newly detected features. */ - virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out) = 0; + virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out) = 0; /** \brief Create a new constraint and link it to the wolf tree * \param _feature_ptr pointer to the parent Feature diff --git a/include/base/processor/processor_tracker_feature_corner.h b/include/base/processor/processor_tracker_feature_corner.h index 508867b73ed0dd716f13356182ab94a43d2600e6..5bc33bf47a51b4b267c64ecf916ccc271047c280 100644 --- a/include/base/processor/processor_tracker_feature_corner.h +++ b/include/base/processor/processor_tracker_feature_corner.h @@ -82,11 +82,11 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature void advanceDerived(); /** \brief Track provided features from \b last to \b incoming - * \param _feature_list_in input list of features in \b last to track - * \param _feature_list_out returned list of features found in \b incoming + * \param _features_last_in input list of features in \b last to track + * \param _features_incoming_out returned list of features found in \b incoming * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr */ - virtual unsigned int trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out, + virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_correspondences); /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin. @@ -115,7 +115,7 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature * The function sets the member new_features_list_, the list of newly detected features, * to be used for landmark initialization. */ - virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out); + virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out); virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr); diff --git a/include/base/processor/processor_tracker_feature_dummy.h b/include/base/processor/processor_tracker_feature_dummy.h index 7c4c453efe8f11ed429fc68305c91cf879c5b3ec..dbc024b7c1187c5a9cf4b0753aaa8d0fc56fe050 100644 --- a/include/base/processor/processor_tracker_feature_dummy.h +++ b/include/base/processor/processor_tracker_feature_dummy.h @@ -32,11 +32,11 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature unsigned int n_feature_; /** \brief Track provided features from \b last to \b incoming - * \param _feature_list_in input list of features in \b last to track - * \param _feature_list_out returned list of features found in \b incoming + * \param _features_last_in input list of features in \b last to track + * \param _features_incoming_out returned list of features found in \b incoming * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr */ - virtual unsigned int trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out, + virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_correspondences); /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin. @@ -65,7 +65,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature * The function sets the member new_features_list_, the list of newly detected features, * to be used for landmark initialization. */ - virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out); + virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out); virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr); diff --git a/include/base/processor/processor_tracker_feature_image.h b/include/base/processor/processor_tracker_feature_image.h index 2bb0d9fb867788e3286bf739f205a17913731e71..48f7e7a705bd0b2006f5ec009653b895e211b11d 100644 --- a/include/base/processor/processor_tracker_feature_image.h +++ b/include/base/processor/processor_tracker_feature_image.h @@ -91,7 +91,7 @@ class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature image_last_ = image_incoming_; } - virtual unsigned int trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out, + virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_correspondences); /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin. @@ -119,7 +119,7 @@ class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature * * \return The number of detected Features. */ - virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out); + virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out); virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr); diff --git a/include/base/processor/processor_tracker_feature_trifocal.h b/include/base/processor/processor_tracker_feature_trifocal.h index 8ff87d9a9d1efa9e1b51a43b2bb2a390c28c7384..dca3b791a6885b5e89296b6f988838a511c212fa 100644 --- a/include/base/processor/processor_tracker_feature_trifocal.h +++ b/include/base/processor/processor_tracker_feature_trifocal.h @@ -62,11 +62,11 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature virtual void configure(SensorBasePtr _sensor) override; /** \brief Track provided features from \b last to \b incoming - * \param _feature_list_in input list of features in \b last to track - * \param _feature_list_out returned list of features found in \b incoming + * \param _features_last_in input list of features in \b last to track + * \param _features_incoming_out returned list of features found in \b incoming * \param _feature_matches returned map of correspondences: _feature_matches[feature_out_ptr] = feature_in_ptr */ - virtual unsigned int trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out, FeatureMatchMap& _feature_matches) override; + virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_matches) override; /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin. * \param _origin_feature input feature in origin capture tracked @@ -92,7 +92,7 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature * * The function sets the member new_features_last_, the list of newly detected features. */ - virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out) override; + virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out) override; /** \brief Create a new constraint and link it to the wolf tree * \param _feature_ptr pointer to the parent Feature diff --git a/include/base/processor/processor_tracker_landmark.h b/include/base/processor/processor_tracker_landmark.h index 961e6f5dcb5004ad3c719aa767f1a070ef0d835d..293570afa29406dfc8515ef9c7d684af2da35cf9 100644 --- a/include/base/processor/processor_tracker_landmark.h +++ b/include/base/processor/processor_tracker_landmark.h @@ -93,31 +93,24 @@ class ProcessorTrackerLandmark : public ProcessorTracker /** \brief Tracker function * \return The number of successful tracks. * + * Find Features in \b incoming Capture corresponding to known landmarks in the \b Map. + * * This is the tracker function to be implemented in derived classes. * It operates on the \b incoming capture pointed by incoming_ptr_. * - * This should do one of the following, depending on the design of the tracker: - * - Track Features against other Features in the \b origin Capture. Tips: - * - An intermediary step of matching against Features in the \b last Capture makes tracking easier. - * - Once tracked against last, then the link to Features in \b origin is provided by the Features' Constraints in \b last. - * - If required, correct the drift by re-comparing against the Features in origin. - * - The Constraints in \b last need to be transferred to \b incoming (moved, not copied). - * - * The function must generate the necessary Features in the \b incoming Capture, - * of the correct type, derived from FeatureBase. - * - * It must also generate the constraints, of the correct type, derived from ConstraintBase - * (through ConstraintAnalytic or ConstraintSparse). + * The function must populate the list of Features in the \b incoming Capture. + * Features need to be of the correct type, derived from FeatureBase. */ virtual unsigned int processKnown(); /** \brief Find provided landmarks in the incoming capture - * \param _landmark_list_in input list of landmarks to be found in incoming - * \param _feature_list_out returned list of incoming features corresponding to a landmark of _landmark_list_in + * \param _landmarks_in input list of landmarks to be found in incoming + * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr */ - virtual unsigned int findLandmarks(const LandmarkBaseList& _landmark_list_in, FeatureBaseList& _feature_list_out, - LandmarkMatchMap& _feature_landmark_correspondences) = 0; + virtual unsigned int findLandmarks(const LandmarkBaseList& _landmarks_in, + FeatureBaseList& _features_incoming_out, + LandmarkMatchMap& _feature_landmark_correspondences) = 0; /** \brief Vote for KeyFrame generation * @@ -146,7 +139,7 @@ class ProcessorTrackerLandmark : public ProcessorTracker * The function sets the member new_features_list_, the list of newly detected features, * to be used for landmark initialization. */ - virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out) = 0; + virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out) = 0; /** \brief Creates a landmark for each of new_features_last_ **/ diff --git a/include/base/processor/processor_tracker_landmark_corner.h b/include/base/processor/processor_tracker_landmark_corner.h index a533ce8e32f512d1cd5ac08583b66ce7724d34a1..bd81b1643980f41468180735b85d9dcabd550346 100644 --- a/include/base/processor/processor_tracker_landmark_corner.h +++ b/include/base/processor/processor_tracker_landmark_corner.h @@ -118,11 +118,11 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark } /** \brief Find provided landmarks in the incoming capture - * \param _landmark_list_in input list of landmarks to be found in incoming - * \param _feature_list_out returned list of incoming features corresponding to a landmark of _landmark_list_in + * \param _landmarks_in input list of landmarks to be found in incoming + * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr */ - virtual unsigned int findLandmarks(const LandmarkBaseList& _landmark_list_in, FeatureBaseList& _feature_list_out, + virtual unsigned int findLandmarks(const LandmarkBaseList& _landmarks_in, FeatureBaseList& _features_incoming_out, LandmarkMatchMap& _feature_landmark_correspondences); /** \brief Vote for KeyFrame generation @@ -144,7 +144,7 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark * The function sets the member new_features_list_, the list of newly detected features, * to be used for landmark initialization. */ - virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out); + virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out); /** \brief Create one landmark * diff --git a/include/base/processor/processor_tracker_landmark_dummy.h b/include/base/processor/processor_tracker_landmark_dummy.h index 612a46a5225cefc56e4edf1026b096d8985e0539..1d9d66bbaeef70cdf9366180ff55b796d78b73ac 100644 --- a/include/base/processor/processor_tracker_landmark_dummy.h +++ b/include/base/processor/processor_tracker_landmark_dummy.h @@ -31,11 +31,11 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark virtual void postProcess(); // implemented /** \brief Find provided landmarks in the incoming capture - * \param _landmark_list_in input list of landmarks to be found in incoming - * \param _feature_list_out returned list of incoming features corresponding to a landmark of _landmark_list_in + * \param _landmarks_in input list of landmarks to be found in incoming + * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr */ - virtual unsigned int findLandmarks(const LandmarkBaseList& _landmark_list_in, FeatureBaseList& _feature_list_out, + virtual unsigned int findLandmarks(const LandmarkBaseList& _landmarks_in, FeatureBaseList& _features_incoming_out, LandmarkMatchMap& _feature_landmark_correspondences); /** \brief Vote for KeyFrame generation @@ -56,7 +56,7 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark * The function sets the member new_features_list_, the list of newly detected features, * to be used for landmark initialization. */ - virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out); + virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out); /** \brief Create one landmark * diff --git a/include/base/processor/processor_tracker_landmark_image.h b/include/base/processor/processor_tracker_landmark_image.h index 9c80999cc69435b6a854f5ef5f19f15b4efad4d3..1b7a0dbd6d5d33d3adb40742768b5470bb2a3c11 100644 --- a/include/base/processor/processor_tracker_landmark_image.h +++ b/include/base/processor/processor_tracker_landmark_image.h @@ -111,11 +111,11 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark //Pure virtual /** \brief Find provided landmarks in the incoming capture - * \param _landmark_list_in input list of landmarks to be found in incoming - * \param _feature_list_out returned list of incoming features corresponding to a landmark of _landmark_list_in + * \param _landmarks_in input list of landmarks to be found in incoming + * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr */ - virtual unsigned int findLandmarks(const LandmarkBaseList& _landmark_list_in, FeatureBaseList& _feature_list_out, + virtual unsigned int findLandmarks(const LandmarkBaseList& _landmarks_in, FeatureBaseList& _features_incoming_out, LandmarkMatchMap& _feature_landmark_correspondences); /** \brief Vote for KeyFrame generation @@ -135,7 +135,7 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark * * \return The number of detected Features. */ - virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out); + virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out); /** \brief Create one landmark * diff --git a/include/base/processor/processor_tracker_landmark_polyline.h b/include/base/processor/processor_tracker_landmark_polyline.h index 1bcf88ba1f3be2e0dccfcb1fefe51ee559a5d385..e8d70824a898a85130775aed2bb712945ef4e6fc 100644 --- a/include/base/processor/processor_tracker_landmark_polyline.h +++ b/include/base/processor/processor_tracker_landmark_polyline.h @@ -129,11 +129,11 @@ class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark void resetDerived(); /** \brief Find provided landmarks in the incoming capture - * \param _landmark_list_in input list of landmarks to be found in incoming - * \param _feature_list_out returned list of incoming features corresponding to a landmark of _landmark_list_in + * \param _landmarks_in input list of landmarks to be found in incoming + * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr */ - virtual unsigned int findLandmarks(const LandmarkBaseList& _landmark_list_in, FeatureBaseList& _feature_list_out, + virtual unsigned int findLandmarks(const LandmarkBaseList& _landmarks_in, FeatureBaseList& _features_incoming_out, LandmarkMatchMap& _feature_landmark_correspondences); /** \brief Vote for KeyFrame generation @@ -155,7 +155,7 @@ class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark * The function sets the member new_features_list_, the list of newly detected features, * to be used for landmark initialization. */ - virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out); + virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out); /** \brief Creates a landmark for each of new_features_last_ **/ @@ -210,11 +210,11 @@ inline ProcessorTrackerLandmarkPolyline::ProcessorTrackerLandmarkPolyline(Proces { } -inline unsigned int ProcessorTrackerLandmarkPolyline::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out) +inline unsigned int ProcessorTrackerLandmarkPolyline::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out) { // already computed since each scan is computed in preprocess() - _feature_list_out = std::move(polylines_last_); - return _feature_list_out.size(); + _features_incoming_out = std::move(polylines_last_); + return _features_incoming_out.size(); } inline void ProcessorTrackerLandmarkPolyline::advanceDerived() diff --git a/include/base/sensor/sensor_base.h b/include/base/sensor/sensor_base.h index d2b3b00798dbf1f0134d8cff4cb45307c266a104..8707d13ac8bcac4b288f139dec53045625f02ac9 100644 --- a/include/base/sensor/sensor_base.h +++ b/include/base/sensor/sensor_base.h @@ -38,7 +38,6 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa SizeEigen calib_size_; static unsigned int sensor_id_count_; ///< Object counter (acts as simple ID factory) - bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove(). protected: unsigned int sensor_id_; // sensor ID @@ -90,7 +89,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa const bool _intr_dyn = false); virtual ~SensorBase(); - void remove(); + virtual void remove(); unsigned int id(); diff --git a/include/base/solver/solver_manager.h b/include/base/solver/solver_manager.h index e51cf5e44a092dd89fa56fe96b7c12cb1faa0a08..4801fd57f08f9a546b4255f51f9e11e4ca1bd354 100644 --- a/include/base/solver/solver_manager.h +++ b/include/base/solver/solver_manager.h @@ -55,6 +55,14 @@ public: virtual void computeCovariances(const StateBlockList& st_list) = 0; + virtual bool hasConverged() = 0; + + virtual SizeStd iterations() = 0; + + virtual Scalar initialCost() = 0; + + virtual Scalar finalCost() = 0; + virtual void update(); ProblemPtr getProblemPtr(); diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp index ec0b94f43bc67c00c8cff7ba909b28a180627eeb..051b93c9ff33929900026a534e2948120480d30d 100644 --- a/src/capture/capture_base.cpp +++ b/src/capture/capture_base.cpp @@ -18,7 +18,6 @@ CaptureBase::CaptureBase(const std::string& _type, sensor_ptr_(_sensor_ptr), state_block_vec_(3), calib_size_(0), - is_removing_(false), capture_id_(++capture_id_count_), time_stamp_(_ts) { diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp index 78777b627b211d9ca085e86f651ec012f0a62d13..ae8cb7b32fa1388d992d13c477929e991924f29b 100644 --- a/src/ceres_wrapper/ceres_manager.cpp +++ b/src/ceres_wrapper/ceres_manager.cpp @@ -359,6 +359,26 @@ void CeresManager::updateStateBlockLocalParametrization(const StateBlockPtr& sta addConstraint(ctr); } +bool CeresManager::hasConverged() +{ + return summary_.termination_type == ceres::CONVERGENCE; +} + +SizeStd CeresManager::iterations() +{ + return summary_.iterations.size(); +} + +Scalar CeresManager::initialCost() +{ + return Scalar(summary_.initial_cost); +} + +Scalar CeresManager::finalCost() +{ + return Scalar(summary_.final_cost); +} + ceres::CostFunctionPtr CeresManager::createCostFunction(const ConstraintBasePtr& _ctr_ptr) { assert(_ctr_ptr != nullptr); diff --git a/src/constraint/constraint_base.cpp b/src/constraint/constraint_base.cpp index 38809e7ded690d7a8ee176237ad9ff888ec62d7b..f521cf214ecd8de80205a7dfbe8572433eca353c 100644 --- a/src/constraint/constraint_base.cpp +++ b/src/constraint/constraint_base.cpp @@ -11,7 +11,6 @@ ConstraintBase::ConstraintBase(const std::string& _tp, ConstraintStatus _status) : NodeBase("CONSTRAINT", _tp), feature_ptr_(), // nullptr - is_removing_(false), constraint_id_(++constraint_id_count_), status_(_status), apply_loss_function_(_apply_loss_function), @@ -32,7 +31,6 @@ ConstraintBase::ConstraintBase(const std::string& _tp, bool _apply_loss_function, ConstraintStatus _status) : NodeBase("CONSTRAINT", _tp), feature_ptr_(), - is_removing_(false), constraint_id_(++constraint_id_count_), status_(_status), apply_loss_function_(_apply_loss_function), diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp index d97b7c96f9e2ca56f65ab29fdc451487223a812c..314457be284f5fc16bfba370c41c47f8ff963ffa 100644 --- a/src/feature/feature_base.cpp +++ b/src/feature/feature_base.cpp @@ -9,7 +9,6 @@ unsigned int FeatureBase::feature_id_count_ = 0; FeatureBase::FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) : NodeBase("FEATURE", _type), capture_ptr_(), - is_removing_(false), feature_id_(++feature_id_count_), track_id_(0), landmark_id_(0), diff --git a/src/frame_base.cpp b/src/frame_base.cpp index 4df54a7f21f1f9bdaa7015941c4d6e9333d8f356..5c1b7fee23ab834725340ac91f5df77c9db4b051 100644 --- a/src/frame_base.cpp +++ b/src/frame_base.cpp @@ -15,7 +15,6 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _ NodeBase("FRAME", "Base"), trajectory_ptr_(), state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed. - is_removing_(false), frame_id_(++frame_id_count_), type_(NON_KEY_FRAME), time_stamp_(_ts) @@ -29,7 +28,6 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr NodeBase("FRAME", "Base"), trajectory_ptr_(), state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed. - is_removing_(false), frame_id_(++frame_id_count_), type_(_tp), time_stamp_(_ts) diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp index f99dbff19315462cb165a3ebb9ce68f014d55253..1ae594cb348c66819b3efffdbcf1acf0878e8fa2 100644 --- a/src/landmark/landmark_base.cpp +++ b/src/landmark/landmark_base.cpp @@ -13,7 +13,6 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State NodeBase("LANDMARK", _type), map_ptr_(), state_block_vec_(2), // allow for 2 state blocks by default. Resize in derived constructors if needed. - is_removing_(false), landmark_id_(++landmark_id_count_) { state_block_vec_[0] = _p_ptr; diff --git a/src/problem.cpp b/src/problem.cpp index 7c471cae6aeffaccf46d69472e2eb1b0b0fead8c..65da6a2279bfcd8959f4267945ee813317aaaafd 100644 --- a/src/problem.cpp +++ b/src/problem.cpp @@ -35,25 +35,26 @@ Problem::Problem(const std::string& _frame_structure) : trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)), map_ptr_(std::make_shared<MapBase>()), processor_motion_ptr_(), - state_size_(0), - state_cov_size_(0), prior_is_set_(false) { if (_frame_structure == "PO 2D") { state_size_ = 3; state_cov_size_ = 3; + dim_ = 2; } else if (_frame_structure == "PO 3D") { state_size_ = 7; state_cov_size_ = 6; + dim_ = 3; } else if (_frame_structure == "POV 3D") { state_size_ = 10; state_cov_size_ = 9; + dim_ = 3; } else std::runtime_error( "Problem::Problem(): Unknown frame structure. Add appropriate frame structure to the switch statement."); @@ -342,6 +343,11 @@ void Problem::getFrameStructureSize(SizeEigen& _x_size, SizeEigen& _cov_size) co _cov_size = state_cov_size_; } +SizeEigen Problem::getDim() const +{ + return dim_; +} + Eigen::VectorXs Problem::zeroState() { Eigen::VectorXs state = Eigen::VectorXs::Zero(getFrameStructureSize()); diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp index ec075ffed881383be09e2a91c5ba8096c32dfc2c..aa259d706262904969e9e27245a50e06456584a0 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -11,8 +11,7 @@ ProcessorBase::ProcessorBase(const std::string& _type, ProcessorParamsBasePtr _p NodeBase("PROCESSOR", _type, _params->name), processor_id_(++processor_id_count_), params_(_params), - sensor_ptr_(), - is_removing_(false) + sensor_ptr_() { // WOLF_DEBUG("constructed +p" , id()); } @@ -96,6 +95,12 @@ PackKeyFramePtr PackKeyFrameBuffer::selectPack(const TimeStamp& _time_stamp, con PackKeyFrameBuffer::Iterator post = container_.upper_bound(_time_stamp); + // remove packs corresponding to removed KFs (keeping the next iterator in post) + while (post != container_.end() && post->second->key_frame->isRemoving()) + post = container_.erase(post); + while (post != container_.begin() && std::prev(post)->second->key_frame->isRemoving()) + container_.erase(std::prev(post)); + bool prev_exists = (post != container_.begin()); bool post_exists = (post != container_.end()); @@ -131,33 +136,33 @@ PackKeyFramePtr PackKeyFrameBuffer::selectPack(const CaptureBasePtr _capture, co return selectPack(_capture->getTimeStamp(), _time_tolerance); } -PackKeyFramePtr PackKeyFrameBuffer::selectPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance) +PackKeyFramePtr PackKeyFrameBuffer::selectFirstPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance) { - if (container_.empty()) - return nullptr; + // remove packs corresponding to removed KFs + while (!container_.empty() && container_.begin()->second->key_frame->isRemoving()) + container_.erase(container_.begin()); - PackKeyFrameBuffer::Iterator post = container_.upper_bound(_time_stamp); + // There is no pack + if (container_.empty()) + return nullptr; - bool prev_exists = (post != container_.begin()); + // Checking on begin() since packs are ordered in time + // Return first pack if is older than time stamp + if (container_.begin()->first < _time_stamp) + return container_.begin()->second; - if (prev_exists) + // Return first pack if despite being newer, it is within the time tolerance + if (checkTimeTolerance(container_.begin()->first, container_.begin()->second->time_tolerance, _time_stamp, _time_tolerance)) return container_.begin()->second; - else - { - bool post_exists = (post != container_.end()); - bool post_ok = post_exists && checkTimeTolerance(post->first, post->second->time_tolerance, _time_stamp, _time_tolerance); - - if (post_ok) - return post->second; - } - + // otherwise return nullptr (no pack before the provided ts or within the tolerance was found) return nullptr; + } -PackKeyFramePtr PackKeyFrameBuffer::selectPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance) +PackKeyFramePtr PackKeyFrameBuffer::selectFirstPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance) { - return selectPackBefore(_capture->getTimeStamp(), _time_tolerance); + return selectFirstPackBefore(_capture->getTimeStamp(), _time_tolerance); } void PackKeyFrameBuffer::print(void) diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index f401cb1202b6feba596d54eedd15e82f41979228..0fe59c59aa026654b2ed5a3d8b924b3d5cfdee48 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/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_) @@ -243,7 +243,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. @@ -268,7 +268,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); @@ -290,35 +290,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); @@ -381,12 +352,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_, @@ -402,11 +373,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(), @@ -495,8 +469,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(); @@ -512,7 +531,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 @@ -527,7 +546,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 @@ -578,7 +597,7 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep() throw std::runtime_error("ProcessorMotion received data before being initialized."); } - PackKeyFramePtr pack = kf_pack_buffer_.selectPackBefore(last_ptr_, params_motion_->time_tolerance); + PackKeyFramePtr pack = kf_pack_buffer_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance); if (pack) { @@ -590,7 +609,7 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep() // throw std::runtime_error("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)"); processing_step_ = RUNNING_WITH_PACK_ON_ORIGIN; } - else if (pack->key_frame->getTimeStamp() < origin_ptr_->getTimeStamp() - params_motion_->time_tolerance) + else if (pack->key_frame->getTimeStamp() < origin_ptr_->getTimeStamp()) processing_step_ = RUNNING_WITH_PACK_BEFORE_ORIGIN; else diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp index a0f584296bec01b657c0219d5cf0baf5e531e448..c6031386542c0e92385877a3e5905d5eb9e7684e 100644 --- a/src/processor/processor_odom_2D.cpp +++ b/src/processor/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/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp index 1c017e2095eb9c82ba3895cb1eb9e82249bdf034..f8138e1fc7e1b0d152bf0576ee6fcb454e75ca0b 100644 --- a/src/processor/processor_tracker.cpp +++ b/src/processor/processor_tracker.cpp @@ -61,8 +61,9 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) pack->key_frame->addCapture(incoming_ptr_); // Process info + // TrackerFeature: We only process new features in Last, here last = nullptr, so we do not have anything to do. + // TrackerLandmark: If we have given a map, all landmarks in the map are know. Process them. processKnown(); - // We only process new features in Last, here last = nullptr, so we do not have anything to do. // Update pointers resetDerived(); diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp index c1c2210e393449e82585840a6fa6f82ae73ecdef..cfb6aa984f179b891cfc43f32524625e8d070a16 100644 --- a/src/processor/processor_tracker_feature.cpp +++ b/src/processor/processor_tracker_feature.cpp @@ -148,9 +148,20 @@ void ProcessorTrackerFeature::establishConstraints() FeatureBasePtr feature_in_origin = pair_trkid_pair.second.first; FeatureBasePtr feature_in_last = pair_trkid_pair.second.second; - auto constraint = createConstraint(feature_in_last, feature_in_origin); - feature_in_last ->addConstraint(constraint); - feature_in_origin->addConstrainedBy(constraint); + auto ctr_ptr = createConstraint(feature_in_last, feature_in_origin); + feature_in_last ->addConstraint(ctr_ptr); + feature_in_origin->addConstrainedBy(ctr_ptr); + + if (ctr_ptr != nullptr) // constraint links + { + FrameBasePtr frm = ctr_ptr->getFrameOtherPtr(); + if (frm) + frm->addConstrainedBy(ctr_ptr); + CaptureBasePtr cap = ctr_ptr->getCaptureOtherPtr(); + if (cap) + cap->addConstrainedBy(ctr_ptr); + } + WOLF_DEBUG( "Constraint: track: " , feature_in_last->trackId(), " origin: " , feature_in_origin->id() , diff --git a/src/processor/processor_tracker_feature_corner.cpp b/src/processor/processor_tracker_feature_corner.cpp index b840602c086660a67d50c379afb848933c1dd5f4..0a8e7c7c232fd465f07c3704bbf63f7d615a20cc 100644 --- a/src/processor/processor_tracker_feature_corner.cpp +++ b/src/processor/processor_tracker_feature_corner.cpp @@ -70,14 +70,14 @@ void ProcessorTrackerFeatureCorner::advanceDerived() corners_last_ = std::move(corners_incoming_); } -unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBaseList& _feature_list_in, - FeatureBaseList& _feature_list_out, +unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBaseList& _features_last_in, + FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_correspondences) { - std::cout << "tracking " << _feature_list_in.size() << " features..." << std::endl; + std::cout << "tracking " << _features_last_in.size() << " features..." << std::endl; Eigen::Vector3s expected_feature_pose; - for (auto feat_in_ptr : _feature_list_in) + for (auto feat_in_ptr : _features_last_in) { expected_feature_pose = R_current_prev_ * feat_in_ptr->getMeasurement().head<3>() + t_current_prev_; @@ -91,7 +91,7 @@ unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBaseList& _feature_correspondences[*feat_out_it] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in_ptr,0})); // move matched feature to list - _feature_list_out.splice(_feature_list_out.end(), corners_incoming_, feat_out_it); + _features_incoming_out.splice(_features_incoming_out.end(), corners_incoming_, feat_out_it); std::cout << "feature " << feat_in_ptr->id() << " tracked!" << std::endl; } @@ -99,7 +99,7 @@ unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBaseList& } } - return _feature_list_out.size(); + return _features_incoming_out.size(); } bool ProcessorTrackerFeatureCorner::voteForKeyFrame() @@ -107,11 +107,11 @@ bool ProcessorTrackerFeatureCorner::voteForKeyFrame() return incoming_ptr_->getFeatureList().size() < params_tracker_feature_corner_->n_corners_th; } -unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out) +unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out) { // in corners_last_ remain all not tracked corners - _feature_list_out = std::move(corners_last_); - return _feature_list_out.size(); + _features_incoming_out = std::move(corners_last_); + return _features_incoming_out.size(); } ConstraintBasePtr ProcessorTrackerFeatureCorner::createConstraint(FeatureBasePtr _feature_ptr, diff --git a/src/processor/processor_tracker_feature_dummy.cpp b/src/processor/processor_tracker_feature_dummy.cpp index c218b04fd3531fbd1aaa9a4ade0acd90e0b8b273..6ce3d4fe18fbabc83d63582a9ac2090c63ce53be 100644 --- a/src/processor/processor_tracker_feature_dummy.cpp +++ b/src/processor/processor_tracker_feature_dummy.cpp @@ -13,13 +13,13 @@ namespace wolf unsigned int ProcessorTrackerFeatureDummy::count_ = 0; -unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList& _feature_list_in, - FeatureBaseList& _feature_list_out, +unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList& _features_last_in, + FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_correspondences) { - WOLF_INFO("tracking " , _feature_list_in.size() , " features..."); + WOLF_INFO("tracking " , _features_last_in.size() , " features..."); - for (auto feat_in : _feature_list_in) + for (auto feat_in : _features_last_in) { if (++count_ % 3 == 2) // lose one every 3 tracks { @@ -28,14 +28,14 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList& else { FeatureBasePtr ftr(std::make_shared<FeatureBase>("POINT IMAGE", feat_in->getMeasurement(), feat_in->getMeasurementCovariance())); - _feature_list_out.push_back(ftr); - _feature_correspondences[_feature_list_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0})); + _features_incoming_out.push_back(ftr); + _feature_correspondences[_features_incoming_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0})); WOLF_INFO("track: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , ftr->id() , " !" ); } } - return _feature_list_out.size(); + return _features_incoming_out.size(); } bool ProcessorTrackerFeatureDummy::voteForKeyFrame() @@ -49,7 +49,7 @@ bool ProcessorTrackerFeatureDummy::voteForKeyFrame() return incoming_ptr_->getFeatureList().size() < params_tracker_feature_->min_features_for_keyframe; } -unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out) +unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out) { WOLF_INFO("Detecting " , _max_features , " new features..." ); @@ -60,14 +60,14 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int& FeatureBasePtr ftr(std::make_shared<FeatureBase>("POINT IMAGE", n_feature_* Eigen::Vector1s::Ones(), Eigen::MatrixXs::Ones(1, 1))); - _feature_list_out.push_back(ftr); + _features_incoming_out.push_back(ftr); WOLF_INFO("feature " , ftr->id() , " detected!" ); } - WOLF_INFO(_feature_list_out.size() , " features detected!"); + WOLF_INFO(_features_incoming_out.size() , " features detected!"); - return _feature_list_out.size(); + return _features_incoming_out.size(); } ConstraintBasePtr ProcessorTrackerFeatureDummy::createConstraint(FeatureBasePtr _feature_ptr, diff --git a/src/processor/processor_tracker_feature_image.cpp b/src/processor/processor_tracker_feature_image.cpp index 716a2cca77b6a8196955a9549490d138b4d93868..486cd623c75ca91b63ef63abca62d3b0bf552be0 100644 --- a/src/processor/processor_tracker_feature_image.cpp +++ b/src/processor/processor_tracker_feature_image.cpp @@ -135,14 +135,14 @@ void ProcessorTrackerFeatureImage::postProcess() { } -unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out, +unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_matches) { KeyPointVector candidate_keypoints; cv::Mat candidate_descriptors; DMatchVector cv_matches; - for (auto feature_base_ptr : _feature_list_in) + for (auto feature_base_ptr : _features_last_in) { FeaturePointImagePtr feature_ptr = std::static_pointer_cast<FeaturePointImage>(feature_base_ptr); @@ -167,7 +167,7 @@ unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBaseList& (candidate_descriptors.row(cv_matches[0].trainIdx)), Eigen::Matrix2s::Identity()*params_tracker_feature_image_->pixel_noise_var); incoming_point_ptr->setIsKnown(feature_ptr->isKnown()); - _feature_list_out.push_back(incoming_point_ptr); + _features_incoming_out.push_back(incoming_point_ptr); _feature_matches[incoming_point_ptr] = std::make_shared<FeatureMatch>(FeatureMatch({feature_base_ptr, normalized_score})); } @@ -185,8 +185,8 @@ unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBaseList& tracker_roi_.pop_back(); } } -// std::cout << "TrackFeatures - Number of Features tracked: " << _feature_list_out.size() << std::endl; - return _feature_list_out.size(); +// std::cout << "TrackFeatures - Number of Features tracked: " << _features_incoming_out.size() << std::endl; + return _features_incoming_out.size(); } bool ProcessorTrackerFeatureImage::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature) @@ -244,7 +244,7 @@ bool ProcessorTrackerFeatureImage::correctFeatureDrift(const FeatureBasePtr _ori } } -unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out) +unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out) { cv::Rect roi; KeyPointVector new_keypoints; @@ -277,7 +277,7 @@ unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int& new_descriptors.row(index), Eigen::Matrix2s::Identity()*params_tracker_feature_image_->pixel_noise_var); point_ptr->setIsKnown(false); - _feature_list_out.push_back(point_ptr); + _features_incoming_out.push_back(point_ptr); active_search_ptr_->hitCell(new_keypoints[0]); diff --git a/src/processor/processor_tracker_feature_trifocal.cpp b/src/processor/processor_tracker_feature_trifocal.cpp index 6a0fa198f7740767641d190b6191e4acb6eb13a8..03cd93f17f9a21b33493cc1cf76a45046061cb4a 100644 --- a/src/processor/processor_tracker_feature_trifocal.cpp +++ b/src/processor/processor_tracker_feature_trifocal.cpp @@ -134,7 +134,8 @@ bool ProcessorTrackerFeatureTrifocal::isInlier(const cv::KeyPoint& _kp_last, con return inlier; } -unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out) + +unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out) { // // DEBUG ===================================== // clock_t debug_tStart; @@ -174,7 +175,7 @@ unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned i capture_last_->descriptors_.row(index_last), Eigen::Matrix2s::Identity() * params_tracker_feature_trifocal_->pixel_noise_std * params_tracker_feature_trifocal_->pixel_noise_std); - _feature_list_out.push_back(last_point_ptr); + _features_incoming_out.push_back(last_point_ptr); // hit cell to acknowledge there's a tracked point in that cell capture_last_->grid_features_->hitTrackingCell(kp_last); @@ -192,17 +193,17 @@ unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned i break; // There are no empty cells } - WOLF_TRACE("DetectNewFeatures - Number of new features detected: " , _feature_list_out.size() ); + WOLF_TRACE("DetectNewFeatures - Number of new features detected: " , _features_incoming_out.size() ); // // DEBUG // debug_comp_time_ = (double)(clock() - debug_tStart) / CLOCKS_PER_SEC; // WOLF_TRACE("--> TIME: detect new features: TOTAL ",debug_comp_time_); // WOLF_TRACE("======== ========= ========="); - return _feature_list_out.size(); + return _features_incoming_out.size(); } -unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out, FeatureMatchMap& _feature_matches) +unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_matches) { // // DEBUG ===================================== // clock_t debug_tStart; @@ -211,7 +212,7 @@ unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBaseLis // WOLF_TRACE("======== TrackFeatures ========="); // // =========================================== - for (auto feature_base_last_ : _feature_list_in) + for (auto feature_base_last_ : _features_last_in) { FeaturePointImagePtr feature_last_ = std::static_pointer_cast<FeaturePointImage>(feature_base_last_); @@ -233,7 +234,7 @@ unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBaseLis capture_incoming_->descriptors_.row(index_kp_incoming), Eigen::Matrix2s::Identity() * params_tracker_feature_trifocal_->pixel_noise_std * params_tracker_feature_trifocal_->pixel_noise_std); - _feature_list_out.push_back(incoming_point_ptr); + _features_incoming_out.push_back(incoming_point_ptr); _feature_matches[incoming_point_ptr] = std::make_shared<FeatureMatch>(FeatureMatch({feature_last_, capture_incoming_->matches_normalized_scores_.at(index_kp_incoming)})); @@ -249,9 +250,9 @@ unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBaseLis // WOLF_TRACE("--> TIME: track: ",debug_comp_time_); // WOLF_TRACE("======== ========= ========="); - WOLF_TRACE("TrAckFeatures - Number of features tracked: " , _feature_list_out.size() ); + WOLF_TRACE("TrAckFeatures - Number of features tracked: " , _features_incoming_out.size() ); - return _feature_list_out.size(); + return _features_incoming_out.size(); } bool ProcessorTrackerFeatureTrifocal::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature) diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp index 1771f97f422798f6b2d42392b1d677d3d9478e23..08bbf8c52b66b01a49fa49389fce11da4affe0b1 100644 --- a/src/processor/processor_tracker_landmark.cpp +++ b/src/processor/processor_tracker_landmark.cpp @@ -115,15 +115,6 @@ void ProcessorTrackerLandmark::createNewLandmarks() unsigned int ProcessorTrackerLandmark::processKnown() { - - //std::cout << "ProcessorTrackerLandmark::processKnown:" << std::endl; - //std::cout << "\tlast correspondences: " << matches_landmark_from_last_.size() << std::endl; - //std::cout << "\tlast features: " << (last_ptr_ == nullptr ? 0 : last_ptr_->getFeatureList().size()) << std::endl; - //std::cout << "\tlast new features: " << new_features_last_.size() << std::endl; - //std::cout << "\tincoming correspondences: " << matches_landmark_from_incoming_.size() << std::endl; - //std::cout << "\tincoming features: " << (incoming_ptr_ == nullptr ? 0 : incoming_ptr_->getFeatureList().size()) << std::endl; - //std::cout << "\tincoming new features: " << new_features_incoming_.size() << std::endl; - // Find landmarks in incoming_ptr_ FeatureBaseList known_features_list_incoming; unsigned int n = findLandmarks(getProblem()->getMapPtr()->getLandmarkList(), @@ -131,22 +122,12 @@ unsigned int ProcessorTrackerLandmark::processKnown() // Append found incoming features incoming_ptr_->addFeatureList(known_features_list_incoming); - //std::cout << "end of processKnown:" << std::endl; - //std::cout << "\tlast correspondences: " << matches_landmark_from_last_.size() << std::endl; - //std::cout << "\tlast features: " << (last_ptr_ == nullptr ? 0 : last_ptr_->getFeatureList().size()) << std::endl; - //std::cout << "\tlast new features: " << new_features_last_.size() << std::endl; - //std::cout << "\tincoming correspondences: " << matches_landmark_from_incoming_.size() << std::endl; - //std::cout << "\tincoming features: " << (incoming_ptr_ == nullptr ? 0 : incoming_ptr_->getFeatureList().size()) << std::endl; - //std::cout << "\tincoming new features: " << new_features_incoming_.size() << std::endl; return n; - } void ProcessorTrackerLandmark::establishConstraints() { - - //std::cout << "\tfeatures:" << last_ptr_->getFeatureList().size() << std::endl; - //std::cout << "\tcorrespondences: " << matches_landmark_from_last_.size() << std::endl; + // Loop all features in last_ptr_ for (auto last_feature : last_ptr_->getFeatureList()) { auto lmk = matches_landmark_from_last_[last_feature]->landmark_ptr_; @@ -159,6 +140,9 @@ void ProcessorTrackerLandmark::establishConstraints() FrameBasePtr frm = ctr_ptr->getFrameOtherPtr(); if (frm) frm->addConstrainedBy(ctr_ptr); + CaptureBasePtr cap = ctr_ptr->getCaptureOtherPtr(); + if (cap) + cap->addConstrainedBy(ctr_ptr); } } } diff --git a/src/processor/processor_tracker_landmark_corner.cpp b/src/processor/processor_tracker_landmark_corner.cpp index 706e12b2b48cad9d8a9b316f69ca4ebbc9b8d2e9..522034204bb5dd4f4b3140d3f55db773faa070af 100644 --- a/src/processor/processor_tracker_landmark_corner.cpp +++ b/src/processor/processor_tracker_landmark_corner.cpp @@ -375,11 +375,11 @@ LandmarkBasePtr ProcessorTrackerLandmarkCorner::createLandmark(FeatureBasePtr _f _feature_ptr->getMeasurement()(3)); } -unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out) +unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out) { // already computed since each scan is computed in preprocess() - _feature_list_out = std::move(corners_last_); - return _feature_list_out.size(); + _features_incoming_out = std::move(corners_last_); + return _features_incoming_out.size(); } ConstraintBasePtr ProcessorTrackerLandmarkCorner::createConstraint(FeatureBasePtr _feature_ptr, diff --git a/src/processor/processor_tracker_landmark_dummy.cpp b/src/processor/processor_tracker_landmark_dummy.cpp index f20af380b631d6cd27539941e761876b2f5a1dea..06c10b37f1f8b9607b773c2c808e101c39dfb412 100644 --- a/src/processor/processor_tracker_landmark_dummy.cpp +++ b/src/processor/processor_tracker_landmark_dummy.cpp @@ -26,16 +26,16 @@ ProcessorTrackerLandmarkDummy::~ProcessorTrackerLandmarkDummy() // } -unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBaseList& _landmark_list_in, - FeatureBaseList& _feature_list_out, +unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBaseList& _landmarks_in, + FeatureBaseList& _features_incoming_out, LandmarkMatchMap& _feature_landmark_correspondences) { std::cout << "\tProcessorTrackerLandmarkDummy::findLandmarks" << std::endl; - std::cout << "\t\t" << _landmark_list_in.size() << " landmarks..." << std::endl; + std::cout << "\t\t" << _landmarks_in.size() << " landmarks..." << std::endl; // loosing the track of the first 2 features auto landmarks_lost = 0; - for (auto landmark_in_ptr : _landmark_list_in) + for (auto landmark_in_ptr : _landmarks_in) { if (landmark_in_ptr->getDescriptor(0) <= landmark_idx_non_visible_) { @@ -44,15 +44,15 @@ unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBaseList } else { - _feature_list_out.push_back(std::make_shared<FeatureBase>( + _features_incoming_out.push_back(std::make_shared<FeatureBase>( "POINT IMAGE", landmark_in_ptr->getDescriptor(), Eigen::MatrixXs::Identity(1,1))); - _feature_landmark_correspondences[_feature_list_out.back()] = std::make_shared<LandmarkMatch>(landmark_in_ptr, 1); + _feature_landmark_correspondences[_features_incoming_out.back()] = std::make_shared<LandmarkMatch>(landmark_in_ptr, 1); std::cout << "\t\tlandmark " << landmark_in_ptr->getDescriptor() << " found!" << std::endl; } } - return _feature_list_out.size(); + return _features_incoming_out.size(); } bool ProcessorTrackerLandmarkDummy::voteForKeyFrame() @@ -63,7 +63,7 @@ bool ProcessorTrackerLandmarkDummy::voteForKeyFrame() return incoming_ptr_->getFeatureList().size() < 4; } -unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out) +unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out) { std::cout << "\tProcessorTrackerLandmarkDummy::detectNewFeatures" << std::endl; @@ -71,11 +71,11 @@ unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const unsigned int for (unsigned int i = 1; i <= _max_features; i++) { n_feature_++; - _feature_list_out.push_back( + _features_incoming_out.push_back( std::make_shared<FeatureBase>("POINT IMAGE", n_feature_ * Eigen::Vector1s::Ones(), Eigen::MatrixXs::Ones(1, 1))); - std::cout << "\t\tfeature " << _feature_list_out.back()->getMeasurement() << " detected!" << std::endl; + std::cout << "\t\tfeature " << _features_incoming_out.back()->getMeasurement() << " detected!" << std::endl; } - return _feature_list_out.size(); + return _features_incoming_out.size(); } LandmarkBasePtr ProcessorTrackerLandmarkDummy::createLandmark(FeatureBasePtr _feature_ptr) diff --git a/src/processor/processor_tracker_landmark_image.cpp b/src/processor/processor_tracker_landmark_image.cpp index 3835cbf60595dc497e98ebd9d68f0b2f577196d0..b055f1353a95302d17f11824744e6c0df482a7db 100644 --- a/src/processor/processor_tracker_landmark_image.cpp +++ b/src/processor/processor_tracker_landmark_image.cpp @@ -144,8 +144,8 @@ void ProcessorTrackerLandmarkImage::postProcess() feat_lmk_found_.clear(); } -unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList& _landmark_list_in, - FeatureBaseList& _feature_list_out, +unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList& _landmarks_in, + FeatureBaseList& _features_incoming_out, LandmarkMatchMap& _feature_landmark_correspondences) { KeyPointVector candidate_keypoints; @@ -154,7 +154,7 @@ unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList Eigen::VectorXs current_state = getProblem()->getState(incoming_ptr_->getTimeStamp()); - for (auto landmark_in_ptr : _landmark_list_in) + for (auto landmark_in_ptr : _landmarks_in) { // project landmark into incoming capture @@ -194,7 +194,7 @@ unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList incoming_point_ptr->setScore(normalized_score); incoming_point_ptr->setExpectation(pixel); - _feature_list_out.push_back(incoming_point_ptr); + _features_incoming_out.push_back(incoming_point_ptr); _feature_landmark_correspondences[incoming_point_ptr] = std::make_shared<LandmarkMatch>(landmark_in_ptr, normalized_score); @@ -214,9 +214,9 @@ unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList // else // std::cout << "NOT in the image" << std::endl; } -// std::cout << "\tNumber of Features tracked: " << _feature_list_out.size() << std::endl; - landmarks_tracked_ = _feature_list_out.size(); - return _feature_list_out.size(); +// std::cout << "\tNumber of Features tracked: " << _features_incoming_out.size() << std::endl; + landmarks_tracked_ = _features_incoming_out.size(); + return _features_incoming_out.size(); } bool ProcessorTrackerLandmarkImage::voteForKeyFrame() @@ -225,7 +225,7 @@ bool ProcessorTrackerLandmarkImage::voteForKeyFrame() // return landmarks_tracked_ < params_tracker_landmark_image_->min_features_for_keyframe; } -unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out) +unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out) { cv::Rect roi; KeyPointVector new_keypoints; @@ -260,7 +260,7 @@ unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int point_ptr->setIsKnown(false); point_ptr->setTrackId(point_ptr->id()); point_ptr->setExpectation(Eigen::Vector2s(new_keypoints[0].pt.x,new_keypoints[0].pt.y)); - _feature_list_out.push_back(point_ptr); + _features_incoming_out.push_back(point_ptr); active_search_ptr_->hitCell(new_keypoints[0]); diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index 2367c480d373a7bab8541fcb610bee2c08e45aea..a05fec1ceb4b1b90912dd4ec380212848c68f98b 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -16,7 +16,6 @@ SensorBase::SensorBase(const std::string& _type, hardware_ptr_(), state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed. calib_size_(0), - is_removing_(false), sensor_id_(++sensor_id_count_), // simple ID factory extrinsic_dynamic_(_extr_dyn), intrinsic_dynamic_(_intr_dyn), @@ -43,7 +42,6 @@ SensorBase::SensorBase(const std::string& _type, hardware_ptr_(), state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed. calib_size_(0), - is_removing_(false), sensor_id_(++sensor_id_count_), // simple ID factory extrinsic_dynamic_(_extr_dyn), intrinsic_dynamic_(_intr_dyn), diff --git a/src/sensor/sensor_odom_2D.cpp b/src/sensor/sensor_odom_2D.cpp index ac9e1bf771adbda48bbb41b248ba461dbe8b0560..2790ebb568673acff94b1e246d8f8a0ec35f5de4 100644 --- a/src/sensor/sensor_odom_2D.cpp +++ b/src/sensor/sensor_odom_2D.cpp @@ -80,8 +80,6 @@ SensorBasePtr SensorOdom2D::createNew(const std::string& _unique_name, const par return odo; } -} // namespace wolf - // Register in the SensorFactory #include "base/sensor/sensor_factory.h" namespace wolf { diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 8126f71f84c5efe5ca2a171483917e179c9b2ffc..454a7bf205facc449a614f802a72cc00bb6ba09b 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -98,7 +98,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/test/gtest_IMU_tools.cpp b/test/gtest_IMU_tools.cpp index 83f4000caff84d4d4c5c326a34e24e6ad189adec..e41007154f27c8c6de6845db934215df22d7c382 100644 --- a/test/gtest_IMU_tools.cpp +++ b/test/gtest_IMU_tools.cpp @@ -128,7 +128,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); @@ -138,11 +138,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/test/gtest_SE3.cpp b/test/gtest_SE3.cpp new file mode 100644 index 0000000000000000000000000000000000000000..d6e3b9b1c2a2154677f4a2a77094b4bac93146f0 --- /dev/null +++ b/test/gtest_SE3.cpp @@ -0,0 +1,236 @@ +/** + * \file gtest_SE3.cpp + * + * Created on: Feb 2, 2019 + * \author: jsola + */ + + +#include "base/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/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp index 6faccee27cc478702ac1ac6a3f62e820e5dcba9e..d1ec1884cc79fb858f4b97f8078ba8281c508753 100644 --- a/test/gtest_capture_base.cpp +++ b/test/gtest_capture_base.cpp @@ -102,6 +102,15 @@ TEST(CaptureBase, addFeatureList) ASSERT_EQ(C->getFeatureList().back(), f_last); } +TEST(CaptureBase, process) +{ + SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2)); + CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, nullptr)); + ASSERT_DEATH({C->process();},""); // No sensor in the capture should fail + C->setSensorPtr(S); + ASSERT_TRUE(C->process()); // This should not fail (although it does nothing) +} + int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp index 65c94f02bc7efd810829187d0e9c2501721a5689..fcf60798e88d353bf18371d2a5c3f455b98f597e 100644 --- a/test/gtest_odom_2D.cpp +++ b/test/gtest_odom_2D.cpp @@ -122,7 +122,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/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp index 919727a4fe2c7e6d19db945e5f70e8b09f65b1c3..8f68a5bd532c5a1408d0ba9654ec9f27283c7ba3 100644 --- a/test/gtest_pack_KF_buffer.cpp +++ b/test/gtest_pack_KF_buffer.cpp @@ -144,7 +144,7 @@ TEST_F(BufferPackKeyFrameTest, selectPack) } } -TEST_F(BufferPackKeyFrameTest, selectPackBefore) +TEST_F(BufferPackKeyFrameTest, selectFirstPackBefore) { pack_kf_buffer.clear(); @@ -194,7 +194,7 @@ TEST_F(BufferPackKeyFrameTest, selectPackBefore) int j = 0; for (auto ts : q_ts) { - packQ = pack_kf_buffer.selectPackBefore(ts, tt); + packQ = pack_kf_buffer.selectFirstPackBefore(ts, tt); if (packQ) res(i,j) = packQ->key_frame->getTimeStamp().get(); diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp index 573b7b1791459c03c52dd89ffd98d4d44da52413..3e379c9a5dd72300a5c2cfa614d2d5b5933f9542 100644 --- a/test/gtest_processor_motion.cpp +++ b/test/gtest_processor_motion.cpp @@ -28,13 +28,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() @@ -51,18 +55,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); - } - }; TEST_F(ProcessorMotion_test, IntegrateStraight) @@ -90,7 +82,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); @@ -105,13 +97,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); diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp index 076e5b82d7d7d69e02809090c7f2d4bc474b6034..9d5fe610b2ef612d2f21d8be00bfafd55e2fbca9 100644 --- a/test/gtest_solver_manager.cpp +++ b/test/gtest_solver_manager.cpp @@ -63,6 +63,14 @@ class SolverManagerWrapper : public SolverManager virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){}; virtual void computeCovariances(const StateBlockList& st_list){}; + // The following are dummy implementations + bool hasConverged() { return true; } + SizeStd iterations() { return 1; } + Scalar initialCost() { return Scalar(1); } + Scalar finalCost() { return Scalar(0); } + + + protected: virtual std::string solveImpl(const ReportVerbosity report_level){ return std::string("");};