Skip to content

Commit 11119c5

Browse files
petercorkepetercorke
committed
better logic for choosing fallback functions
add IPT bwdistgeodesic extend coverage for bug2 and distancexform
1 parent bc94b52 commit 11119c5

File tree

2 files changed

+121
-62
lines changed

2 files changed

+121
-62
lines changed

‎distancexform.m‎

Lines changed: 65 additions & 62 deletions
Original file line numberDiff line numberDiff line change
@@ -40,23 +40,23 @@
4040
% Copyright (C) 1993-2017, by Peter I. Corke
4141
%
4242
% This file is part of The Robotics Toolbox for MATLAB (RTB).
43-
%
43+
%
4444
% RTB is free software: you can redistribute it and/or modify
4545
% it under the terms of the GNU Lesser General Public License as published by
4646
% the Free Software Foundation, either version 3 of the License, or
4747
% (at your option) any later version.
48-
%
48+
%
4949
% RTB is distributed in the hope that it will be useful,
5050
% but WITHOUT ANY WARRANTY; without even the implied warranty of
5151
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
5252
% GNU Lesser General Public License for more details.
53-
%
53+
%
5454
% You should have received a copy of the GNU Leser General Public License
5555
% along with RTB. If not, see <http://www.gnu.org/licenses/>.
5656
%
5757
% http://www.petercorke.com
5858

59-
function d = distancexform(occgrid, varargin)
59+
function dx = distancexform(occgrid, varargin)
6060

6161
opt.show = 0;
6262
opt.ipt = true;
@@ -68,6 +68,20 @@
6868
opt.ipt = false;
6969
opt.vlfeat = false;
7070
end
71+
count = [];
72+
switch opt.metric
73+
case 'cityblock'
74+
ipt_metric = opt.metric; % if we use bwdistgeodesic
75+
m = [ inf 1 inf
76+
1 0 1
77+
inf 1 inf ];
78+
case 'euclidean'
79+
ipt_metric = 'quasi-euclidean'; % if we use bwdistgeodesic
80+
r2 = sqrt(2);
81+
m = [ r2 1 r2
82+
1 0 1
83+
r2 1 r2 ];
84+
end
7185

7286
if ~isempty(args) && isvec(args{1}, 2)
7387
%% path planning interpretation
@@ -76,30 +90,13 @@
7690
goal = args{1};
7791
occgrid = double(occgrid);
7892

79-
if exist('imorph', 'file') ~= 3
80-
error('Machine Vision Toolbox is required by this function');
81-
end
82-
83-
switch opt.metric
84-
case 'cityblock'
85-
m = [ inf 1 inf
86-
1 0 1
87-
inf 1 inf ];
88-
case 'euclidean'
89-
r2 = sqrt(2);
90-
m = [ r2 1 r2
91-
1 0 1
92-
r2 1 r2 ];
93-
otherwise
94-
error('unknown distance metric');
95-
end
96-
9793
% check the goal point is sane
98-
if occgrid(goal(2), goal(1)) > 0
99-
error('goal inside obstacle')
100-
end
94+
assert(occgrid(goal(2), goal(1)) == 0, 'RTB:distancexform:badarg', 'goal inside obstacle')
10195

102-
if opt.fast && exist('imorph', 'file') == 3
96+
if exist('imorph', 'file') && opt.fast
97+
if opt.verbose
98+
fprintf('using MVTB:imorph\n');
99+
end
103100
% setup to use imorph
104101
% - set obstacles to NaN
105102
% - set free space to Inf
@@ -132,7 +129,20 @@
132129
end
133130
ninf = ninfnow;
134131
end
132+
dx = occgrid;
133+
134+
elseif exist('bwdistgeodesic', 'file') && opt.ipt
135+
if opt.verbose
136+
fprintf('using IPT:bwdistgeodesic\n');
137+
end
138+
% solve using IPT
139+
140+
dx = double( bwdistgeodesic(occgrid==0, goal(1), goal(2), ipt_metric) );
141+
135142
else
143+
if opt.verbose
144+
fprintf('using MATLAB code, faster if you install MVTB\n');
145+
end
136146
% setup to use M-function
137147

138148
occgrid(occgrid>0) = NaN;
@@ -146,7 +156,7 @@
146156

147157
occgrid = dxstep(occgrid, m);
148158
occgrid(nans) = NaN;
149-
159+
150160
count = count+1;
151161
if opt.show
152162
cmap = [1 0 0; gray(count)];
@@ -166,39 +176,20 @@
166176
end
167177
ninf = ninfnow;
168178
end
179+
dx = occgrid;
169180
end
170181

171-
if opt.show
182+
if opt.show && ~isempty(count)
172183
fprintf('%d iterations, %d unreachable cells\n', count, ninf);
173184
end
174185

175-
d = occgrid;
176186
else
177187
%% image processing interpretation
178188
% distancexform(world, [metric])
179189

180-
% use other toolboxes if they exist
181-
if opt.fast && exist('bwdist') && opt.ipt
182-
d = bwdist(occgrid, opt.metric);
183-
184-
elseif exist('vl_imdisttf') == 3 && opt.vlfeat
185-
im = double(occgrid);
186-
im(im==0) = inf;
187-
im(im==1) = 0;
188-
d2 = vl_imdisttf(im);
189-
d = sqrt(d2);
190-
191-
elseif opt.fast && exist('imorph', 'file') == 3
192-
193-
switch opt.metric
194-
case 'cityblock'
195-
m = ones(3,3);
196-
m(2,2) = 0;
197-
case 'euclidean'
198-
r2 = sqrt(2);
199-
m = [r2 1 r2; 1 0 1; r2 1 r2];
200-
otherwise
201-
error('unknown distance metric');
190+
if exist('imorph', 'file') && opt.fast
191+
if opt.verbose
192+
fprintf('using MVTB:imorph\n');
202193
end
203194

204195
% setup to use imorph
@@ -228,17 +219,27 @@
228219
break;
229220
end
230221
end
231-
d = occgrid;
222+
dx = occgrid;
223+
elseif exist('bwdist') && opt.ipt
224+
if opt.verbose
225+
fprintf('using IPT:bwdist\n');
226+
end
227+
% use IPT
228+
dx = bwdist(occgrid, ipt_metric);
229+
230+
elseif exist('vl_imdisttf') && opt.vlfeat
231+
if opt.verbose
232+
fprintf('using VLFEAT:vl_imsdisttf\n');
233+
end
234+
im = double(occgrid);
235+
im(im==0) = inf;
236+
im(im==1) = 0;
237+
d2 = vl_imdisttf(im);
238+
dx = sqrt(d2);
239+
232240
else
233-
switch opt.metric
234-
case 'cityblock'
235-
m = ones(3,3);
236-
m(2,2) = 0;
237-
case 'euclidean'
238-
r2 = sqrt(2);
239-
m = [r2 1 r2; 1 0 1; r2 1 r2];
240-
otherwise
241-
error('unknown distance metric');
241+
if opt.verbose
242+
fprintf('using MATLAB code, faster if you install MVTB\n');
242243
end
243244

244245
occgrid = double(occgrid);
@@ -265,11 +266,13 @@
265266
break;
266267
end
267268
end
268-
d = occgrid;
269+
dx = occgrid;
269270
end
270271
end
271272
end
272273

274+
% MATLAB implementation of computational kernel
275+
273276
function out = dxstep(G, m)
274277

275278
[h,w] = size(G); % get size of occ grid

‎unit_test/PlanTest.m‎

Lines changed: 56 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,21 @@ function bug2_test(tc)
3232

3333
nav.plot()
3434
nav.plot(p);
35+
36+
fname = fullfile(tempdir, 'bug.mp4');
37+
p = nav.query(tc.TestData.start, tc.TestData.goal, 'animate', 'movie', fname);
38+
tc.verifyTrue(exist(fname, 'file') == 2);
39+
delete(fname);
40+
41+
tc.verifyError( @() nav.plan(), 'RTB:Bug2:badcall');
42+
43+
map = zeros(10,10);
44+
map(3:7,3:7) = 1;
45+
map(4:6,4:6) = 0;
46+
nav = Bug2(map);
47+
tc.verifyError( @() nav.query([5 5], [2 2]), 'RTB:bug2:noplan');
48+
49+
3550
end
3651

3752

@@ -67,6 +82,47 @@ function dxform_test(tc)
6782
nav.plot3d(p);
6883
end
6984

85+
function distancexform_test(tc)
86+
map = zeros(10,10);
87+
map(4:6,4:6) =1;
88+
%args= {'verbose'}
89+
args = {};
90+
91+
dx1 = distancexform(map, [5 8], 'fast', 'noipt', args{:});
92+
tc.verifyClass(dx1, 'double');
93+
tc.verifyEqual(dx1(8,5), 0);
94+
tc.verifyTrue(all(all(isnan(dx1(map==1)))));
95+
i=sub2ind(size(dx1), 8, 5);
96+
tc.verifyTrue(all(dx1(i+[-11 -10 -9 -1 1 11 10 9])) > 0);
97+
98+
tc.verifySize(dx1, size(map));
99+
dx2 = distancexform(map, [5 8], 'nofast', 'ipt', args{:});
100+
tc.verifyClass(dx1, 'double');
101+
tc.verifySize(dx1, size(map));
102+
dx3 = distancexform(map, [5 8], 'nofast', 'noipt', args{:});
103+
tc.verifyClass(dx1, 'double');
104+
tc.verifySize(dx1, size(map));
105+
106+
tc.verifyEqual(dx1, dx2, 'absTol', 1e-6, 'MEX ~= bwdist');
107+
tc.verifyEqual(dx1, dx3, 'MEX ~= MATLAB');
108+
109+
dx1 = distancexform(map, [5 8], 'cityblock', 'fast', 'noipt', args{:});
110+
tc.verifyClass(dx1, 'double');
111+
tc.verifyTrue(all(all(isnan(dx1(map==1)))));
112+
i=sub2ind(size(dx1), 8, 5);
113+
tc.verifyTrue(all(dx1(i+[-11 -10 -9 -1 1 11 10 9])) > 0);
114+
tc.verifySize(dx1, size(map));
115+
dx2 = distancexform(map, [5 8], 'cityblock', 'nofast', 'ipt', args{:});
116+
tc.verifyClass(dx1, 'double');
117+
tc.verifySize(dx1, size(map));
118+
dx3 = distancexform(map, [5 8], 'cityblock', 'nofast', 'noipt', args{:});
119+
tc.verifyClass(dx1, 'double');
120+
tc.verifySize(dx1, size(map));
121+
122+
tc.verifyEqual(dx1, dx2, 'absTol', 1e-6, 'MEX ~= bwdist');
123+
tc.verifyEqual(dx1, dx3, 'MEX ~= MATLAB');
124+
end
125+
70126
function dstar_test(tc)
71127

72128
% create a planner

0 commit comments

Comments
 (0)