Unverified Commit 5d0fb00f authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #504 from jcarpent/devel

Fix bug related to new version of Eigen 3.3.5
parents e37ac7fe a23d7ffb
......@@ -118,24 +118,23 @@ namespace se3
template <class ConfigIn_t, class Tangent_t, class ConfigOut_t>
struct LieGroupIntegrateVisitor : visitor::LieGroupVisitorBase< LieGroupIntegrateVisitor<ConfigIn_t,Tangent_t,ConfigOut_t> >
{
typedef boost::fusion::vector<const Eigen::MatrixBase<ConfigIn_t> &,
const Eigen::MatrixBase<Tangent_t> &,
const Eigen::MatrixBase<ConfigOut_t> &> ArgsType;
typedef boost::fusion::vector<const ConfigIn_t &,
const Tangent_t &,
ConfigOut_t &> ArgsType;
LIE_GROUP_VISITOR(LieGroupIntegrateVisitor);
template<typename D>
static void algo(const LieGroupBase<D> & lg,
template<typename LieGroupDerived>
static void algo(const LieGroupBase<LieGroupDerived> & lg,
const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<ConfigOut_t>& qout)
{
ConfigOut_t & qout_ = const_cast< ConfigOut_t& >(qout.derived());
lg.integrate(Eigen::Ref<const typename D::ConfigVector_t>(q),
Eigen::Ref<const typename D::TangentVector_t>(v),
Eigen::Ref<typename D::ConfigVector_t>(qout_));
lg.integrate(Eigen::Ref<const typename LieGroupDerived::ConfigVector_t>(q),
Eigen::Ref<const typename LieGroupDerived::TangentVector_t>(v),
Eigen::Ref<typename LieGroupDerived::ConfigVector_t>(qout_));
}
};
template <class ConfigIn_t, class Tangent_t, class ConfigOut_t>
......@@ -144,13 +143,17 @@ namespace se3
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<ConfigOut_t>& qout)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(ConfigIn_t)
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Tangent_t)
EIGEN_STATIC_ASSERT_VECTOR_ONLY(ConfigOut_t)
typedef LieGroupIntegrateVisitor<ConfigIn_t,Tangent_t,ConfigOut_t> Operation;
assert(q.size() == nq(lg));
assert(v.size() == nv(lg));
assert(qout.size() == nq(lg));
ConfigOut_t & qout_ = const_cast< ConfigOut_t& >(qout.derived());
Operation::run(lg,typename Operation::ArgsType(q,v,qout_));
Operation::run(lg,typename Operation::ArgsType(q.derived(),v.derived(),qout_.derived()));
}
}
......
......@@ -460,10 +460,13 @@ struct TestLieGroupVariantVisitor
ConfigVector_t q0 = lg.random();
TangentVector_t v = TangentVector_t::Random(lg.nv());
ConfigVector_t qout(lg.nq()), qout_ref(lg.nq());
ConfigVector_t qout_ref(lg.nq());
lg.integrate(q0, v, qout_ref);
integrate(lg_variant, q0, v, qout);
typedef Eigen::VectorXd ConfigVectorGeneric;
typedef Eigen::VectorXd TangentVectorGeneric;
ConfigVectorGeneric qout(lg.nq());
integrate(lg_variant, ConfigVectorGeneric(q0), TangentVectorGeneric(v), qout);
BOOST_CHECK(qout.isApprox(qout_ref));
}
};
......
Markdown is supported
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