Skip to content
Snippets Groups Projects
Commit 99431c9d authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

[C++] Clean code in CartesianProductOperation

parent 4248be10
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/multibody/liegroup/cartesian-product.hpp
+ 47
− 52
View file @ 99431c9d
......@@ -77,8 +77,8 @@ namespace se3
{
ConfigVector_t n;
n.resize (nq ());
n.head (lg1_.nq ()) = lg1_.neutral ();
n.tail (lg2_.nq ()) = lg2_.neutral ();
Qo1(n) = lg1_.neutral ();
Qo2(n) = lg2_.neutral ();
return n;
}
......@@ -93,9 +93,8 @@ namespace se3
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<Tangent_t> & d) const
{
Tangent_t& out = const_cast< Eigen::MatrixBase<Tangent_t>& > (d).derived();
lg1_.difference(q0.head(lg1_.nq()), q1.head(lg1_.nq()), out.head(lg1_.nv()));
lg2_.difference(q0.tail(lg2_.nq()), q1.tail(lg2_.nq()), out.tail(lg2_.nv()));
lg1_.difference(Q1(q0), Q1(q1), Vo1(d));
lg2_.difference(Q2(q0), Q2(q1), Vo2(d));
}
template <class ConfigL_t, class ConfigR_t, class JacobianLOut_t, class JacobianROut_t>
......@@ -104,24 +103,14 @@ namespace se3
const Eigen::MatrixBase<JacobianLOut_t>& J0,
const Eigen::MatrixBase<JacobianROut_t>& J1) const
{
JacobianLOut_t& J0out = const_cast< JacobianLOut_t& >(J0.derived());
J0out.topRightCorner(lg1_.nv(),lg2_.nv()).setZero();
J0out.bottomLeftCorner(lg2_.nv(),lg1_.nv()).setZero();
JacobianROut_t& J1out = const_cast< JacobianROut_t& >(J1.derived());
J1out.topRightCorner(lg1_.nv(),lg2_.nv()).setZero();
J1out.bottomLeftCorner(lg2_.nv(),lg1_.nv()).setZero();
lg1_.Jdifference(
q0.head(lg1_.nq()),
q1.head(lg1_.nq()),
J0out.topLeftCorner(lg1_.nv(),lg1_.nv()),
J1out.topLeftCorner(lg1_.nv(),lg1_.nv()));
lg2_.Jdifference(
q0.tail(lg2_.nq()),
q1.tail(lg2_.nq()),
J0out.bottomRightCorner(lg2_.nv(),lg2_.nv()),
J1out.bottomRightCorner(lg2_.nv(),lg2_.nv()));
J12(J0).setZero();
J21(J0).setZero();
J12(J1).setZero();
J21(J1).setZero();
lg1_.Jdifference (Q1(q0), Q1(q1), J11(J0), J11(J1));
lg2_.Jdifference (Q2(q0), Q2(q1), J22(J0), J22(J1));
}
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
......@@ -129,9 +118,8 @@ namespace se3
const Eigen::MatrixBase<Velocity_t> & v,
const Eigen::MatrixBase<ConfigOut_t> & qout) const
{
ConfigOut_t& out = const_cast< Eigen::MatrixBase<ConfigOut_t>& > (qout).derived();
LieGroup1().integrate(q.head(lg1_.nq()), v.head(lg1_.nv()), out.head(lg1_.nq()));
LieGroup2().integrate(q.tail(lg2_.nq()), v.tail(lg2_.nv()), out.tail(lg2_.nq()));
lg1_.integrate(Q1(q), V1(v), Qo1(qout));
lg2_.integrate(Q2(q), V2(v), Qo2(qout));
}
template <class Config_t, class Tangent_t, class JacobianOut_t>
......@@ -139,11 +127,10 @@ namespace se3
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J) const
{
JacobianOut_t& Jout = const_cast< JacobianOut_t& >(J.derived());
Jout. topRightCorner(lg1_.nv(),lg2_.nv()).setZero();
Jout.bottomLeftCorner(lg2_.nv(),lg1_.nv()).setZero();
lg1_.dIntegrate_dq(v.head(lg1_.nv()), Jout. topLeftCorner(lg1_.nv(),lg1_.nv()));
lg2_.dIntegrate_dq(v.tail(lg2_.nv()), Jout.bottomRightCorner(lg2_.nv(),lg2_.nv()));
J12(J).setZero();
J21(J).setZero();
lg1_.dIntegrate_dq(Q1(q), V1(v), J11(J));
lg2_.dIntegrate_dq(Q2(q), V2(v), J22(J));
}
template <class Config_t, class Tangent_t, class JacobianOut_t>
......@@ -151,34 +138,32 @@ namespace se3
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J) const
{
JacobianOut_t& Jout = const_cast< JacobianOut_t& >(J.derived());
Jout. topRightCorner(lg1_.nv(),lg2_.nv()).setZero();
Jout.bottomLeftCorner(lg2_.nv(),lg1_.nv()).setZero();
lg1_.dIntegrate_dv(v.head(lg1_.nv()), Jout. topLeftCorner(lg1_.nv(),lg1_.nv()));
lg2_.dIntegrate_dv(v.tail(lg2_.nv()), Jout.bottomRightCorner(lg2_.nv(),lg2_.nv()));
J12(J).setZero();
J21(J).setZero();
lg1_.dIntegrate_dv(Q1(q), V1(v), J11(J));
lg2_.dIntegrate_dv(Q2(q), V2(v), J22(J));
}
template <class ConfigL_t, class ConfigR_t>
Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1) const
{
return lg1_.squaredDistance(q0.head(lg1_.nq()), q1.head(lg1_.nq()))
+ lg2_.squaredDistance(q0.tail(lg2_.nq()), q1.tail(lg2_.nq()));
return lg1_.squaredDistance(Q1(q0), Q1(q1))
+ lg2_.squaredDistance(Q2(q0), Q2(q1));
}
template <class Config_t>
void normalize_impl (const Eigen::MatrixBase<Config_t>& qout) const
{
lg1_.normalize(qout.derived().head(lg1_.nq()));
lg2_.normalize(qout.derived().tail(lg2_.nq()));
lg1_.normalize(Qo1(qout));
lg2_.normalize(Qo2(qout));
}
template <class Config_t>
void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
{
Config_t& out = const_cast< Eigen::MatrixBase<Config_t>& > (qout).derived();
lg1_.random(out.head(lg1_.nq()));
lg2_.random(out.tail(lg2_.nq()));
lg1_.random(Qo1(qout));
lg2_.random(Qo2(qout));
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
......@@ -187,13 +172,8 @@ namespace se3
const Eigen::MatrixBase<ConfigOut_t> & qout)
const
{
ConfigOut_t& out = const_cast< Eigen::MatrixBase<ConfigOut_t>& > (qout).derived();
lg1_.randomConfiguration(lower.head(lg1_.nq()),
upper.head(lg1_.nq()),
out.head(lg1_.nq()));
lg2_.randomConfiguration(lower.tail(lg2_.nq()),
upper.tail(lg2_.nq()),
out.tail(lg2_.nq()));
lg1_.randomConfiguration(Q1(lower), Q1(upper), Qo1(qout));
lg2_.randomConfiguration(Q2(lower), Q2(upper), Qo2(qout));
}
template <class ConfigL_t, class ConfigR_t>
......@@ -201,12 +181,27 @@ namespace se3
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec) const
{
return LieGroup1().isSameConfiguration(q0.head(lg1_.nq()), q1.head(lg1_.nq()), prec)
+ LieGroup2().isSameConfiguration(q0.tail(lg2_.nq()), q1.tail(lg2_.nq()), prec);
return lg1_.isSameConfiguration(Q1(q0), Q1(q1), prec)
&& lg2_.isSameConfiguration(Q2(q0), Q2(q1), prec);
}
private:
LieGroup1 lg1_;
LieGroup2 lg2_;
template <typename Config > typename Config ::template ConstFixedSegmentReturnType<LieGroup1::NQ>::Type Q1 (const Eigen::MatrixBase<Config >& q) const { return q.derived().template head<LieGroup1::NQ>(lg1_.nq()); }
template <typename Config > typename Config ::template ConstFixedSegmentReturnType<LieGroup2::NQ>::Type Q2 (const Eigen::MatrixBase<Config >& q) const { return q.derived().template tail<LieGroup2::NQ>(lg2_.nq()); }
template <typename Tangent> typename Tangent::template ConstFixedSegmentReturnType<LieGroup1::NV>::Type V1 (const Eigen::MatrixBase<Tangent>& v) const { return v.derived().template head<LieGroup1::NV>(lg1_.nv()); }
template <typename Tangent> typename Tangent::template ConstFixedSegmentReturnType<LieGroup2::NV>::Type V2 (const Eigen::MatrixBase<Tangent>& v) const { return v.derived().template tail<LieGroup2::NV>(lg2_.nv()); }
template <typename Config > typename Config ::template FixedSegmentReturnType<LieGroup1::NQ>::Type Qo1 (const Eigen::MatrixBase<Config >& q) const { return const_cast<Config &>(q.derived()).template head<LieGroup1::NQ>(lg1_.nq()); }
template <typename Config > typename Config ::template FixedSegmentReturnType<LieGroup2::NQ>::Type Qo2 (const Eigen::MatrixBase<Config >& q) const { return const_cast<Config &>(q.derived()).template tail<LieGroup2::NQ>(lg2_.nq()); }
template <typename Tangent> typename Tangent::template FixedSegmentReturnType<LieGroup1::NV>::Type Vo1 (const Eigen::MatrixBase<Tangent>& v) const { return const_cast<Tangent&>(v.derived()).template head<LieGroup1::NV>(lg1_.nv()); }
template <typename Tangent> typename Tangent::template FixedSegmentReturnType<LieGroup2::NV>::Type Vo2 (const Eigen::MatrixBase<Tangent>& v) const { return const_cast<Tangent&>(v.derived()).template tail<LieGroup2::NV>(lg2_.nv()); }
template <typename Jac> Eigen::Block<Jac, LieGroup1::NV, LieGroup1::NV> J11 (const Eigen::MatrixBase<Jac>& J) const { return const_cast<Jac&>(J.derived()).template topLeftCorner<LieGroup1::NV, LieGroup1::NV>(lg1_.nv(),lg1_.nv()); }
template <typename Jac> Eigen::Block<Jac, LieGroup1::NV, LieGroup2::NV> J12 (const Eigen::MatrixBase<Jac>& J) const { return const_cast<Jac&>(J.derived()).template topRightCorner<LieGroup1::NV, LieGroup2::NV>(lg1_.nv(),lg2_.nv()); }
template <typename Jac> Eigen::Block<Jac, LieGroup2::NV, LieGroup1::NV> J21 (const Eigen::MatrixBase<Jac>& J) const { return const_cast<Jac&>(J.derived()).template bottomLeftCorner<LieGroup2::NV, LieGroup1::NV>(lg2_.nv(),lg1_.nv()); }
template <typename Jac> Eigen::Block<Jac, LieGroup2::NV, LieGroup2::NV> J22 (const Eigen::MatrixBase<Jac>& J) const { return const_cast<Jac&>(J.derived()).template bottomRightCorner<LieGroup2::NV, LieGroup2::NV>(lg2_.nv(),lg2_.nv()); }
}; // struct CartesianProductOperation
} // namespace se3
......
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