diff --git a/cpp/Pose2Factor.h b/cpp/Pose2Factor.h
index 86783578b..2241b7943 100644
--- a/cpp/Pose2Factor.h
+++ b/cpp/Pose2Factor.h
@@ -20,14 +20,18 @@ private:
 	std::string key1_, key2_; /** The keys of the two poses, order matters */
 	Pose2 measured_;
 	Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */
+	std::list<std::string> keys_;
 
 public:
 
 	typedef boost::shared_ptr<Pose2Factor> shared_ptr; // shorthand for a smart pointer to a factor
 
 	Pose2Factor(const std::string& key1, const std::string& key2,
-			const Pose2& measured, const Matrix& measurement_covariance): key1_(key1),key2_(key2),measured_(measured) {
+			const Pose2& measured, const Matrix& measurement_covariance) :
+			  key1_(key1),key2_(key2),measured_(measured) {
 		square_root_inverse_covariance_ = inverse_square_root(measurement_covariance);
+		keys_.push_back(key1);
+		keys_.push_back(key2);
 	}
 
 	/** implement functions needed for Testable */
@@ -45,16 +49,16 @@ public:
 	Vector error_vector(const Pose2Config& config) const {
 		//z-h
 		Pose2 p1 = config.get(key1_), p2 = config.get(key2_);
-		return (measured_ - between(p1,p2)).vector();
+		return -between(p1,p2).log(measured_);
 	}
 
-	std::list<std::string> keys() const { std::list<std::string> l; return l; }
-	std::size_t size() const { return 2;}
+	std::list<std::string> keys() const { return keys_; }
+	std::size_t size() const { return keys_.size(); }
 
 	/** linearize */
 	boost::shared_ptr<GaussianFactor> linearize(const Pose2Config& config) const {
 		Pose2 p1 = config.get(key1_), p2 = config.get(key2_);
-		Vector b = (measured_ - between(p1,p2)).vector();
+		Vector b = -between(p1,p2).log(measured_);
 		Matrix H1 = Dbetween1(p1,p2);
 		Matrix H2 = Dbetween2(p1,p2);
 		boost::shared_ptr<GaussianFactor> linearized(new GaussianFactor(
diff --git a/cpp/testPose2Factor.cpp b/cpp/testPose2Factor.cpp
index 6bb6deca6..a2231ce82 100644
--- a/cpp/testPose2Factor.cpp
+++ b/cpp/testPose2Factor.cpp
@@ -28,7 +28,7 @@ TEST( Pose2Factor, constructor )
 
 	// Choose a linearization point
 	Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
-	Pose2 p2(-1,4.1,M_PI);  // robot at (-1,4) looking at negative (ground truth is at 4.1,2)
+	Pose2 p2(-1,4.1,M_PI);  // robot at (-1,4.1) looking at negative (ground truth is at 4.1,2)
 	Pose2Config config;
 	config.insert("p1",p1);
 	config.insert("p2",p2);
@@ -59,11 +59,12 @@ TEST( Pose2Factor, constructor )
 	GaussianFactor expected(
 			"p1", square_root_inverse_covariance*expectedH1,
 			"p2", square_root_inverse_covariance*expectedH2,
-			Vector_(3,-0.1,-0.1,0.0), 1.0);
+			Vector_(3,0.1,0.1,0.0), 1.0);
 
 	CHECK(assert_equal(expected,*actual));
 }
 
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;