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