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

Merge branch 'threeD' into 'master'

Add some size checks and the like in three_D_tools.h

See merge request mobile_robotics/wolf!203
parents 446ad278 9f9abdaf
No related branches found
No related tags found
1 merge request!203Add some size checks and the like in 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>
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment