Fixed quaternion derivatives

gtsam/geometry/Quaternion.h

@@ -54,40 +54,34 @@ struct traits<QUATERNION_TYPE> {
   /// @{
   static Q Compose(const Q &g, const Q & h,
       ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
-    if (Hg)
+    if (Hg) *Hg = h.toRotationMatrix().transpose();
-    *Hg = h.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart ( h.toRotationMatrix().transpose() ? )
+    if (Hh) *Hh = I_3x3;
-    if (Hh)
-    *Hh = I_3x3;// TODO : check Jacobian consistent with chart ( I(3)? )
     return g * h;
   }
 
   static Q Between(const Q &g, const Q & h,
       ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
     Q d = g.inverse() * h;
-    if (Hg)
+    if (Hg) *Hg = -d.toRotationMatrix().transpose();
-    *Hg = -d.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart
+    if (Hh) *Hh = I_3x3;
-    if (Hh)
-    *Hh = I_3x3;// TODO : check Jacobian consistent with chart (my guess I(3) )
     return d;
   }
 
   static Q Inverse(const Q &g,
       ChartJacobian H = boost::none) {
-    if (H)
+    if (H) *H = -g.toRotationMatrix();
-    *H = -g.toRotationMatrix(); // TODO : check Jacobian consistent with chart
     return g.inverse();
   }
 
   /// Exponential map, simply be converting omega to axis/angle representation
   static Q Expmap(const Eigen::Ref<const TangentVector>& omega,
       ChartJacobian H = boost::none) {
-    if (omega.isZero())
+    if(H) *H = SO3::ExpmapDerivative(omega);
-    return Q::Identity();
+    if (omega.isZero()) return Q::Identity();
     else {
       _Scalar angle = omega.norm();
       return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle));
     }
-    if(H) *H = SO3::ExpmapDerivative(omega);
   }
 
   /// We use our own Logmap, as there is a slight bug in Eigen
@@ -105,7 +99,7 @@ struct traits<QUATERNION_TYPE> {
     if (qw > NearlyOne) {
       // Taylor expansion of (angle / s) at 1
       //return (2 + 2 * (1-qw) / 3) * q.vec();
-      return (8. / 3. - 2. / 3. * qw) * q.vec();
+      omega = (8. / 3. - 2. / 3. * qw) * q.vec();
     } else if (qw < NearlyNegativeOne) {
       // Taylor expansion of (angle / s) at -1
       //return (-2 - 2 * (1 + qw) / 3) * q.vec();
@@ -128,15 +122,24 @@ struct traits<QUATERNION_TYPE> {
   /// @}
   /// @name Manifold traits
   /// @{
-  static TangentVector Local(const Q& origin, const Q& other,
+
-      ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
+  static TangentVector Local(const Q& g, const Q& h,
-    return Logmap(Between(origin, other, Horigin, Hother));
+      ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
-    // TODO: incorporate Jacobian of Logmap
+    Q b = Between(g, h, H1, H2);
+    Matrix3 D_v_b;
+    TangentVector v = Logmap(b, (H1 || H2) ? &D_v_b : 0);
+    if (H1) *H1 = D_v_b * (*H1);
+    if (H2) *H2 = D_v_b * (*H2);
+    return v;
   }
-  static Q Retract(const Q& origin, const TangentVector& v,
+
-      ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) {
+  static Q Retract(const Q& g, const TangentVector& v,
-    return Compose(origin, Expmap(v), Horigin, Hv);
+      ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
-    // TODO : incorporate Jacobian of Expmap
+    Matrix3 D_h_v;
+    Q b = Expmap(v,H2 ? &D_h_v : 0);
+    Q h = Compose(g, b, H1, H2);
+    if (H2) *H2 = (*H2) * D_h_v;
+    return h;
   }
 
   /// @}

gtsam/geometry/tests/testQuaternion.cpp

@@ -73,8 +73,8 @@ TEST(Quaternion , Compose) {
 }
 
 //******************************************************************************
-Q id;
 Vector3 z_axis(0, 0, 1);
+Q id(Eigen::AngleAxisd(0, z_axis));
 Q R1(Eigen::AngleAxisd(1, z_axis));
 Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));