From 3fbc857a9e4a2f43056df3d7daa5cff7fabc7e88 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 31 Dec 2009 10:28:43 +0000
Subject: [PATCH] fixed behavior of error for sigma==0

---
 cpp/NonlinearFactor.h | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/cpp/NonlinearFactor.h b/cpp/NonlinearFactor.h
index 03d35765e..21e188fdc 100644
--- a/cpp/NonlinearFactor.h
+++ b/cpp/NonlinearFactor.h
@@ -13,6 +13,7 @@
 #pragma once
 
 #include <list>
+#include <limits>
 
 #include <boost/shared_ptr.hpp>
 #include <boost/serialization/list.hpp>
@@ -91,6 +92,10 @@ namespace gtsam {
 
     /** calculate the error of the factor */
     double error(const Config& c) const {
+    	if (sigma_==0.0) {
+        Vector e = error_vector(c);
+    		return (inner_prod(e,e)>0) ? std::numeric_limits<double>::infinity() : 0.0;
+    	}
       Vector e = error_vector(c) / sigma_;
       return 0.5 * inner_prod(e,e);
     };