Add jacobians to retract functions of Unit3 and OrientedPlane3

gtsam/geometry/OrientedPlane3.cpp

@@ -83,8 +83,13 @@ Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobia
 }
 
 /* ************************************************************************* */
-OrientedPlane3 OrientedPlane3::retract(const Vector3& v) const {
-  return OrientedPlane3(n_.retract(Vector2(v(0), v(1))), d_ + v(2));
+OrientedPlane3 OrientedPlane3::retract(const Vector3& v, OptionalJacobian<4,3> H) const {
+  Matrix32 H_n;
+  Unit3 n_retract (n_.retract(Vector2(v(0), v(1)), H? &H_n : nullptr));
+  if (H) {
+    *H << H_n, Vector3::Zero(), 0, 0, 1;
+  }
+  return OrientedPlane3(n_retract, d_ + v(2));
 }
 
 /* ************************************************************************* */

gtsam/geometry/OrientedPlane3.h

@@ -134,7 +134,7 @@ public:
   }
 
   /// The retract function
-  OrientedPlane3 retract(const Vector3& v) const;
+  OrientedPlane3 retract(const Vector3& v, OptionalJacobian<4,3> H = boost::none) const;
 
   /// The local coordinates function
   Vector3 localCoordinates(const OrientedPlane3& s) const;

gtsam/geometry/Unit3.cpp

@@ -240,18 +240,34 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const {
 }
 
 /* ************************************************************************* */
-Unit3 Unit3::retract(const Vector2& v) const {
+Unit3 Unit3::retract(const Vector2& v, OptionalJacobian<3,2> H) const {
   // Compute the 3D xi_hat vector
   const Vector3 xi_hat = basis() * v;
   const double theta = xi_hat.norm();
 
   // Treat case of very small v differently
   if (theta < std::numeric_limits<double>::epsilon()) {
+      // Jacobian
+      // TODO what happens if theta = 0 ? sin(theta)/theta -> 1 when theta -> 0.
+      if (H) {
+        *H = -p_ * xi_hat.transpose() * basis() +
+            basis();
+      }
+
       return Unit3(Vector3(std::cos(theta) * p_ + xi_hat));
   }
 
   const Vector3 exp_p_xi_hat =
       std::cos(theta) * p_ + xi_hat * (sin(theta) / theta);
+
+  // Jacobian
+  if (H) {
+    *H = p_ * (-std::sin(theta)) * xi_hat.transpose() / theta * basis() +
+        std::sin(theta) / theta * basis() +
+        xi_hat * ((theta * std::cos(theta) - std::sin(theta)) / std::pow(theta, 2))
+        * xi_hat.transpose() / theta * basis();
+  }
+
   return Unit3(exp_p_xi_hat);
 }
 

gtsam/geometry/Unit3.h

@@ -188,7 +188,7 @@ public:
   };
 
   /// The retract function
-  Unit3 retract(const Vector2& v) const;
+  Unit3 retract(const Vector2& v, OptionalJacobian<3,2> H = boost::none) const;
 
   /// The local coordinates function
   Vector2 localCoordinates(const Unit3& s) const;