diff --git a/src/three_D_tools.h b/src/three_D_tools.h index ea3386b30aed3011d055dd77875db7ace06b52e4..73ec2d413475ccd8ca82a4b9db14f84787d38509 100644 --- a/src/three_D_tools.h +++ b/src/three_D_tools.h @@ -43,6 +43,7 @@ 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(); } @@ -50,6 +51,8 @@ inline void identity(MatrixBase<D1>& p, QuaternionBase<D2>& q) 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); @@ -134,7 +137,7 @@ inline void compose(const MatrixBase<D1>& d1, template<typename D1, typename D2> inline Matrix<typename D1::Scalar, 7, 1> compose(const MatrixBase<D1>& d1, - const MatrixBase<D2>& d2 ) + const MatrixBase<D2>& d2 ) { Matrix<typename D1::Scalar, 7, 1> ret; compose(d1, d2, ret); @@ -209,6 +212,8 @@ 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; @@ -255,8 +260,11 @@ 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) { - plus_p = dp1 + dp2; - plus_q = dq1 * exp_q(do2); + 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>