From 7c8716f746a235ec61192e1f1057612b09507247 Mon Sep 17 00:00:00 2001
From: Lachlan Toohey <l.toohey@acfr.usyd.edu.au>
Date: Tue, 7 Oct 2014 11:29:09 +1100
Subject: [PATCH] Fixed PoseRotationPrior error calculation.

Use measured_.localCoordinates not difference between Logmap
values to handle wraparound properly.
---
 gtsam/slam/PoseRotationPrior.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h
index 7834f235f..7d31e2e2c 100644
--- a/gtsam/slam/PoseRotationPrior.h
+++ b/gtsam/slam/PoseRotationPrior.h
@@ -77,7 +77,7 @@ public:
       (*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim);
     }
 
-    return Rotation::Logmap(newR) - Rotation::Logmap(measured_);
+    return measured_.localCoordinates(newR);
   }
 
 private: