diff --git a/matlab/createGaussianFactorGraph.m b/matlab/createGaussianFactorGraph.m index b9560ebf1..8b50b0075 100644 --- a/matlab/createGaussianFactorGraph.m +++ b/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 \ No newline at end of file diff --git a/matlab/create_gaussian_factor_graph.m b/matlab/create_gaussian_factor_graph.m new file mode 100644 index 000000000..35dc824cb --- /dev/null +++ b/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 \ No newline at end of file diff --git a/matlab/testGaussianFactorGraph.m b/matlab/testGaussianFactorGraph.m index e4d2703d1..7b96a876d 100644 --- a/matlab/testGaussianFactorGraph.m +++ b/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)); %----------------------------------------------------------------------- diff --git a/matlab/testPose2Factor.m b/matlab/testPose2Factor.m new file mode 100644 index 000000000..b549cd369 --- /dev/null +++ b/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; \ No newline at end of file diff --git a/matlab/test_time_average.m b/matlab/test_time_average.m index e778ed5cc..990fa849e 100644 --- a/matlab/test_time_average.m +++ b/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; diff --git a/matlab/test_time_new.m b/matlab/test_time_new.m index 3cd09650c..835ced70b 100644 --- a/matlab/test_time_new.m +++ b/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;