fixed Matlab examples

matlab/createGaussianFactorGraph.m

@@ -0,0 +1,61 @@
+% create a linear factor graph
+% The non-linear graph above evaluated at NoisyConfig
+function fg = createGaussianFactorGraph()
+
+c = createNoisyConfig(); 
+
+% Create
+fg = GaussianFactorGraph;
+sigma1=.1;
+
+% prior on x1
+A11=eye(2);
+b = - c.get('x1');
+
+f1 = GaussianFactor('x1', A11, b, sigma1); % generate a Gaussian factor of odometry
+fg.push_back(f1);
+
+% odometry between x1 and x2
+sigma2=.1;
+
+A21=-eye(2);
+A22=eye(2);
+b = [.2;-.1];
+
+f2 = GaussianFactor('x1', A21,  'x2', A22, b,sigma2);
+
+fg.push_back(f2);
+
+% measurement between x1 and l1
+sigma3=.2;
+A31=-eye(2);
+A33=eye(2);
+b = [0;.2];
+
+
+f3 = GaussianFactor('x1', A31, 'l1', A33, b,sigma3);
+
+fg.push_back(f3);
+
+% measurement between x2 and l1
+sigma4=.2;
+A42=-eye(2);
+A43=eye(2);
+b = [-.2;.3];
+
+
+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/createLinearFactorGraph.m

@@ -1,62 +0,0 @@
-% create a linear factor graph
-% The non-linear graph above evaluated at NoisyConfig
-function fg = createGaussianFactorGraph()
-
-c = createNoisyConfig();
-
-% Create
-fg = GaussianFactorGraph;
-sigma1=.1;
-
-% prior on x1
-A11=eye(2);
-b = - c.get('x1');
-
-<<<<<<< .mine
-f1 = LinearFactor('x1', A11, b, sigma1);
-=======
-f1 = GaussianFactor('x1', A11, b);
->>>>>>> .r1017
-fg.push_back(f1);
-
-% odometry between x1 and x2
-sigma2=.1;
-
-<<<<<<< .mine
-A21=-eye(2);
-A22=eye(2);
-b = [.2;-.1];
-
-f2 = LinearFactor('x1', A21,  'x2', A22, b,sigma2);
-=======
-f2 = GaussianFactor('x1', A21,  'x2', A22, b);
->>>>>>> .r1017
-fg.push_back(f2);
-
-% measurement between x1 and l1
-sigma3=.2;
-A31=-eye(2);
-A33=eye(2);
-b = [0;.2];
-
-<<<<<<< .mine
-f3 = LinearFactor('x1', A31, 'l1', A33, b,sigma3);
-=======
-f3 = GaussianFactor('x1', A31, 'l1', A32, b);
->>>>>>> .r1017
-fg.push_back(f3);
-
-% measurement between x2 and l1
-sigma4=.2;
-A42=-eye(2);
-A43=eye(2);
-b = [-.2;.3];
-
-<<<<<<< .mine
-f4 = LinearFactor('x2', A42, 'l1', A43, b,sigma4);
-=======
-f4 = GaussianFactor('x2', A41, 'l1', A42, b);
->>>>>>> .r1017
-fg.push_back(f4);
-
-end

matlab/create_config.m

@@ -6,7 +6,7 @@ function config = create_config(n,m)
 config = VectorConfig();
 
 for j = 1:n
-    config.insert(sprintf('m%d',j), [0;0]);
+    config.insert(sprintf('l%d',j), [0;0]);
 end
 
 for i = 1:m

matlab/create_good_ordering.m

@@ -10,7 +10,7 @@ mes=size(measurements,2);
 while (pose<=m)&&(j<=mes)
     ord.push_back(sprintf('x%d',pose));
     while (j<n)&&(measurements{j}.i==pose)
-        ord.push_back(sprintf('m%d',j));
+        ord.push_back(sprintf('l%d',j));
         j=j+1;
     end
     pose=pose+1;

matlab/create_linear_factor_graph.m

@@ -15,7 +15,7 @@ lfg.push_back(lf);
 
 % add prior for landmarks
 for j = 1:n
-    key = sprintf('m%d',j);
+    key = sprintf('l%d',j);
     prior = Point2Prior([0;0],1000,key);
     lf = prior.linearize(config); 
     lfg.push_back(lf);
@@ -25,7 +25,7 @@ end
 for k = 1 : size(measurements,2) 
     measurement = measurements{k};
     i = sprintf('x%d',measurement.i);
-    j = sprintf('m%d',measurement.j); 
+    j = sprintf('l%d',measurement.j); 
     nlf = Simulated2DMeasurement(measurement.z, measurement_sigma, i, j);
     lf = nlf.linearize(config);
     lfg.push_back(lf);

matlab/create_ordering.m

@@ -6,7 +6,7 @@ function ord = create_ordering(n,m)
 ord = Ordering();
 
 for j = 1:n
-    ord.push_back(sprintf('m%d',j));
+    ord.push_back(sprintf('l%d',j));
 end
 
 for i = 1:m

