From e83b6c3b8400d7407a0d8b0ca3d2087c4929b5d4 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Tue, 9 Feb 2010 22:44:02 +0000
Subject: [PATCH] Normalizing rotation matrix to fix numerical instability when
 composing many poses

---
 cpp/Rot2.h | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/cpp/Rot2.h b/cpp/Rot2.h
index 0ddfadf21..b6a885a65 100644
--- a/cpp/Rot2.h
+++ b/cpp/Rot2.h
@@ -27,6 +27,14 @@ namespace gtsam {
     /** constructor from cos/sin */
     Rot2(double c, double s) :
       c_(c), s_(s) {
+    	// rtodo: Could do this scale correction only when creating from compose
+    	// Don't let scale drift
+    	double scale = c*c + s*s;
+    	if(scale != 1.0) {
+    		scale = pow(scale, -0.5);
+    		c_ *= scale;
+    		s_ *= scale;
+    	}
     }
 
     /** default constructor, zero rotation */