update Matlab files

matlab/createGaussianFactorGraph.m

@@ -48,14 +48,4 @@ f4 = GaussianFactor('x2', A42, 'l1', A43, b,sigma4);
 
 fg.push_back(f4);
 
-
-% Optimization
-n=1;
-m=2;
-
-ord = create_ordering(n,m);
-
-%BayesNet = GaussianFactorGraph.eliminate_(ord);
-
-
 end

matlab/create_gaussian_factor_graph.m

@@ -0,0 +1,42 @@
+% Christan Potthast
+% create linear factor graph
+
+function lfg = create_gaussian_factor_graph(config, measurements, odometry, measurement_sigma, odo_sigma, n)
+
+m = size(measurements,2);
+
+% create linear factor graph
+lfg = GaussianFactorGraph();
+
+% create prior for initial robot pose
+prior = Point2Prior([0;0],0.2,'x1');
+lf = prior.linearize(config);
+lfg.push_back(lf);
+
+% add prior for landmarks
+for j = 1:n
+    key = sprintf('l%d',j);
+    prior = Point2Prior([0;0],1000,key);
+    lf = prior.linearize(config); 
+    lfg.push_back(lf);
+end
+
+% add measurement factors
+for k = 1 : size(measurements,2) 
+    measurement = measurements{k};
+    i = sprintf('x%d',measurement.i);
+    j = sprintf('l%d',measurement.j); 
+    nlf = Simulated2DMeasurement(measurement.z, measurement_sigma, i, j);
+    lf = nlf.linearize(config);
+    lfg.push_back(lf);
+end
+
+% add odometry factors
+for i = 2 : size(odometry,2)
+    odo = odometry{i};
+    p = sprintf('x%d',i-1);
+    c = sprintf('x%d',i);
+    nlf = Simulated2DOdometry(odo, odo_sigma, p, c);
+    lf = nlf.linearize(config);
+    lfg.push_back(lf);
+end

matlab/testGaussianFactorGraph.m

@@ -11,47 +11,6 @@ actual = fg.error(cfg);
 DOUBLES_EQUAL( 5.625, actual, 1e-9 );
 
 %-----------------------------------------------------------------------
-% combine_factors_x1
-fg = createGaussianFactorGraph();
-%actual = fg.combine_factors('x1');
-actual = fg.combined('x1');
-Al1 = [
-   0., 0.
-   0., 0.
-   0., 0.
-   0., 0.
-   5., 0.
-   0., 5.
-  ];
-                     
-Ax1 = [
-  10.,   0.
-  0.00, 10.
-  -10.,  0.
-  0.00,-10.
-  -5.,   0.
-  00.,  -5.
-  ];
-
-Ax2 = [
-   0., 0.
-   0., 0.
-   10., 0.
-   +0.,10.
-   0., 0.
-   0., 0.
-  ];
-
-b=[-1;-1;2;-1;0;1];
-
-expected = GaussianFactor('l1',Al1,'x1',Ax1,'x2',Ax2,b);
-CHECK('combine_factors_x1', actual.equals(expected,1e-9));
-
-%-----------------------------------------------------------------------
-% combine_factors_x2
-fg = createGaussianFactorGraph();
-actual = fg.combine_factors('x2');
-
 %-----------------------------------------------------------------------
 % eliminate_x1
 fg = createGaussianFactorGraph();
@@ -67,13 +26,13 @@ actual = fg.eliminateOne('x2');
 sigma1=.1;
 R1 = eye(2);
 d1=[-.1;-.1];
-cg1 = ConditionalGaussian('x1',d1, R1,sigma1);
+cg1 = GaussianConditional('x1',d1, R1,sigma1);
 
 sigma2=0.149071;
 R2 = eye(2);
 A1= -eye(2);
 d2=[0; .2];
-cg2 = ConditionalGaussian('l1',d2, R2, 'x1', A1,sigma2);
+cg2 = GaussianConditional('l1',d2, R2, 'x1', A1,sigma2);
 
 sigma3=0.0894427;
 R3 = eye(2);
