-
Notifications
You must be signed in to change notification settings - Fork 0
/
BruteMatch.m
62 lines (48 loc) · 1.83 KB
/
BruteMatch.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
function [pose, bestHits] = BruteMatch(gridmap, scan, pose,...
searchWindow)
% Grid map information
logLookupTable = gridmap.logLookupTable;
minX = gridmap.origin(1);
minY = gridmap.origin(2);
nCols = size(logLookupTable, 2);
nRows = size(logLookupTable, 1);
step = gridmap.resolution;
% Search space
xs = (pose(1) - searchWindow(1)) : step : (pose(1) + searchWindow(1));
ys = (pose(2) - searchWindow(2)) : step : (pose(2) + searchWindow(2));
rs = (pose(3) - searchWindow(3)) : deg2rad(1) : (pose(3) + searchWindow(3));
nx = length(xs); ny = length(ys); nr = length(rs);
% Searching
scores = Inf(nx, ny, nr);
bestScore = Inf;
t1 = tic;
% Rotation
for ir = 1 : nr
theta = rs(ir);
R_gc = theta2R(theta); % rotation from current scan to global frame
GcellScan = R_gc * scan;
% Translation along x-axis
for ix = 1 : nx
tx = xs(ix);
ScanIDx = round((GcellScan(1,:)+tx-minX)/ gridmap.resolution) + 1;
% Translate along y-axis
for iy = 1 : ny
ty = ys(iy);
ScanIDy = round((GcellScan(2,:)+ty-minY)/ gridmap.resolution) + 1;
% Metric score
isIn = ScanIDx>1 & ScanIDy>1 & ScanIDx<nCols & ScanIDy<nRows;
% idx = ScanIDy(isIn) + (ScanIDx(isIn)-1)*nRows;
idx = nRows - ScanIDy(isIn) + 1 + (ScanIDx(isIn)-1)*nRows;
hits = logLookupTable(idx);
score = sum(hits);
scores(ix, iy, ir) = score;
if score < bestScore
bestScore = score;
bestHits = hits;
pose = [tx; ty; theta];
end
end
end
end
t2 = toc(t1);
disp(['Brute Force runtime: ', num2str(t2),'s.']);