matlab/example.m

@@ -1,68 +0,0 @@
-% Set up a small SLAM example in MATLAB
-% Authors: Christian Potthast, Frank Dellaert
-
-clear;
-
-n = 1000;
-m = 200;
-
-% Set up the map 
-    map = create_random_landmarks(n,[1000,1000]);
-figure(1);clf;
-plot(map(1,:), map(2,:),'g.'); hold on;
-
-% have the robot move in this world
-trajectory = random_walk([0.1,0.1],5,m);
-plot(trajectory(1,:),trajectory(2,:),'b+');
-axis([0 1000 0 1000]);axis square;
-
-% Check visibility and plot this on the problem figure
-visibility = create_visibility(map, trajectory,50);
-gplot(visibility,[map trajectory]');
-figure(2);clf;
-spy(visibility)
-
-% simulate the measurements
-measurement_sigma = 1;
-odo_sigma = 0.1;
-[measurements, odometry] = simulate_measurements(map, trajectory, visibility, measurement_sigma, odo_sigma);
-
-% create a configuration of all zeroes
-config = create_config(n,m);
-
-% create the factor graph
-linearFactorGraph = create_linear_factor_graph(config, measurements, odometry, measurement_sigma, odo_sigma, n);
-
-% create an ordering
-ord = create_ordering(n,m);
-
-% show the matrix
-figure(3); clf;
-[A_dense,b] = linearFactorGraph.matrix(ord);
-%spy(A);
- A=sparse(A_dense);
-% eliminate with that ordering
-ck = cputime;
-BayesNet = linearFactorGraph.eliminate(ord);
-time_gtsam = cputime - ck
-
-ckqr = cputime;
-R = qr(A);
-time_qr = cputime - ckqr
-
-
-%time_gtsam=[time_gtsam,(cputime-ck)]
-% show the eliminated matrix
-figure(4); clf;
-[R,d] = BayesNet.matrix();
-spy(R);
-
-% optimize in the BayesNet
-optimal = BayesNet.optimize;
-
-% plot the solution
-figure(5);clf; 
-plot_config(optimal,n,m);hold on
-plot(trajectory(1,:),trajectory(2,:),'b+');
-plot(map(1,:), map(2,:),'g.');
-axis([0 1000 0 1000]);axis square;

matlab/plot_config.m

@@ -1,19 +0,0 @@
-% Christian Potthast
-% plot a configuration 
-
-function plot_config(config,n,m)
-
-hold on;
-
-for j = 1:n
-    key = sprintf('m%d',j);
-    mj = config.get(key);
-    plot(mj(1), mj(2),'r*');
-end
-
-for i = 1:m
-    key = sprintf('x%d',i);
-    xi = config.get(key);
-    plot(xi(1), xi(2),'rx');
-end
-

matlab/testConditionalGaussian.m

@@ -1,28 +0,0 @@
-%-----------------------------------------------------------------------
-% solve
-
-expected = [15.0471 ; -18.8824];
-
-% create a conditional gaussion node
-A1 =[1 2; 3 4];
-A2 = [6 0.2;8 0.4];
-R = [0.1 0.3; 0.0 0.34];
-d=[0.2;0.5];
-tau=[1;.34];
-
-
-cg = ConditionalGaussian('x',d, R, 'x1', A1, 'l1', A2, tau);
-
-sx1 = [0.2;0.5];
-sl1 = [0.5;0.8];
-
-%solution = FGConfig;
-solution.insert('x1', sx1);
-solution.insert('l1', sl1);
-
-result = cg.solve(solution);
-
-if(~all( abs(expected - result) < 0.0001 )) warning('solve failed'); end
-    
-%-----------------------------------------------------------------------
-

matlab/testGaussianFactor.m

@@ -48,7 +48,7 @@ S13 = [
 +0.00,-8.94427
 ];
 d=[2.23607;-1.56525];
-expectedCG = ConditionalGaussian('x2',d,R11,'l1',S12,'x1',S13,[1 1]');
+expectedCG = GaussianConditional('x2',d,R11,'l1',S12,'x1',S13,[1 1]');
 
 % the expected linear factor
 Bl1 = [

matlab/testGaussianFactorGraph.m

@@ -0,0 +1,112 @@
+%-----------------------------------------------------------------------
+% equals
+fg = createGaussianFactorGraph();
+fg2 = createGaussianFactorGraph();
+CHECK('equals',fg.equals(fg,fg2));
+
+%-----------------------------------------------------------------------
+% error
+cfg = createZeroDelta();
+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();
+actual = fg.eliminateOne('x1');
+
+%-----------------------------------------------------------------------
+% eliminate_x2
+fg = createGaussianFactorGraph();
+actual = fg.eliminateOne('x2');
+
+%-----------------------------------------------------------------------
+% eliminateAll
+sigma1=.1;
+R1 = eye(2);
+d1=[-.1;-.1];
+cg1 = ConditionalGaussian('x1',d1, R1,sigma1);
+
+sigma2=0.149071;
+R2 = eye(2);
+A1= -eye(2);
+d2=[0; .2];
+cg2 = ConditionalGaussian('l1',d2, R2, 'x1', A1,sigma2);
+
+sigma3=0.0894427;
+R3 = eye(2);
+A21 = [ -.2, 0.0
+    0.0, -.2];
+A22 = [-.8, 0.0
+    0.0, -.8];
+d3 =[.2; -.14];
+cg3 = ConditionalGaussian('x2',d3, R3, 'l1', A21, 'x1', A22, sigma3);
+
+expected = GaussianBayesNet;
+expected.push_back(cg1);
+expected.push_back(cg2);
+expected.push_back(cg3);
+expected.print_();
+% Check one ordering
+fg1 = createGaussianFactorGraph();
+ord1 = Ordering;
+ord1.push_back('x2');
+ord1.push_back('l1');
+ord1.push_back('x1');
+actual1 = fg1.eliminate_(ord1);
+actual1.print();
+%CHECK('eliminateAll', actual1.equals(expected));
+
+%-----------------------------------------------------------------------
+% matrix
+
+fg = createGaussianFactorGraph();
+ord = Ordering;
+ord.push_back('x2');
+ord.push_back('l1');
+ord.push_back('x1');
+
+A = fg.matrix(ord);
+

matlab/test_time.m

@@ -1,96 +0,0 @@
-% Set up a small SLAM example in MATLAB to test the execution time
-
-clear;
-
-%Parameters
-noRuns=5;
-steps=1;
-m = 5;
-velocity=1;
-time_qr=[];
-time_gtsam=[];
-for steps=1:noRuns
- 
-    %figure(1);clf;
-    % robot moves in the world
-    trajectory = walk([0.1,0.1],velocity,m);
-    mappingArea=max(trajectory,[],2);
-    %plot(trajectory(1,:),trajectory(2,:),'b+'); hold on;
-
-    visibilityTh=sqrt(mappingArea(1)^2+mappingArea(2)^2)/m; %distance between poses
-    % Set up the map
-    map = create_landmarks(visibilityTh, mappingArea,steps);
-    %plot(map(1,:), map(2,:),'g.');
-    %axis([0 mappingArea(1) 0 mappingArea(2)]); axis square;
-    n=size(map,1)*size(map,2);
-    % Check visibility and plot this on the problem figure
-    visibilityTh=visibilityTh+steps;
-    visibility = create_visibility(map, trajectory,visibilityTh);
-    %gplot(visibility,[map trajectory]');
-    
-steps
-    % simulate the measurements
-    measurement_sigma = 1;
-    odo_sigma = 0.1;
-    [measurements, odometry] = simulate_measurements(map, trajectory, visibility, measurement_sigma, odo_sigma);
-    
-    
-%     % create a configuration of all zeroes
-     config = create_config(n,m);
-
-    % create the factor graph
-    linearFactorGraph = create_linear_factor_graph(config, measurements, odometry, measurement_sigma, odo_sigma, n);
-    % 
-    % create an ordering
-    ord = create_ordering(n,m);
-
-    % show the matrix
-   % figure(3); clf;
-    %[A_dense,b] = linearFactorGraph.matrix(ord);
-    %A=sparse(A_dense);
-
-    ijs = linearFactorGraph.sparse(ord);
-    A=sparse(ijs(1,:),ijs(2,:),ijs(3,:)); 
-    
-    %spy(A);
-    %time qr
-    ck=cputime;
-    R_qr = qr(A);
-    time_qr=[time_qr,(cputime-ck)];
-
-    %figure(2)
-    %clf
-    %spy(R_qr);
-    
-    % eliminate with that ordering
-    %time gt_sam
-    ck=cputime;
-    BayesNet = linearFactorGraph.eliminate_(ord);
-    time_gtsam=[time_gtsam,(cputime-ck)];
-    
-    clear trajectory visibility linearFactorGraph measurements odometry;
-    m = m+5;
-    velocity=velocity+1;
-    steps=steps+1;
-end
-plot(time_qr,'r');hold on;
-plot(time_gtsam,'b');
-
-
-
-
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% % show the eliminated matrix
-% figure(4); clf;
-% [R,d] = BayesNet.matrix();
-% spy(R);
-% 
-% % optimize in the BayesNet
-% optimal = BayesNet.optimize;
-% 
-% % plot the solution
-% figure(5);clf; 
-% plot_config(optimal,n,m);hold on
-% plot(trajectory(1,:),trajectory(2,:),'b+');
-% plot(map(1,:), map(2,:),'g.');
-% axis([0 10 0 10]);axis square;

matlab/test_time_new.m

@@ -78,7 +78,7 @@ time_gtsam=[];
 %     end
 ck_gt=cputime;
 for i=1:runs+10
-    BayesNet = linearFactorGraph.eliminate(ord);
+    BayesNet = linearFactorGraph.eliminate_(ord);
 end
     time_gtsam=(cputime-ck_gt)/runs
     %time_gtsam=[time_gtsam,(cputime-ck)];