ediv_ is same as ediv except returns 0 for 0/0 division, needed for dealing with constraints

cpp/Vector.cpp

@@ -174,6 +174,18 @@ namespace gtsam {
 		return c;
 		}
 
+  /* ************************************************************************* */
+  Vector ediv_(const Vector &a, const Vector &b) {
+  	size_t n = a.size();
+		assert (b.size()==n);
+		Vector c(n);
+		for( size_t i = 0; i < n; i++ ) {
+			double ai = a(i), bi = b(i);
+			c(i) = (bi==0.0 && ai==0.0) ? 0.0 : a(i)/b(i);
+		}
+		return c;
+		}
+
   /* ************************************************************************* */
   double sum(const Vector &a) {
   	double result = 0.0;

cpp/Vector.h

@@ -141,6 +141,14 @@ Vector emul(const Vector &a, const Vector &b);
  */
 Vector ediv(const Vector &a, const Vector &b);
 
+/**
+ * elementwise division, but 0/0 = 0, not inf
+ * @param a first vector
+ * @param b second vector
+ * @return vector [a(i)/b(i)]
+ */
+Vector ediv_(const Vector &a, const Vector &b);
+
 /**
  * sum vector elements
  * @param a vector