From 376c910e2b6650af2c70f943fa34543271f24e0a Mon Sep 17 00:00:00 2001
From: kartik arcot <kartik.arcot@verdantrobotics.com>
Date: Tue, 10 Jan 2023 16:04:44 -0800
Subject: [PATCH] timePose2 updates

---
 timing/timePose2.cpp | 42 ++++--------------------------------------
 1 file changed, 4 insertions(+), 38 deletions(-)

diff --git a/timing/timePose2.cpp b/timing/timePose2.cpp
index bf04b9e93..4a9fd7aa9 100644
--- a/timing/timePose2.cpp
+++ b/timing/timePose2.cpp
@@ -37,7 +37,7 @@ Pose2 Pose2betweenDefault(const Pose2& r1, const Pose2& r2) {
 
 /* ************************************************************************* */
 Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2,
-  boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) {
+  OptionalJacobian<3,3> H1 = {}, OptionalJacobian<3,3> H2 = {}) {
   // get cosines and sines from rotation matrices
   const Rot2& R1 = r1.r(), R2 = r2.r();
   double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
@@ -68,43 +68,9 @@ Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2,
   return Pose2(R,t);
 }
 
-/* ************************************************************************* */
-Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2,
-  boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2)
-{
-  // get cosines and sines from rotation matrices
-  const Rot2& R1 = r1.r(), R2 = r2.r();
-  double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
-
-  // Assert that R1 and R2 are normalized
-  assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
-
-  // Calculate delta rotation = between(R1,R2)
-  double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
-  Rot2 R(Rot2::atan2(s,c)); // normalizes
-
-  // Calculate delta translation = unrotate(R1, dt);
-  Point2 dt = r2.t() - r1.t();
-  double x = dt.x(), y = dt.y();
-  Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
-
-  // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
-  if (H1) {
-    double dt1 = -s2 * x + c2 * y;
-    double dt2 = -c2 * x - s2 * y;
-    *H1 = Matrix3(); (*H1) <<
-      -c,  -s,  dt1,
-      s,  -c,  dt2,
-      0.0, 0.0,-1.0;
-  }
-  if (H2) *H2 = I_3x3;
-
-  return Pose2(R,t);
-}
-
 /* ************************************************************************* */
 Vector Pose2BetweenFactorEvaluateErrorDefault(const Pose2& measured, const Pose2& p1, const Pose2& p2,
-  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2)
+  OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2)
 {
   Pose2 hx = p1.between(p2, H1, H2); // h(x)
   // manifold equivalent of h(x)-z -> log(z,h(x))
@@ -113,7 +79,7 @@ Vector Pose2BetweenFactorEvaluateErrorDefault(const Pose2& measured, const Pose2
 
 /* ************************************************************************* */
 Vector Pose2BetweenFactorEvaluateErrorOptimizedBetween(const Pose2& measured, const Pose2& p1, const Pose2& p2,
-  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2)
+  OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2)
 {
   Pose2 hx = Pose2betweenOptimized(p1, p2, H1, H2); // h(x)
   // manifold equivalent of h(x)-z -> log(z,h(x))
@@ -122,7 +88,7 @@ Vector Pose2BetweenFactorEvaluateErrorOptimizedBetween(const Pose2& measured, co
 
 /* ************************************************************************* */
 Vector Pose2BetweenFactorEvaluateErrorOptimizedBetweenFixed(const Pose2& measured, const Pose2& p1, const Pose2& p2,
-  boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2)
+  OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2)
 {
   // TODO: Implement
   Pose2 hx = Pose2betweenOptimized(p1, p2, H1, H2); // h(x)