Moved matlab to experimental
Richard Roberts
parent
078f07650f
commit
e9804afc07
|
|
@ -1,85 +0,0 @@
|
|||
% Script to perform SQP on a simple example from the SQP tutorial
|
||||
% This makes use of LDL solving of a full quadratic programming
|
||||
% problem
|
||||
%
|
||||
% Problem:
|
||||
% min(x) f(x) = (x2-2)^2 - x1^2
|
||||
% s.t. c(x) = 4x1^2 + x2^2 - 1 =0
|
||||
% state is x = [x1 x2]' , with dim(state) = 2
|
||||
% constraint has dim p = 1
|
||||
|
||||
n = 2;
|
||||
p = 1;
|
||||
|
||||
% initial conditions
|
||||
x0 = [2; 4];
|
||||
lam0 = -0.5;
|
||||
x = x0; lam = lam0;
|
||||
|
||||
maxIt = 100;
|
||||
|
||||
X = x0; Lam = lam0;
|
||||
Bi = eye(2);
|
||||
|
||||
for i=1:maxIt
|
||||
i
|
||||
x1 = x(1); x2 = x(2);
|
||||
|
||||
% evaluate functions
|
||||
fx = (x2-2)^2 + x1^2;
|
||||
cx = 4*x1^2 + x2^2 - 1;
|
||||
|
||||
% evaluate derivatives in x
|
||||
dfx = [2*x1; 2*(x2-2)];
|
||||
dcx = [8*x1; 2*x2];
|
||||
dL = dfx - lam * dcx;
|
||||
|
||||
% update the hessian (BFGS) - note: this does not use the full lagrangian
|
||||
if (i>1)
|
||||
Bis = Bi*s;
|
||||
y = dfx - prev_dfx;
|
||||
Bi = Bi + (y*y')/(y'*s) - (Bis*Bis')/(s'*Bis);
|
||||
end
|
||||
prev_dfx = dfx;
|
||||
|
||||
% evaluate hessians in x
|
||||
ddfx = diag([2, 2]);
|
||||
ddcx = diag([8, 2]);
|
||||
|
||||
% construct and solve CQP subproblem
|
||||
Bgn0 = dfx * dfx';
|
||||
Bgn1 = dfx * dfx' - lam * dcx * dcx'; % GN approx 1
|
||||
Bgn2 = dL * dL'; % GN approx 2
|
||||
Ba = ddfx - lam * ddcx; % analytic hessians
|
||||
|
||||
B = Bi;
|
||||
g = dfx;
|
||||
h = -cx;
|
||||
[delta lambda] = solveCQP(B, -dcx, -dcx', g, h);
|
||||
|
||||
% update
|
||||
s = 0.1*delta;
|
||||
x = x + s
|
||||
lam = lambda
|
||||
|
||||
|
||||
% save
|
||||
X = [X x];
|
||||
Lam = [Lam lam];
|
||||
|
||||
end
|
||||
|
||||
% verify
|
||||
xstar = [0; 1];
|
||||
lamstar = -1;
|
||||
display(fx)
|
||||
display(cx)
|
||||
final_error = norm(x-xstar) + norm(lam-lamstar)
|
||||
|
||||
% draw
|
||||
figure(1);
|
||||
clf;
|
||||
ezcontour('(x2-2)^2 + x1^2');
|
||||
hold on;
|
||||
ezplot('4*x1^2 + x2^2 - 1');
|
||||
plot(X(1,:), X(2,:), 'r*-');
|
||||
|
|
@ -1,59 +0,0 @@
|
|||
% Set up a small SLAM example
|
||||
% Christian Potthast, Frank Dellaert
|
||||
|
||||
clear;
|
||||
close all;
|
||||
|
||||
n = 100;
|
||||
m = 20;
|
||||
|
||||
% have the robot move in this world
|
||||
trajectory = random_walk([0.1,0.1],5,m);
|
||||
plot(trajectory(1,:),trajectory(2,:),'b+');
|
||||
axis([0 100 0 100]);axis square;
|
||||
|
||||
% Set up the map
|
||||
mappingArea=max(trajectory,[],2);
|
||||
map = create_random_landmarks(n, mappingArea);
|
||||
figure(1);clf;
|
||||
plot(map(1,:), map(2,:),'g.'); hold on;
|
||||
axis([0 mappingArea(1) 0 mappingArea(2)]);axis square;
|
||||
|
||||
% Check visibility and plot this on the problem figure
|
||||
visibility = create_visibility(map, trajectory,20);
|
||||
gplot(visibility,[map trajectory]');
|
||||
|
||||
% 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
|
||||
factorGraph = create_gaussian_factor_graph(config, measurements, odometry, measurement_sigma, odo_sigma, n);
|
||||
|
||||
% create an ordering
|
||||
ord = create_ordering(n,m);
|
||||
|
||||
% show the matrix
|
||||
figure(2); clf;
|
||||
ijs = factorGraph.sparse(ord);
|
||||
A=sparse(ijs(1,:),ijs(2,:),ijs(3,:));
|
||||
spy(A);
|
||||
|
||||
% optimizing a BayesNet is not possible from MATLAB as
|
||||
% GaussianBayesNet is a typedef not a real class :-(
|
||||
% BayesNet = factorGraph.eliminate_(ord);
|
||||
% optimal = BayesNet.optimize;
|
||||
|
||||
% However, we can call the optimize_ method of a GaussianFactorGraph
|
||||
optimal = factorGraph.optimize_(ord);
|
||||
|
||||
% plot the solution
|
||||
figure(3);clf;
|
||||
plot_config(optimal,n,m);hold on
|
||||
plot(trajectory(1,:),trajectory(2,:),'b+');
|
||||
plot(map(1,:), map(2,:),'g.');
|
||||
axis([0 mappingArea(1) 0 mappingArea(2)]);axis square;
|
||||
|
|
@ -1,15 +0,0 @@
|
|||
% Christian Potthast
|
||||
% create a configuration
|
||||
|
||||
function config = create_config(n,m)
|
||||
|
||||
config = Simulated2DConfig();
|
||||
origin = Point2;
|
||||
|
||||
for i = 1:m
|
||||
config.insertPose(i, origin);
|
||||
end
|
||||
|
||||
for j = 1:n
|
||||
config.insertPoint(j, origin);
|
||||
end
|
||||
|
|
@ -1,46 +0,0 @@
|
|||
% 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();
|
||||
|
||||
% Point2 at origin
|
||||
origin = Point2;
|
||||
|
||||
% create prior for initial robot pose
|
||||
model0_2 = SharedDiagonal([0.2;0.2]);
|
||||
prior = Simulated2DPosePrior(origin,model0_2,1);
|
||||
lf = prior.linearize(config);
|
||||
lfg.push_back(lf);
|
||||
|
||||
% add prior for landmarks
|
||||
model1000 = SharedDiagonal([1000;1000]);
|
||||
for j = 1:n
|
||||
prior = Simulated2DPointPrior(origin,model1000,j);
|
||||
lf = prior.linearize(config);
|
||||
lfg.push_back(lf);
|
||||
end
|
||||
|
||||
% add odometry factors
|
||||
odo_model = SharedDiagonal([odo_sigma;odo_sigma]);
|
||||
for i = 2 : size(odometry,2)
|
||||
odo = Point2(odometry{i}(1),odometry{i}(2));
|
||||
nlf = Simulated2DOdometry(odo, odo_model, i-1, i);
|
||||
lf = nlf.linearize(config);
|
||||
lfg.push_back(lf);
|
||||
end
|
||||
|
||||
% add measurement factors
|
||||
measurement_model = SharedDiagonal([measurement_sigma;measurement_sigma]);
|
||||
for k = 1 : size(measurements,2)
|
||||
measurement = measurements{k};
|
||||
point = Point2(measurement.z(1),measurement.z(2));
|
||||
nlf = Simulated2DMeasurement(point, measurement_model, measurement.i, measurement.j);
|
||||
lf = nlf.linearize(config);
|
||||
lfg.push_back(lf);
|
||||
end
|
||||
|
||||
|
|
@ -1,17 +0,0 @@
|
|||
% Christian Potthast
|
||||
% create an elimination ordering
|
||||
|
||||
function ord = create_good_ordering(n,m,measurements)
|
||||
|
||||
ord = Ordering();
|
||||
j=1;
|
||||
pose=1;
|
||||
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('l%d',j));
|
||||
j=j+1;
|
||||
end
|
||||
pose=pose+1;
|
||||
end
|
||||
|
|
@ -1,16 +0,0 @@
|
|||
% Christian Potthast
|
||||
% Create a map with random landmarks
|
||||
|
||||
function map = create_random_landmarks(visibilityTh, mappingArea, steps)
|
||||
map=[];
|
||||
points1=1:visibilityTh:mappingArea(1);
|
||||
points2=1:visibilityTh:mappingArea(2);
|
||||
for i=1:size(points1,2)
|
||||
map=[map,[points1(1,i)-steps;points2(1,i)],[points1(1,i);points2(1,i)-steps]];
|
||||
end
|
||||
|
||||
% for i=1:size(points1,2)
|
||||
% for j=1:size(points2,2)
|
||||
% map=[map,[points1(1,i);points2(1,j)]];
|
||||
% end
|
||||
% end
|
||||
|
|
@ -1,14 +0,0 @@
|
|||
% Christian Potthast
|
||||
% create an elimination ordering
|
||||
|
||||
function ord = create_ordering(n,m)
|
||||
|
||||
ord = Ordering();
|
||||
|
||||
for j = 1:n
|
||||
ord.push_back(sprintf('l%d',j));
|
||||
end
|
||||
|
||||
for i = 1:m
|
||||
ord.push_back(sprintf('x%d',i));
|
||||
end
|
||||
|
|
@ -1,8 +0,0 @@
|
|||
% Christian Potthast
|
||||
% Create a map with random landmarks
|
||||
|
||||
function map = create_random_landmarks(numberOfLandmarks, mappingArea)
|
||||
|
||||
points = rand(2,numberOfLandmarks);
|
||||
|
||||
map=[points(1,:)*mappingArea(1);points(2,:)*mappingArea(2)];
|
||||
|
|
@ -1,20 +0,0 @@
|
|||
% Christian Potthast
|
||||
% create visibility matrix
|
||||
|
||||
function V = create_visibility(map,pose,threshold)
|
||||
|
||||
n = size(map,2);
|
||||
m = size(pose,2);
|
||||
V = sparse([],[],[],n+m,n+m);
|
||||
|
||||
for t = 1:m
|
||||
% find measurements within Manhattan range
|
||||
js = find(2==sum(abs(map-pose(:,t)*ones(1,n))<[threshold;threshold]*ones(1,n)));
|
||||
for j = js
|
||||
V(j,t+n)=1;
|
||||
end
|
||||
% add in odometry links
|
||||
if t>1
|
||||
V((t+n)-1,t+n)=1;
|
||||
end
|
||||
end
|
||||
|
|
@ -1,19 +0,0 @@
|
|||
% Christian Potthast
|
||||
% plot a configuration
|
||||
|
||||
function plot_config(config,n,m)
|
||||
|
||||
hold on;
|
||||
|
||||
for j = 1:n
|
||||
key = sprintf('l%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
|
||||
|
||||
|
|
@ -1,20 +0,0 @@
|
|||
% Christian Potthast
|
||||
% random walk from a robot
|
||||
|
||||
function pose = random_walk(initial, velocity, steps)
|
||||
|
||||
pose(:,1) = initial;
|
||||
bearing = 1;
|
||||
|
||||
|
||||
for step = 2:steps
|
||||
|
||||
bearing = bearing + 0.05*randn();
|
||||
|
||||
pose(1,step) = pose(1,step-1) + sin(bearing) * velocity;
|
||||
pose(2,step) = pose(2,step-1) + cos(bearing) * velocity;
|
||||
|
||||
end
|
||||
|
||||
|
||||
|
||||
|
|
@ -1,23 +0,0 @@
|
|||
% Christian Potthast
|
||||
% simulate measurements
|
||||
|
||||
function [measurements,odometry] = simulate_measurements(map, pose, visibility, noise_sigma, odo_sigma)
|
||||
m = size(pose,2);
|
||||
n = size(map,2);
|
||||
measurements = {};
|
||||
odometry = {};
|
||||
k =1;
|
||||
for i = 1:m
|
||||
js = find(visibility(1:n,i+n));
|
||||
if size(js ,1) > 0
|
||||
for j = js'
|
||||
z = map(:,j)-pose(:,i)+randn(2,1)*noise_sigma;
|
||||
measurement = struct('z',z,'i',i,'j',j);
|
||||
measurements{k}=measurement;
|
||||
k = k+1;
|
||||
end
|
||||
end
|
||||
if i>1
|
||||
odometry{i}= pose(:,i)-pose(:,i-1)+randn(2,1)*odo_sigma;
|
||||
end
|
||||
end
|
||||
|
|
@ -1,16 +0,0 @@
|
|||
function [delta lambda] = solveCQP(B, A, At, g, h)
|
||||
|
||||
n = size(B,1);
|
||||
p = size(A,2);
|
||||
|
||||
% form the KKT matrix system
|
||||
G = [B A; At zeros(p,p)];
|
||||
rhs = -[g; h];
|
||||
|
||||
% solve with LDL
|
||||
[L D] = ldl(G);
|
||||
approx_error = norm(G - L*D*L'); %% verify error
|
||||
sol = L'\(D\(L\rhs));
|
||||
|
||||
delta = sol(1:n);
|
||||
lambda = sol(n+1:size(sol));
|
||||
|
|
@ -1,16 +0,0 @@
|
|||
% Test example for simple Constrained Quadratic Programming problem
|
||||
|
||||
A = [-1 -1;
|
||||
-2 1;
|
||||
1 -1];
|
||||
At = A';
|
||||
B = 2*eye(3,3);
|
||||
|
||||
b = [4; -2];
|
||||
g = zeros(3,1);
|
||||
|
||||
[delta lambda] = solveCQP(B, A, At, g, b);
|
||||
|
||||
expX = [2/7 10/7 -6/7]';
|
||||
|
||||
state_error = norm(expX-delta)
|
||||
|
|
@ -1,20 +0,0 @@
|
|||
% Christian Potthast
|
||||
% random walk from a robot
|
||||
|
||||
function pose = walk(initial, velocity, steps)
|
||||
|
||||
pose(:,1) = initial;
|
||||
bearing = 0.7854; % 45?
|
||||
|
||||
|
||||
for step = 2:steps
|
||||
|
||||
%bearing = bearing + 0.05;
|
||||
|
||||
pose(1,step) = pose(1,step-1) + sin(bearing) * velocity;
|
||||
pose(2,step) = pose(2,step-1) + cos(bearing) * velocity;
|
||||
|
||||
end
|
||||
|
||||
|
||||
|
||||
|
|
@ -1,14 +0,0 @@
|
|||
% Matlab code to test gtsam toolbox
|
||||
%
|
||||
%
|
||||
%
|
||||
%
|
||||
clc;
|
||||
disp(' ');
|
||||
disp(' Matlab scripts to test GTSAM toolbox ');
|
||||
disp(' ');
|
||||
|
||||
path(path,genpath(pwd));
|
||||
toolboxpath = '~/toolbox/gtsam'
|
||||
addpath(toolboxpath);
|
||||
|
||||
|
|
@ -1,5 +0,0 @@
|
|||
function CHECK(name,assertion)
|
||||
|
||||
if (assertion~=1)
|
||||
error(['CHECK ' name ' fails']);
|
||||
end
|
||||
|
|
@ -1,5 +0,0 @@
|
|||
function DOUBLES_EQUAL(expected,actual,atol)
|
||||
|
||||
if abs(expected-actual)>atol
|
||||
error(sprintf('DOUBLES_EQUAL fails: expected %g but got %g',expected,actual));
|
||||
end
|
||||
|
|
@ -1,30 +0,0 @@
|
|||
% create a linear factor graph
|
||||
% The non-linear graph above evaluated at NoisyConfig
|
||||
function fg = createGaussianFactorGraph()
|
||||
|
||||
c = createNoisyConfig();
|
||||
|
||||
% Create
|
||||
fg = GaussianFactorGraph;
|
||||
|
||||
% Create shared Noise model
|
||||
unit2 = SharedDiagonal([1;1]);
|
||||
|
||||
% prior on x1
|
||||
I=eye(2);
|
||||
f1 = GaussianFactor('x1', 10*I, [-1;-1], unit2);
|
||||
fg.push_back(f1);
|
||||
|
||||
% odometry between x1 and x2
|
||||
f2 = GaussianFactor('x1', -10*I, 'x2', 10*I, [2;-1], unit2);
|
||||
fg.push_back(f2);
|
||||
|
||||
% measurement between x1 and l1
|
||||
f3 = GaussianFactor('x1', -5*I, 'l1', 5*I, [0;1], unit2);
|
||||
fg.push_back(f3);
|
||||
|
||||
% measurement between x2 and l1
|
||||
f4 = GaussianFactor('x2', -5*I, 'l1', 5*I, [-1;1.5], unit2);
|
||||
fg.push_back(f4);
|
||||
|
||||
end
|
||||
|
|
@ -1,10 +0,0 @@
|
|||
% A noise configuration
|
||||
function c = createNoisyConfig()
|
||||
v_x1 = [0.1; 0.1];
|
||||
v_x2 = [1.4; 0.2];
|
||||
v_l1 = [0.1; -1.1];
|
||||
c = VectorConfig();
|
||||
c.insert('x1', v_x1);
|
||||
c.insert('x2', v_x2);
|
||||
c.insert('l1', v_l1);
|
||||
end
|
||||
|
|
@ -1,10 +0,0 @@
|
|||
% A zero delta configuration
|
||||
function c = createZeroDelta()
|
||||
v_x1 = [0; 0];
|
||||
v_x2 = [0; 0];
|
||||
v_l1 = [0; 0];
|
||||
c = VectorValues();
|
||||
c.insert('x1', v_x1);
|
||||
c.insert('x2', v_x2);
|
||||
c.insert('l1', v_l1);
|
||||
end
|
||||
|
|
@ -1,4 +0,0 @@
|
|||
% run all matlab unit tests
|
||||
testGaussianFactor
|
||||
testConditionalGaussian
|
||||
testGaussianFactorGraph
|
||||
|
|
@ -1,70 +0,0 @@
|
|||
%-----------------------------------------------------------------------
|
||||
% eliminate
|
||||
|
||||
% the combined linear factor
|
||||
Ax2 = [
|
||||
-5., 0.
|
||||
+0.,-5.
|
||||
10., 0.
|
||||
+0.,10.
|
||||
];
|
||||
|
||||
Al1 = [
|
||||
5., 0.
|
||||
0., 5.
|
||||
0., 0.
|
||||
0., 0.
|
||||
];
|
||||
|
||||
Ax1 = [
|
||||
0.00, 0. % f4
|
||||
0.00, 0. % f4
|
||||
-10., 0. % f2
|
||||
0.00,-10. % f2
|
||||
];
|
||||
|
||||
% the RHS
|
||||
b2=[-1;1.5;2;-1];
|
||||
model4 = SharedDiagonal([1;1;1;1]);
|
||||
combined = GaussianFactor('x2', Ax2, 'l1', Al1, 'x1', Ax1, b2, model4);
|
||||
|
||||
% eliminate the combined factor
|
||||
[actualCG,actualLF] = combined.eliminate('x2');
|
||||
|
||||
% create expected Conditional Gaussian
|
||||
R11 = [
|
||||
11.1803, 0.00
|
||||
0.00, 11.1803
|
||||
];
|
||||
S12 = [
|
||||
-2.23607, 0.00
|
||||
+0.00,-2.23607
|
||||
];
|
||||
S13 = [
|
||||
-8.94427, 0.00
|
||||
+0.00,-8.94427
|
||||
];
|
||||
d=[2.23607;-1.56525];
|
||||
expectedCG = GaussianConditional('x2',d,R11,'l1',S12,'x1',S13,[1;1]);
|
||||
|
||||
% the expected linear factor
|
||||
Bl1 = [
|
||||
4.47214, 0.00
|
||||
0.00, 4.47214
|
||||
];
|
||||
|
||||
Bx1 = [
|
||||
% x1
|
||||
-4.47214, 0.00
|
||||
+0.00, -4.47214
|
||||
];
|
||||
|
||||
% the RHS
|
||||
b1= [0.0;0.894427];
|
||||
|
||||
model2 = SharedDiagonal([1;1]);
|
||||
expectedLF = GaussianFactor('l1', Bl1, 'x1', Bx1, b1, model2);
|
||||
|
||||
% check if the result matches
|
||||
CHECK('actualCG.equals(expectedCG,1e-5)',actualCG.equals(expectedCG,1e-4));
|
||||
CHECK('actualLF.equals(expectedLF,1e-5)',actualLF.equals(expectedLF,1e-4));
|
||||
|
|
@ -1,62 +0,0 @@
|
|||
%-----------------------------------------------------------------------
|
||||
% equals
|
||||
fg = createGaussianFactorGraph();
|
||||
fg2 = createGaussianFactorGraph();
|
||||
CHECK('equals',fg.equals(fg2,1e-9));
|
||||
|
||||
%-----------------------------------------------------------------------
|
||||
% error
|
||||
zero = createZeroDelta();
|
||||
actual = fg.error(zero);
|
||||
DOUBLES_EQUAL( 5.625, actual, 1e-9 );
|
||||
|
||||
%-----------------------------------------------------------------------
|
||||
% eliminate_x1
|
||||
fg = createGaussianFactorGraph();
|
||||
actual = fg.eliminateOne('x1');
|
||||
|
||||
%-----------------------------------------------------------------------
|
||||
% eliminate_x2
|
||||
fg = createGaussianFactorGraph();
|
||||
actual = fg.eliminateOne('x2');
|
||||
|
||||
%-----------------------------------------------------------------------
|
||||
% eliminateAll
|
||||
I = eye(2);
|
||||
cg1 = GaussianConditional('x1',[-1;-1], 10*I,[1;1]);
|
||||
|
||||
sig1=0.149071;
|
||||
d2=[0; .2]/sig1;
|
||||
cg2 = GaussianConditional('l1', d2, I/sig1, 'x1', -I/sig1, [1;1]);
|
||||
|
||||
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);
|
||||
expected.push_back(cg2);
|
||||
expected.push_back(cg1);
|
||||
|
||||
% Check one ordering
|
||||
fg1 = createGaussianFactorGraph();
|
||||
ord1 = Ordering;
|
||||
ord1.push_back('x2');
|
||||
ord1.push_back('l1');
|
||||
ord1.push_back('x1');
|
||||
actual1 = fg1.eliminate_(ord1);
|
||||
CHECK('eliminateAll', actual1.equals(expected,1e-5));
|
||||
|
||||
%-----------------------------------------------------------------------
|
||||
% matrix
|
||||
|
||||
fg = createGaussianFactorGraph();
|
||||
ord = Ordering;
|
||||
ord.push_back('x1');
|
||||
ord.push_back('x2');
|
||||
ord.push_back('l1');
|
||||
|
||||
[H,z] = fg.matrix(ord);
|
||||
|
||||
|
|
@ -1,26 +0,0 @@
|
|||
% test linearize Pose2Factor
|
||||
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');
|
||||
|
||||
|
||||
% test linearize Pose2Graph
|
||||
|
||||
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() ;
|
||||
config.insert('x1',p1);
|
||||
config.insert('x2',p2);
|
||||
|
||||
lf = factor.linearize(config);
|
||||
lf.print('lf ');
|
||||
|
||||
factors = Pose2Graph;
|
||||
factors.push_back(factor);
|
||||
|
||||
lfg=factors.linearize_(config);
|
||||
lfg.print('lfg');
|
||||
|
|
@ -1,76 +0,0 @@
|
|||
% Set up a small SLAM example in MATLAB to test the execution time
|
||||
|
||||
clear;
|
||||
clf;
|
||||
%Parameters
|
||||
noRuns=100;
|
||||
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
|
||||
gaussianFactorGraph = create_gaussian_factor_graph(config, measurements, odometry, measurement_sigma, odo_sigma, n);
|
||||
%
|
||||
% create an ordering
|
||||
ord = create_ordering(n,m);
|
||||
|
||||
ijs = gaussianFactorGraph.sparse(ord);
|
||||
A=sparse(ijs(1,:),ijs(2,:),ijs(3,:));
|
||||
|
||||
|
||||
runs=50; % for each graph run QR and elimination several times and average the time
|
||||
|
||||
ck_qr=cputime;
|
||||
for it=1:runs
|
||||
R_qr = qr(A);
|
||||
end
|
||||
time_qr=[time_qr,(cputime-ck_qr)/runs];
|
||||
|
||||
% eliminate with that ordering
|
||||
%time gt_sam
|
||||
for it=1:runs+1
|
||||
if it==2
|
||||
ck_gt=cputime;
|
||||
end
|
||||
BayesNet = gaussianFactorGraph.eliminate_(ord);
|
||||
end
|
||||
time_gtsam=[time_gtsam,(cputime-ck_gt)/runs];
|
||||
|
||||
clear trajectory visibility gaussianFactorGraph measurements odometry;
|
||||
m = m+5;
|
||||
velocity=velocity+1;
|
||||
steps=steps+1;
|
||||
end
|
||||
plot(time_qr,'r');hold on;
|
||||
plot(time_gtsam,'b');
|
||||
|
||||
|
|
@ -1,112 +0,0 @@
|
|||
% Set up a small SLAM example in MATLAB to test the execution time
|
||||
|
||||
clear;
|
||||
|
||||
%Parameters
|
||||
|
||||
noRuns=5;
|
||||
steps=50;
|
||||
m = 5*steps;
|
||||
velocity=1*steps;
|
||||
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,2);
|
||||
% Check visibility and plot this on the problem figure
|
||||
visibilityTh=visibilityTh+steps;
|
||||
visibility = create_visibility(map, trajectory,visibilityTh);
|
||||
gplot(visibility,[map trajectory]');
|
||||
|
||||
|
||||
% 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
|
||||
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] = gaussianFactorGraph.matrix(ord);
|
||||
%A=sparse(A_dense);
|
||||
|
||||
%sparse matrix !!!
|
||||
ijs = gaussianFactorGraph.sparse(ord);
|
||||
A=sparse(ijs(1,:),ijs(2,:),ijs(3,:));
|
||||
%spy(A);
|
||||
%time qr
|
||||
runs=1;
|
||||
|
||||
ck_qr=cputime;
|
||||
for i=1:runs
|
||||
R_qr = qr(A);
|
||||
end
|
||||
time_qr=(cputime-ck_qr)/runs
|
||||
%time_qr=[time_qr,(cputime-ck)];
|
||||
|
||||
%figure(2)
|
||||
%clf
|
||||
%spy(R_qr);
|
||||
|
||||
% eliminate with that ordering
|
||||
%time gt_sam
|
||||
% for i=1:runs+10
|
||||
% if i==11
|
||||
% ck_gt=cputime;
|
||||
% end
|
||||
% BayesNet = gaussianFactorGraph.eliminate(ord);
|
||||
% end
|
||||
ck_gt=cputime;
|
||||
for i=1:runs+10
|
||||
BayesNet = gaussianFactorGraph.eliminate_(ord);
|
||||
end
|
||||
time_gtsam=(cputime-ck_gt)/runs
|
||||
%time_gtsam=[time_gtsam,(cputime-ck)];
|
||||
|
||||
% clear trajectory visibility gaussianFactorGraph measurements odometry;
|
||||
% m = m+5;
|
||||
% velocity=velocity+1;
|
||||
|
||||
% end
|
||||
% %time_qr=time_qr/noRuns
|
||||
% 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;
|
||||
|
|
@ -1,23 +0,0 @@
|
|||
dataSet = 'intel';
|
||||
saveFiles = 0;
|
||||
maxID = 0;
|
||||
dim = 3;
|
||||
|
||||
[vertices,edges]=loadDataSet(dataSet,saveFiles);
|
||||
[vert, ed]=cut2maxID(maxID, vertices, edges);
|
||||
|
||||
p0 = vert(1,2:end)';
|
||||
s0 = diag([ed(1,6),ed(1,8),ed(1,9)]);
|
||||
|
||||
fprintf('Initial config...');
|
||||
config=InitialConfig(ed,dim,p0);
|
||||
|
||||
fprintf('graph2FactorGraph...');
|
||||
[A,b]=graph2FactorGraph(config,ed,dim,s0);
|
||||
|
||||
tic
|
||||
for trial=1:100
|
||||
fprintf('Trial %d\n', trial);
|
||||
x = spqr_solve(A,b);
|
||||
end
|
||||
toc
|
||||
Loading…
Reference in New Issue