![]() |
DetectLoopsPURPOSE
Propose loops based on the distance between poses searching linearly.
SYNOPSIS
function N=DetectLoops(F,matchArea,pdRange)
DESCRIPTION
Propose loops based on the distance between poses searching linearly. One of the procedures to detect the possible loop closure points from the filter information. Parameters F: The filter from where to get the information. matchArea: Area around the current robot's pose where to search for neighbouring poses. This is a pose displacement expressed in the frame of the current robot's pose. This is 'v_r' in the paper. pdRange: Interval [p1,p2]. If a pose has probabiliby higher than p1 of being in the matchArea, it is consired a neighbouring pose for the current robot's pose. If the probability is above p2, the it is considered a very close neighbouring pose. This is 's' in the paper but here we have a range for this parameter and not a single value. Outputs: N: A cell array with the neighbours where each element is a struct with two field step: The identifier of the neighbour pose pd: The neighbouring probability (always larger than pdRange(1) See also NoLoops, AllLoops, TreeDetectLoops. CROSS-REFERENCE INFORMATION
This function calls:
SOURCE CODE
0001 function N=DetectLoops(F,matchArea,pdRange) 0002 % Propose loops based on the distance between poses searching linearly. 0003 % 0004 % One of the procedures to detect the possible loop closure 0005 % points from the filter information. 0006 % 0007 % Parameters 0008 % F: The filter from where to get the information. 0009 % matchArea: Area around the current robot's pose where to search for 0010 % neighbouring poses. This is a pose displacement expressed 0011 % in the frame of the current robot's pose. 0012 % This is 'v_r' in the paper. 0013 % pdRange: Interval [p1,p2]. If a pose has probabiliby higher than p1 0014 % of being in the matchArea, it is consired a neighbouring 0015 % pose for the current robot's pose. 0016 % If the probability is above p2, the it is considered a very 0017 % close neighbouring pose. 0018 % This is 's' in the paper but here we have a range for this 0019 % parameter and not a single value. 0020 % 0021 % Outputs: 0022 % N: A cell array with the neighbours where each element is a struct 0023 % with two field 0024 % step: The identifier of the neighbour pose 0025 % pd: The neighbouring probability (always larger than pdRange(1) 0026 % 0027 % See also NoLoops, AllLoops, TreeDetectLoops. 0028 0029 % default outputs 0030 N={}; 0031 nn=0; %neighbours so far 0032 for i=1:F.nStates-1 0033 step=F.State2Step(i); 0034 pd=ProbDistance2PoseBelowThreshold(F,step,matchArea,@GetRelativeDistance2Pose); 0035 if (pd>pdRange(1)) 0036 nn=nn+1; 0037 N{nn}.step=step; 0038 N{nn}.pd=pd; 0039 end 0040 end 0041 |