liegroups: update integrate coeffWise with the new explog quaternion operators

src/multibody/liegroup/special-euclidean.hpp

@@ -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>

src/multibody/liegroup/special-orthogonal.hpp

@@ -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>

unittest/explog.cpp

@@ -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>();

unittest/liegroups.cpp

@@ -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());
   
 }