Fixed two unit tests in MATLAB, needed some small changes in C++ as well

cpp/GaussianFactor.h

@@ -19,11 +19,11 @@
 #include "Matrix.h"
 #include "VectorConfig.h"
 #include "SharedDiagonal.h"
+#include "GaussianConditional.h" // Needed for MATLAB
 #include "SymbolMap.h"
 
 namespace gtsam {
 
-	class GaussianConditional;
 	class Ordering;
 
 /**

cpp/NonlinearOptimizer.h

@@ -28,15 +28,14 @@ namespace gtsam {
 	 *
 	 * The class is parameterized by the Graph type $G$, Config class type $T$,
 	 * linear system class $L$ and the non linear solver type $S$.
-	 * the config type is in order to be able to optimize over non-vector configurations as well.
+	 * the config type is in order to be able to optimize over non-vector configurations.
 	 * To use in code, include <gtsam/NonlinearOptimizer-inl.h> in your cpp file
-	 * (the trick in http://www.ddj.com/cpp/184403420 did not work).
 	 *
 	 * For example, in a 2D case, $G$ can be Pose2Graph, $T$ can be Pose2Config,
 	 * $L$ can be GaussianFactorGraph and $S$ can be Factorization<Pose2Graph, Pose2Config>.
-	 * The solver class has two main functions: linear and optimize. The first one linear the
-	 * nonlinear cost function around the current estimate, and the second one optimize the
-	 * linearized system using various methods.
+	 * The solver class has two main functions: linearize and optimize. The first one
+	 * linearizes the nonlinear cost function around the current estimate, and the second
+	 * one optimizes the linearized system using various methods.
 	 */
 	template<class G, class T, class L = GaussianFactorGraph, class S = Factorization<G, T>, class Writer = NullOptimizerWriter>
 	class NonlinearOptimizer {

cpp/gtsam.h

@@ -73,6 +73,7 @@ class GaussianFactor {
   void print(string s) const;
   bool equals(const GaussianFactor& lf, double tol) const;
   pair<Matrix,Vector> matrix(const Ordering& ordering) const;
+  pair<GaussianConditional*,GaussianFactor*> eliminate(string key) const;
 };
 
 class GaussianFactorSet {

cpp/smallExample.cpp

@@ -124,30 +124,19 @@ namespace example {
 		// Create empty graph
 		GaussianFactorGraph fg;
 
+		SharedDiagonal unit2 = noiseModel::Unit::Create(2);
+
 		// linearized prior on x1: c["x1"]+x1=0 i.e. x1=-c["x1"]
-		Vector b1 = -Vector_(2,0.1, 0.1);
-		//fg.add("x1", I, b1, sigma0_1);
-		fg.add("x1", 10*eye(2), -1.0*ones(2), noiseModel::Unit::Create(2));
+		fg.add("x1", 10*eye(2), -1.0*ones(2), unit2);
 
 		// odometry between x1 and x2: x2-x1=[0.2;-0.1]
-		Vector b2 = Vector_(2, 0.2, -0.1);
-		//fg.add("x1", -I, "x2", I, b2, sigma0_1);
-		fg.add("x1", -10*eye(2),"x2", 10*eye(2), Vector_(2, 2.0, -1.0),
-				noiseModel::Unit::Create(2));
+		 fg.add("x1", -10*eye(2),"x2", 10*eye(2), Vector_(2, 2.0, -1.0), unit2);
 
 		// measurement between x1 and l1: l1-x1=[0.0;0.2]
-		Vector b3 = Vector_(2, 0.0, 0.2);
-		//fg.add("x1", -I, "l1", I, b3, sigma0_2);
-		fg.add("x1", -5*eye(2),
-				"l1", 5*eye(2), Vector_(2, 0.0, 1.0),
-				noiseModel::Unit::Create(2));
+		fg.add("x1", -5*eye(2), "l1", 5*eye(2), Vector_(2, 0.0, 1.0), unit2);
 
 		// measurement between x2 and l1: l1-x2=[-0.2;0.3]
-		Vector b4 = Vector_(2, -0.2, 0.3);
-		//fg.add("x2", -I, "l1", I, b4, sigma0_2);
-		fg.add("x2", -5*eye(2),
-				"l1", 5*eye(2), Vector_(2, -1.0, 1.5),
-				noiseModel::Unit::Create(2));
+		fg.add("x2", -5*eye(2), "l1", 5*eye(2), Vector_(2, -1.0, 1.5), unit2);
 
 		return fg;
 	}

matlab/createGaussianFactorGraph.m

@@ -7,35 +7,24 @@ c = createNoisyConfig();
 % Create
 fg = GaussianFactorGraph;
 
-% Create shared Noise models
-sigma0_1 = SharedDiagonal([0.1;0.1]);
-sigma0_2 = SharedDiagonal([0.2;0.2]);
+% Create shared Noise model
+unit2 = SharedDiagonal([1;1]);
 
 % prior on x1
-A11=eye(2);
-b = - c.get('x1');
-f1 = GaussianFactor('x1', A11, b, sigma0_1); % generate a Gaussian factor of odometry
+I=eye(2);
+f1 = GaussianFactor('x1', 10*I, [-1;-1], unit2);
 fg.push_back(f1);
 
 % odometry between x1 and x2
-A21=-eye(2);
-A22=eye(2);
-b = [.2;-.1];
-f2 = GaussianFactor('x1', A21,  'x2', A22, b,sigma0_1);
+f2 = GaussianFactor('x1', -10*I, 'x2', 10*I, [2;-1], unit2);
 fg.push_back(f2);
 
 % measurement between x1 and l1
-A31=-eye(2);
-A33=eye(2);
-b = [0;.2];
-f3 = GaussianFactor('x1', A31, 'l1', A33, b,sigma0_2);
+f3 = GaussianFactor('x1', -5*I, 'l1', 5*I, [0;1], unit2);
 fg.push_back(f3);
 
 % measurement between x2 and l1
-A42=-eye(2);
-A43=eye(2);
-b = [-.2;.3];
-f4 = GaussianFactor('x2', A42, 'l1', A43, b,sigma0_2);
+f4 = GaussianFactor('x2', -5*I, 'l1', 5*I, [-1;1.5], unit2);
 fg.push_back(f4);
 
 end

matlab/testGaussianFactor.m

@@ -25,12 +25,10 @@ Ax1 = [
 
 % the RHS
 b2=[-1;1.5;2;-1];
-model = SharedDiagonal([1;1]);
-combined = GaussianFactor('x2', Ax2,  'l1', Al1, 'x1', Ax1, b2, model);
+model4 = SharedDiagonal([1;1;1;1]);
+combined = GaussianFactor('x2', Ax2,  'l1', Al1, 'x1', Ax1, b2, model4);
 
 % eliminate the combined factor
-% NOT WORKING
-% this is not working because the eliminate has to be added back to gtsam.h
 [actualCG,actualLF] = combined.eliminate('x2');
 
 % create expected Conditional Gaussian
@@ -47,7 +45,7 @@ S13 = [
 +0.00,-8.94427
 ];
 d=[2.23607;-1.56525];
-expectedCG = GaussianConditional('x2',d,R11,'l1',S12,'x1',S13,model);
+expectedCG = GaussianConditional('x2',d,R11,'l1',S12,'x1',S13,[1;1]);
 
 % the expected linear factor
 Bl1 = [
@@ -64,10 +62,9 @@ Bx1 = [
 % the RHS
 b1= [0.0;0.894427];
 
-expectedLF = GaussianFactor('l1', Bl1, 'x1', Bx1, b1, 1);
+model2 = SharedDiagonal([1;1]);
+expectedLF = GaussianFactor('l1', Bl1, 'x1', Bx1, b1, model2);
 
 % check if the result matches
-% NOT WORKING 
-% because can not be computed with GaussianFactor.eliminate
-if(~actualCG.equals(expectedCG)), warning('GTSAM:unit','~actualCG.equals(expectedCG)'); end
-if(~actualLF.equals(expectedLF,1e-5)), warning('GTSAM:unit','~actualLF.equals(expectedLF,1e-5)');end
+CHECK('actualCG.equals(expectedCG,1e-5)',actualCG.equals(expectedCG,1e-4));
+CHECK('actualLF.equals(expectedLF,1e-5)',actualLF.equals(expectedLF,1e-4));

matlab/testGaussianFactorGraph.m

@@ -22,24 +22,18 @@ actual = fg.eliminateOne('x2');
 
 %-----------------------------------------------------------------------
 % eliminateAll
-sigma1=[.1;.1];
 I = eye(2);
-R1 = I;
-d1=[-.1;-.1];
-cg1 = GaussianConditional('x1',d1, R1,sigma1);
+cg1 = GaussianConditional('x1',[-1;-1], 10*I,[1;1]);
 
-sigma2=[0.149071; 0.149071];
-R2 = I;
-A1= -I;
-d2=[0; .2];
-cg2 = GaussianConditional('l1',d2, R2, 'x1', A1,sigma2);
+sig1=0.149071;
+d2=[0; .2]/sig1;
+cg2 = GaussianConditional('l1', d2, I/sig1, 'x1', -I/sig1, [1;1]);
 
-sigma3=[0.0894427; 0.0894427];
-R3 = I;
-A21 = -0.2*I;
-A22 = -0.8*I;
-d3 =[.2; -.14];
-cg3 = GaussianConditional('x2',d3, R3, 'l1', A21, 'x1', A22, sigma3);
+sig2 = 0.0894427;
+A21 = -0.2*I/sig2;
+A22 = -0.8*I/sig2;
+d3 =[.2; -.14]/sig2;
+cg3 = GaussianConditional('x2',d3, I/sig2, 'l1', A21, 'x1', A22, [1;1]);
 
 expected = GaussianBayesNet;
 expected.push_back(cg3);