-
Notifications
You must be signed in to change notification settings - Fork 326
/
perform_dijkstra_fm.m
119 lines (99 loc) · 2.98 KB
/
perform_dijkstra_fm.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
function [D,Dsvg,Ssvg] = perform_dijstra_fm(W, pstart, options)
% perform_fm_dijstra - *slow* (matlab) implementation of Dijstra and FM
%
% [D,Dsvg,Ssvg] = perform_fm_dijstra(W, pstart, options);
%
% W is an (n,n) metric matrix.
% pstart is a (2,k) starting points.
% options.method is either 'fm' or 'dijstra'
%
% D is the final distance map to pstart
% options.svg_rate gives the rate at wich Dsvg and Ssvg is filled.
% options.niter can be used to limit the total number of steps (partial propagation).
%
% Copyright (c) 2012 Gabriel Peyre
method = getoptions(options, 'method', 'fm');
svg_rate = getoptions(options, 'svg_rate', 10);
niter = getoptions(options, 'niter', Inf);
bound = getoptions(options, 'bound', 'sym');
%%
% Size.
n = size(W,1);
%%
% The four displacement vector to go to the four neightbors.
neigh = [[1;0] [-1;0] [0;1] [0;-1]];
%%
% For simplicity of implementation, we use periodic boundary conditions.
switch bound
case 'per'
boundary = @(x)mod(x-1,n)+1;
case 'sym'
boundary = @(x)x.*(x<=n & x>0) + (2-x).*(x<=0) + (2*n-x).*(x>n);
otherwise
error('Works only for per and sym.');
end
%%
% For a given grid index |k|, and a given neighboring index k in \({1,2,3,4}\),
% |Neigh(k,i)| gives the corresponding grid neigboring index.
ind2sub1 = @(k)[rem(k-1, n)+1; (k - rem(k-1, n) - 1)/n + 1];
sub2ind1 = @(u)(u(2)-1)*n+u(1);
Neigh = @(k,i)sub2ind1( boundary(ind2sub1(k)+neigh(:,i)) );
%%
% Stack of starting points.
for i=1:size(pstart,2)
I(i) = sub2ind1(pstart(:,i));
end
%%
% Initialize the distance to \(+\infty\), excepted for the boundary conditions.
D = zeros(n)+Inf; % current distance
D(I) = 0;
%%
% Initialize the state to 0 (unexplored), excepted for the boundary point to \(1\)
% (front).
S = zeros(n);
S(I) = 1; % open
%%
% Run!
iter = 0;
Dsvg = []; Ssvg = [];
while not(isempty(I)) && iter<=niter
iter = iter+1;
% pop from stack
[tmp,j] = sort(D(I)); j = j(1);
i = I(j); I(j) = [];
% declare dead
S(i) = -1;
% The list of the four neighbors
J = [Neigh(i,1); Neigh(i,2); Neigh(i,3); Neigh(i,4)];
% Remove those that are dead
J(S(J)==-1) = [];
% Add them to the front
J1 = J(S(J)==0);
I = [I; J1];
S(J1) = 1;
% update neighbor values
for j=J'
dx = min( D([Neigh(j,1) Neigh(j,2)]) );
dy = min( D([Neigh(j,3) Neigh(j,4)]) );
switch method
case 'dijkstra'
% Dijkstra update
D(j) = min(dx+W(j), dy+W(j));
case 'fm'
% FM update
Delta = 2*W(j) - (dx-dy)^2;
if Delta>=0
D(j) = (dx+dy+sqrt(Delta))/2;
else
D(j) = min(dx+W(j), dy+W(j));
end
otherwise
error('Only dijstra and fm are allowed.');
end
end
% svd
if mod(iter,svg_rate)==1 && nargout>2
Dsvg(:,:,end+1) = D;
Ssvg(:,:,end+1) = S;
end
end