From ccdd1471ed148aa0df051c59b4a9f043ce7aa459 Mon Sep 17 00:00:00 2001
From: ss <ss>
Date: Thu, 6 Aug 2020 14:26:22 -0400
Subject: [PATCH] Fix pose2 align bug.

---
 gtsam/geometry/Pose2.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index 9c41a76c8..71df0f753 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -322,10 +322,10 @@ boost::optional<Pose2> align(const vector<Point2Pair>& pairs) {
   // calculate cos and sin
   double c=0,s=0;
   for(const Point2Pair& pair: pairs) {
-    Point2 dq = pair.first  - cp;
-    Point2 dp = pair.second - cq;
-    c +=  dp.x() * dq.x() + dp.y() * dq.y();
-    s +=  dp.y() * dq.x() - dp.x() * dq.y(); // this works but is negative from formula above !! :-(
+    Point2 dp = pair.first - cp;
+    Point2 dq = pair.second - cq;
+    c += dp.x() * dq.x() + dp.y() * dq.y();
+    s += -dp.y() * dq.x() + dp.x() * dq.y();
   }
 
   // calculate angle and translation