Commit ae166692 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

[C++] Add size checks in LieGroupOperationBase

parent aacfb2e7
...@@ -118,6 +118,9 @@ namespace se3 ...@@ -118,6 +118,9 @@ namespace se3
const Eigen::MatrixBase<Tangent_t> & v, const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<ConfigOut_t>& qout) const Eigen::MatrixBase<ConfigOut_t>& qout)
{ {
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigIn_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigOut_t, ConfigVector_t);
Derived::integrate_impl(q, v, qout); Derived::integrate_impl(q, v, qout);
} }
...@@ -149,7 +152,9 @@ namespace se3 ...@@ -149,7 +152,9 @@ namespace se3
const Scalar& u, const Scalar& u,
const Eigen::MatrixBase<ConfigOut_t>& qout) const Eigen::MatrixBase<ConfigOut_t>& qout)
{ {
// assert(is_config_vector_size(q0) && is_config_vector_size(q1)); EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigOut_t, ConfigVector_t);
Derived::interpolate_impl(q0, q1, u, qout); Derived::interpolate_impl(q0, q1, u, qout);
} }
...@@ -205,7 +210,12 @@ namespace se3 ...@@ -205,7 +210,12 @@ namespace se3
static void randomConfiguration(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit, static void randomConfiguration(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit, const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
const Eigen::MatrixBase<ConfigOut_t> & qout) const Eigen::MatrixBase<ConfigOut_t> & qout)
{ Derived::randomConfiguration_impl(lower_pos_limit, upper_pos_limit, qout); } {
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigOut_t, ConfigVector_t);
Derived::randomConfiguration_impl(lower_pos_limit, upper_pos_limit, qout);
}
/** /**
* @brief the tangent vector that must be integrated during one unit time to go from q0 to q1 * @brief the tangent vector that must be integrated during one unit time to go from q0 to q1
...@@ -229,7 +239,9 @@ namespace se3 ...@@ -229,7 +239,9 @@ namespace se3
const Eigen::MatrixBase<ConfigR_t> & q1, const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<Tangent_t>& d) const Eigen::MatrixBase<Tangent_t>& d)
{ {
// assert(is_config_vector_size(q0) && is_config_vector_size(q1)); EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t, TangentVector_t);
Derived::difference_impl(q0, q1, d); Derived::difference_impl(q0, q1, d);
} }
...@@ -244,7 +256,11 @@ namespace se3 ...@@ -244,7 +256,11 @@ namespace se3
template <class ConfigL_t, class ConfigR_t> template <class ConfigL_t, class ConfigR_t>
static double squaredDistance(const Eigen::MatrixBase<ConfigL_t> & q0, static double squaredDistance(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1) const Eigen::MatrixBase<ConfigR_t> & q1)
{ return Derived::squaredDistance_impl(q0, q1); } {
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
return Derived::squaredDistance_impl(q0, q1);
}
template <class ConfigL_t, class ConfigR_t> template <class ConfigL_t, class ConfigR_t>
static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0, static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
...@@ -299,7 +315,11 @@ namespace se3 ...@@ -299,7 +315,11 @@ namespace se3
static bool isSameConfiguration(const Eigen::MatrixBase<ConfigL_t> & q0, static bool isSameConfiguration(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1, const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
{ return Derived::isSameConfiguration_impl(q0,q1, prec); } {
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
return Derived::isSameConfiguration_impl(q0, q1, prec);
}
template <class ConfigL_t, class ConfigR_t> template <class ConfigL_t, class ConfigR_t>
static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0, static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment