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
| function costsscanline = so(costs, mins, maxs, step, stepy, i1, inidisp, idxso)
tmpcosts = inf(maxs - mins + 1, size(i1, 1), size(i1, 2));
tic
if stepy >= 0
yst = 1;
endy = size(i1, 1);
else
endy = 1;
yst = size(i1, 1);
end
if stepy == 0
qsy = 1;
else
qsy = stepy;
end
for y=yst:qsy:endy
if step >= 0
xst = 1;
endx = size(i1, 2);
else
endx = 1;
xst = size(i1, 2);
end
if step == 0
qsx = 1;
else
qsx = step;
end
for x=xst:qsx:endx
ite = 1;
for itedis=mins:1:maxs
if x == 1 || x == size(i1, 2) || y == 1 || y == size(i1, 1)
tmpcosts(ite, y, x) = costs(ite, y, x);
else
%myval = costs(ite, y, x);
ite2 = 1;
minn = inf;
theta1 = 80;
theta2 = 120;
%for itedis2=mins:1:maxs
%itedis2 = itedis + 1;
% if diff1 == inf && abs(itedis2 - itedis) == 1
diff1 = max(abs(i1(min(max(y-stepy, 1), size(i1, 1)), min(max(x - step, 1), size(i1, 2)), 1)...
- i1(y, x, 1)), ...
abs(i1(min(max(y-stepy, 1), size(i1, 1)), min(max(x - step, 1), size(i1, 2)), 2) - i1(y, x, 2)));
diff1 = max(diff1, abs(i1(min(max(y-stepy, 1), size(i1, 1)), min(max(x - step, 1), size(i1, 2)), 3) - i1(y, x, 3)));
%end
%itedis2 = itedis + 2;
%if diff2 == inf && abs(itedis2 - itedis) > 1
diff2 = max(abs(i1(min(max(y-stepy, 1), size(i1, 1)), min(max(x - step + itedis, 1), size(i1, 2)), 1) - i1(y, min(max(x + itedis, 1), size(i1, 2)), 1)), ...
abs(i1(min(max(y-stepy, 1), size(i1, 1)), min(max(x - step + itedis, 1), size(i1, 2)), 2) - i1(y, min(max(x + itedis, 1), size(i1, 2)), 2)));
diff2 = max(diff2, abs(i1(min(max(y-stepy, 1), size(i1, 1)), min(max(x - step + itedis, 1), size(i1, 2)), 3) - i1(y, min(max(x + itedis, 1), size(i1, 2)), 3)));
%end
%end
thresh = 5;
%disp(diff2);
if diff1 <= thresh && diff2 <= thresh
p1 = theta1;
p2 = theta2;
end
if diff1 <= thresh && diff2 >= thresh
p1 = theta1 / 4;
p2 = theta2 / 4;
end
if diff1 >= thresh && diff2 <= thresh
p1 = theta1 / 4;
p2 = theta2 / 4;
end
if diff1 >= thresh && diff2 >= thresh
p1 = theta1 / 10;
p2 = theta2 / 10;
end
minwpen = 0;
for itedis2=mins:1:maxs
pen = 0;
if abs(itedis2 - itedis) == 1
pen = p1;
end
if abs(itedis2 - itedis) > 1
pen = p2;
end
if tmpcosts(ite2, y - stepy, x - step) + pen < minn
minn = tmpcosts(ite2, y - stepy, x - step) + pen;
%minwpen = tmpcosts(ite2, y, x - 1);
end
if tmpcosts(ite2, y - stepy, x - step) < minwpen
minwpen = tmpcosts(ite2, y - stepy, x - step);
end
ite2 = ite2 + 1;
end
myval = costs(ite, y, x) + minn - minwpen;
tmpcosts(ite, y, x) = myval;
end
ite = ite + 1;
end
end
end
costsscanline = tmpcosts;
dur = toc;
disp(dur);
end |
Partager