Added constructor from just mean and covariance of a Gaussian. If there is a better way let me know.

gtsam/linear/HessianFactor.cpp

@@ -72,8 +72,8 @@ HessianFactor::HessianFactor() : info_(matrix_) {
 }
 
 /* ************************************************************************* */
-HessianFactor::HessianFactor(Index j1, const Matrix& G, const Vector& g, double f) :
-      		GaussianFactor(j1), info_(matrix_) {
+HessianFactor::HessianFactor(Index j, const Matrix& G, const Vector& g, double f) :
+      		GaussianFactor(j), info_(matrix_) {
 	if(G.rows() != G.cols() || G.rows() != g.size())
 		throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
 	size_t dims[] = { G.rows(), 1 };
@@ -86,6 +86,26 @@ HessianFactor::HessianFactor(Index j1, const Matrix& G, const Vector& g, double
 	assertInvariants();
 }
 
+/* ************************************************************************* */
+// error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu)
+// where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g
+HessianFactor::HessianFactor(Index j, const Vector& mu, const Matrix& Sigma) :
+		GaussianFactor(j), info_(matrix_) {
+	if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument(
+			"Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
+	Matrix G = inverse(Sigma);
+	Vector g = G * mu;
+	double f = dot(mu, g);
+	size_t dims[] = { G.rows(), 1 };
+	InfoMatrix fullMatrix(G.rows() + 1, G.rows() + 1);
+	BlockInfo infoMatrix(fullMatrix, dims, dims + 2);
+	infoMatrix(0, 0) = G;
+	infoMatrix.column(0, 1, 0) = g;
+	infoMatrix(1, 1)(0, 0) = f;
+	infoMatrix.swap(info_);
+	assertInvariants();
+}
+
 /* ************************************************************************* */
 HessianFactor::HessianFactor(Index j1, Index j2,
 		const Matrix& G11, const Matrix& G12, const Vector& g1,

gtsam/linear/HessianFactor.h

@@ -25,6 +25,7 @@
 // Forward declarations for friend unit tests
 class ConversionConstructorHessianFactorTest;
 class Constructor1HessianFactorTest;
+class Constructor1bHessianFactorTest;
 class combineHessianFactorTest;
 
 
@@ -78,6 +79,11 @@ namespace gtsam {
      */
     HessianFactor(Index j, const Matrix& G, const Vector& g, double f);
 
+    /** Construct a unary factor, given a mean and covariance matrix.
+     * error is 0.5*(x-mu)'*inv(Sigma)*(x-mu)
+    */
+    HessianFactor(Index j, const Vector& mu, const Matrix& Sigma);
+
     /** Construct a binary factor.  Gxx are the upper-triangle blocks of the
      * quadratic term (the Hessian matrix), gx the pieces of the linear vector
      * term, and f the constant term.  The quadratic error is:
@@ -133,6 +139,7 @@ namespace gtsam {
     // Friend unit test classes
     friend class ::ConversionConstructorHessianFactorTest;
     friend class ::Constructor1HessianFactorTest;
+    friend class ::Constructor1bHessianFactorTest;
     friend class ::combineHessianFactorTest;
 
     // Friend JacobianFactor for conversion

gtsam/linear/tests/testHessianFactor.cpp

@@ -134,6 +134,26 @@ TEST(HessianFactor, Constructor1)
   DOUBLES_EQUAL(expected, actual, 1e-10);
 }
 
+/* ************************************************************************* */
+TEST(HessianFactor, Constructor1b)
+{
+	Vector mu = Vector_(2,1.0,2.0);
+	Matrix Sigma = eye(2,2);
+
+  HessianFactor factor(0, mu, Sigma);
+
+  Matrix G = eye(2,2);
+  Vector g = G*mu;
+  double f = dot(g,mu);
+
+  // Check
+  Matrix info_matrix = factor.info_.range(0, 1, 0, 1);
+  EXPECT(assert_equal(Matrix(G), info_matrix));
+  EXPECT_DOUBLES_EQUAL(f, factor.constant_term(), 1e-10);
+  EXPECT(assert_equal(g, Vector(factor.linear_term()), 1e-10));
+  EXPECT_LONGS_EQUAL(1, factor.size());
+}
+
 /* ************************************************************************* */
 TEST(HessianFactor, Constructor2)
 {