@@ -82,13 +41,13 @@ A21 = [ -.2, 0.0
 A22 = [-.8, 0.0
     0.0, -.8];
 d3 =[.2; -.14];
-cg3 = ConditionalGaussian('x2',d3, R3, 'l1', A21, 'x1', A22, sigma3);
+cg3 = GaussianConditional('x2',d3, R3, 'l1', A21, 'x1', A22, sigma3);
 
 expected = GaussianBayesNet;
 expected.push_back(cg1);
 expected.push_back(cg2);
 expected.push_back(cg3);
-expected.print_();
+expected.print('expected');
 % Check one ordering
 fg1 = createGaussianFactorGraph();
 ord1 = Ordering;
@@ -96,7 +55,7 @@ ord1.push_back('x2');
 ord1.push_back('l1');
 ord1.push_back('x1');
 actual1 = fg1.eliminate_(ord1);
-actual1.print();
+actual1.print('actual');
 %CHECK('eliminateAll', actual1.equals(expected));
 
 %-----------------------------------------------------------------------

matlab/testPose2Factor.m

@@ -0,0 +1,13 @@
+cov = [ 0.25, 0, 0; 0, 0.25, 0; 0, 0, 0.01];
+key1='x1';
+key2='x2';
+measured=Pose2(2,2, pi/2);
+measured.print('Pose');
+factor=Pose2Factor(key1,key2,measured, cov);
+factor.print('Factor');
+
+p1=Pose2(1.1,2,pi/2); % robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
+p2=Pose2(-1,4.1,pi);  % robot at (-1,4) looking at negative (ground truth is at 4.1,2)
+config= Pose2Config() ;
+
+%fg = Pose2Graph;

matlab/test_time_average.m

@@ -39,12 +39,12 @@ for steps=1:noRuns
      config = create_config(n,m);
 
     % create the factor graph
-    linearFactorGraph = create_linear_factor_graph(config, measurements, odometry, measurement_sigma, odo_sigma, n);
+    gaussianFactorGraph = create_gaussian_factor_graph(config, measurements, odometry, measurement_sigma, odo_sigma, n);
     % 
     % create an ordering
     ord = create_ordering(n,m);
 
-    ijs = linearFactorGraph.sparse(ord);
+    ijs = gaussianFactorGraph.sparse(ord);
     A=sparse(ijs(1,:),ijs(2,:),ijs(3,:)); 
 
     
@@ -62,11 +62,11 @@ for steps=1:noRuns
         if it==2
         ck_gt=cputime;
         end
-        BayesNet = linearFactorGraph.eliminate_(ord); 
+        BayesNet = gaussianFactorGraph.eliminate_(ord); 
     end
     time_gtsam=[time_gtsam,(cputime-ck_gt)/runs];
     
-    clear trajectory visibility linearFactorGraph measurements odometry;
+    clear trajectory visibility gaussianFactorGraph measurements odometry;
     m = m+5;
     velocity=velocity+1;
     steps=steps+1;

matlab/test_time_new.m

@@ -40,18 +40,18 @@ time_gtsam=[];
      config = create_config(n,m);
 
     % create the factor graph
-    linearFactorGraph = create_linear_factor_graph(config, measurements, odometry, measurement_sigma, odo_sigma, n);
+    gaussianFactorGraph = create_linear_factor_graph(config, measurements, odometry, measurement_sigma, odo_sigma, n);
     % 
     % create an ordering
     %ord = create_good_ordering(n,m,measurements);
     ord = create_ordering(n,m);
     % show the matrix
    % figure(3); clf;
-    %[A_dense,b] = linearFactorGraph.matrix(ord);
+    %[A_dense,b] = gaussianFactorGraph.matrix(ord);
    %A=sparse(A_dense);
     
     %sparse matrix !!!
-    ijs = linearFactorGraph.sparse(ord);
+    ijs = gaussianFactorGraph.sparse(ord);
     A=sparse(ijs(1,:),ijs(2,:),ijs(3,:)); 
     %spy(A);
     %time qr
@@ -74,16 +74,16 @@ time_gtsam=[];
 %         if i==11
 %         ck_gt=cputime;
 %         end
-%         BayesNet = linearFactorGraph.eliminate(ord); 
+%         BayesNet = gaussianFactorGraph.eliminate(ord); 
 %     end
 ck_gt=cputime;
 for i=1:runs+10
-    BayesNet = linearFactorGraph.eliminate_(ord);
+    BayesNet = gaussianFactorGraph.eliminate_(ord);
 end
     time_gtsam=(cputime-ck_gt)/runs
     %time_gtsam=[time_gtsam,(cputime-ck)];
     
-%     clear trajectory visibility linearFactorGraph measurements odometry;
+%     clear trajectory visibility gaussianFactorGraph measurements odometry;
 %     m = m+5;
 %     velocity=velocity+1;