From a8afe0da7c81b533167ca5d684f510914f4abcd8 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 16 Jan 2010 07:00:07 +0000
Subject: [PATCH] Forgot pre-multiply of error vector

---
 cpp/Pose2Prior.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/cpp/Pose2Prior.h b/cpp/Pose2Prior.h
index 927c6f3bd..b777fcd83 100644
--- a/cpp/Pose2Prior.h
+++ b/cpp/Pose2Prior.h
@@ -46,7 +46,7 @@ public:
 	/** implement functions needed to derive from Factor */
 	Vector error_vector(const Pose2Config& config) const {
 		Pose2 p = config[key_];
-		return -logmap(p,measured_);
+		return square_root_inverse_covariance_ * logmap(measured_,p);
 	}
 
 	//std::list<Key> keys() const { return keys_; }
@@ -55,7 +55,7 @@ public:
 	/** linearize */
 	boost::shared_ptr<GaussianFactor> linearize(const Pose2Config& config) const {
 		Pose2 p = config[key_];
-		Vector b = logmap(p,measured_);
+		Vector b = - error_vector(config);
 		Matrix H(3,3);
 		H(0,0)=1; H(0,1)=0; H(0,2)=0;
 		H(1,0)=0; H(1,1)=1; H(1,2)=0;