Verified Commit caf8530c authored by Justin Carpentier's avatar Justin Carpentier
Browse files

liegroups: update integrate coeffWise with the new explog quaternion operators

parent f6cb57fe
......@@ -495,12 +495,29 @@ namespace se3
{
assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
typedef typename EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlain;
typedef typename JacobianPlain::Scalar Scalar;
Jacobian_t & Jout = EIGEN_CONST_CAST(Jacobian_t,J);
Jout.setZero();
ConstQuaternionMap_t quat(q.derived().template tail<4>().data());
Jout.template topLeftCorner<3,3>() = quat.toRotationMatrix();
Jexp3(quat,Jout.template bottomRightCorner<4,3>());
ConstQuaternionMap_t quat_map(q.derived().template tail<4>().data());
Jout.template topLeftCorner<3,3>() = quat_map.toRotationMatrix();
// Jexp3(quat,Jout.template bottomRightCorner<4,3>());
typedef Eigen::Matrix<Scalar,4,3,JacobianPlain::Options> Jacobian43;
typedef SE3Tpl<Scalar,JacobianPlain::Options> SE3;
Jacobian43 Jexp3QuatCoeffWise;
Scalar theta;
typename SE3::Vector3 v = quaternion::log3(quat_map,theta);
quaternion::Jexp3CoeffWise(v,Jexp3QuatCoeffWise);
typename SE3::Matrix3 Jlog;
Jlog3(theta,v,Jlog);
if(quat_map.w() >= 0.) // comes from the log3 for quaternions which may change the sign.
EIGEN_CONST_CAST(Jacobian_t,J).template bottomRightCorner<4,3>().noalias() = Jexp3QuatCoeffWise * Jlog;
else
EIGEN_CONST_CAST(Jacobian_t,J).template bottomRightCorner<4,3>().noalias() = -Jexp3QuatCoeffWise * Jlog;
}
template <class Config_t, class Tangent_t, class JacobianOut_t>
......
......@@ -337,7 +337,7 @@ namespace se3
ConstQuaternionMap_t quat(q.derived().data());
QuaternionMap_t quat_map(EIGEN_CONST_CAST(ConfigOut_t,qout).data());
Quaternion_t pOmega; (exp3(v,pOmega));
Quaternion_t pOmega; quaternion::exp3(v,pOmega);
quat_map = quat * pOmega;
quaternion::firstOrderNormalize(quat_map);
}
......@@ -347,9 +347,28 @@ namespace se3
const Eigen::MatrixBase<Jacobian_t> & J)
{
assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
typedef typename EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlain;
typedef typename JacobianPlain::Scalar Scalar;
typedef SE3Tpl<Scalar,JacobianPlain::Options> SE3;
typedef typename SE3::Vector3 Vector3;
typedef typename SE3::Matrix3 Matrix3;
ConstQuaternionMap_t quat_map(q.derived().data());
Jexp3(quat_map,EIGEN_CONST_CAST(Jacobian_t,J).template topLeftCorner<NQ,NV>());
Eigen::Matrix<Scalar,NQ,NV,JacobianPlain::Options> Jexp3QuatCoeffWise;
Scalar theta;
Vector3 v = quaternion::log3(quat_map,theta);
quaternion::Jexp3CoeffWise(v,Jexp3QuatCoeffWise);
Matrix3 Jlog;
Jlog3(theta,v,Jlog);
if(quat_map.w() >= 0.) // comes from the log3 for quaternions which may change the sign.
EIGEN_CONST_CAST(Jacobian_t,J).noalias() = Jexp3QuatCoeffWise * Jlog;
else
EIGEN_CONST_CAST(Jacobian_t,J).noalias() = -Jexp3QuatCoeffWise * Jlog;
// Jexp3(quat_map,EIGEN_CONST_CAST(Jacobian_t,J).template topLeftCorner<NQ,NV>());
}
template <class Config_t, class Tangent_t, class JacobianOut_t>
......
......@@ -83,8 +83,11 @@ BOOST_AUTO_TEST_CASE(log)
for(int k = 0; k < 1e3; ++k)
{
quat = M.rotation();
BOOST_CHECK(quat.toRotationMatrix().isApprox(M.rotation()));
SE3::Vector3 w; w.setRandom();
quaternion::exp3(w,quat);
SE3::Matrix3 rot = exp3(w);
BOOST_CHECK(quat.toRotationMatrix().isApprox(rot));
double theta;
omega = quaternion::log3(quat,theta);
const double PI_value = PI<double>();
......
......@@ -284,13 +284,13 @@ struct LieGroup_JintegrateCoeffWise
ConfigVector_t q = lg.random();
TangentVector_t dv(TangentVector_t::Zero(lg.nv()));
// std::cout << "name: " << lg.name() << std::endl;
typedef Eigen::Matrix<Scalar,T::NQ,T::NV> JacobianCoeffs;
JacobianCoeffs Jintegrate(JacobianCoeffs::Zero(lg.nq(),lg.nv()));
lg.integrateCoeffWiseJacobian(q,Jintegrate);
JacobianCoeffs Jintegrate_fd(JacobianCoeffs::Zero(lg.nq(),lg.nv()));;
const Scalar eps = 1e-6;
const Scalar eps = 1e-8;
for (int i = 0; i < lg.nv(); ++i)
{
dv[i] = eps;
......@@ -302,6 +302,8 @@ struct LieGroup_JintegrateCoeffWise
}
BOOST_CHECK(Jintegrate.isApprox(Jintegrate_fd,sqrt(eps)));
// std::cout << "Jintegrate\n" << Jintegrate << std::endl;
// std::cout << "Jintegrate_fd\n" << Jintegrate_fd << std::endl;
}
};
......@@ -398,7 +400,7 @@ BOOST_AUTO_TEST_CASE(JintegrateCoeffWise)
SpecialOrthogonalOperationTpl<3,Scalar,Options>
>
> Types;
for (int i = 0; i < 1; ++i)
for (int i = 0; i < 20; ++i)
boost::mpl::for_each<Types>(LieGroup_JintegrateCoeffWise());
}
......
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