Added MATLAB-friendly versions of eliminate and optimize in LinearFactorGraph (with trailing underscore). Also fixed some MATLAB code.

cpp/LinearFactorGraph.cpp

@@ -64,6 +64,18 @@ VectorConfig LinearFactorGraph::optimize(const Ordering& ordering)
 	return newConfig;
 }
 
+/* ************************************************************************* */
+boost::shared_ptr<GaussianBayesNet>
+LinearFactorGraph::eliminate_(const Ordering& ordering) {
+	return boost::shared_ptr<GaussianBayesNet>(new GaussianBayesNet(eliminate(ordering)));
+}
+
+/* ************************************************************************* */
+boost::shared_ptr<VectorConfig>
+LinearFactorGraph::optimize_(const Ordering& ordering) {
+	return boost::shared_ptr<VectorConfig>(new VectorConfig(optimize(ordering)));
+}
+
 /* ************************************************************************* */
 void LinearFactorGraph::combine(const LinearFactorGraph &lfg){
 	for(const_iterator factor=lfg.factors_.begin(); factor!=lfg.factors_.end(); factor++){

cpp/LinearFactorGraph.h

@@ -83,6 +83,12 @@ namespace gtsam {
      */
     VectorConfig optimize(const Ordering& ordering);
 
+    /**
+     * shared pointer versions for MATLAB
+     */
+    boost::shared_ptr<GaussianBayesNet> eliminate_(const Ordering&);
+    boost::shared_ptr<VectorConfig> optimize_(const Ordering&);
+
     /**
      * static function that combines two factor graphs
      * @param const &lfg1 Linear factor graph

cpp/gtsam.h

@@ -48,13 +48,13 @@ class ConditionalGaussian {
   ConditionalGaussian(string key,
   		    Vector d,
 		      Matrix R,
-		      Vector precisions);
+		      Vector sigmas);
   ConditionalGaussian(string key,
   		    Vector d,
 		      Matrix R,
 		      string name1,
 		      Matrix S,
-		      Vector precisions);
+		      Vector sigmas);
   ConditionalGaussian(string key,
   		    Vector d,
 		      Matrix R,
@@ -62,7 +62,7 @@ class ConditionalGaussian {
 		      Matrix S,
 		      string name2,
 		      Matrix T,
-		      Vector precisions);
+		      Vector sigmas);
   void print() const;
   Vector solve(const VectorConfig& x);
   void add(string key, Matrix S);
@@ -91,8 +91,8 @@ class LinearFactorGraph {
   void print() const;
   bool equals(const LinearFactorGraph& lfgraph) const;
 
-  VectorConfig optimize(const Ordering& ordering);
-  void eliminate(const Ordering& ordering);
+  VectorConfig* optimize_(const Ordering& ordering);
+  GaussianBayesNet* eliminate_(const Ordering& ordering);
   pair<Matrix,Vector> matrix(const Ordering& ordering) const;
   Matrix sparse(const Ordering& ordering) const;
 };

matlab/createNoisyConfig.m

@@ -3,7 +3,7 @@ function c = createNoisyConfig()
 v_x1 = [0.1;  0.1];
 v_x2 = [1.4;  0.2];
 v_l1 = [0.1; -1.1];
-c = FGConfig();
+c = VectorConfig();
 c.insert('x1', v_x1);
 c.insert('x2', v_x2);
 c.insert('l1', v_l1);

matlab/testLinearFactor.m

@@ -26,7 +26,7 @@ Ax1 = [
 % the RHS
 b2=[-1;1.5;2;-1];
 
-combined = LinearFactor('x2', Ax2,  'l1', Al1, 'x1', Ax1, b2);
+combined = LinearFactor('x2', Ax2,  'l1', Al1, 'x1', Ax1, b2, 1);
 
 % eliminate the combined factor
 % NOT WORKING
@@ -48,7 +48,7 @@ S13 = [
 +0.00,-8.94427
 ];
 d=[2.23607;-1.56525];
-expectedCG = ConditionalGaussian(d,R11,'l1',S12,'x1',S13);
+expectedCG = ConditionalGaussian('x2',d,R11,'l1',S12,'x1',S13,[1 1]');
 
 % the expected linear factor
 Bl1 = [
@@ -65,7 +65,7 @@ Bx1 = [
 % the RHS
 b1= [0.0;0.894427];
 
-expectedLF = LinearFactor('l1', Bl1, 'x1', Bx1, b1);
+expectedLF = LinearFactor('l1', Bl1, 'x1', Bx1, b1, 1);
 
 % check if the result matches
 % NOT WORKING 

matlab/test_time.m

@@ -62,7 +62,7 @@ for steps=1:noRuns
     % eliminate with that ordering
     %time gt_sam
     ck=cputime;
-    BayesNet = linearFactorGraph.eliminate(ord);
+    BayesNet = linearFactorGraph.eliminate_(ord);
     time_gtsam=[time_gtsam,(cputime-ck)];
     
     clear trajectory visibility linearFactorGraph measurements odometry;