![]() |
TreeDetectLoopsPURPOSE
Propose loops based on the distance between poses using a tree for the search.
SYNOPSIS
function N=TreeDetectLoops(F,T,matchArea,pdRange)
DESCRIPTION
Propose loops based on the distance between poses using a tree for the search. One of the procedures to detect the possible loop closure points from the filter information. This implements the same algorithm as DetectLoops but the search for neighbouring poses is done using a tree and thus it is faster. Parameters F: The filter from where to get the information. T: The tree of poses to use in the search. 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. 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. CROSS-REFERENCE INFORMATION
This function calls:
SOURCE CODE
0001 function N=TreeDetectLoops(F,T,matchArea,pdRange) 0002 % Propose loops based on the distance between poses using a tree for the search. 0003 % 0004 % One of the procedures to detect the possible loop closure 0005 % points from the filter information. 0006 % 0007 % This implements the same algorithm as DetectLoops but the search for 0008 % neighbouring poses is done using a tree and thus it is faster. 0009 % 0010 % Parameters 0011 % F: The filter from where to get the information. 0012 % T: The tree of poses to use in the search. 0013 % matchArea: Area around the current robot's pose where to search for 0014 % neighbouring poses. This is a pose displacement expressed 0015 % in the frame of the current robot's pose. 0016 % pdRange: Interval [p1,p2]. If a pose has probabiliby higher than p1 0017 % of being in the matchArea, it is consired a neighbouring 0018 % pose for the current robot's pose. 0019 % If the probability is above p2, the it is considered a very 0020 % close neighbouring pose. 0021 % This is 's' in the paper but here we have a range for this 0022 % parameter and not a single value. 0023 0024 % Outputs: 0025 % N: A cell array with the neighbours where each element is a struct 0026 % with two field 0027 % step: The identifier of the neighbour pose 0028 % pd: The neighbouring probability (always larger than pdRange(1). 0029 % 0030 % See also NoLoops, AllLoops, DetectLoops. 0031 0032 r=get(T,'root'); 0033 if r==0 0034 N={}; 0035 else 0036 S=SearchInTree(T,F,matchArea,pdRange(1),r); 0037 0038 nn=size(S,2); % number neighbours 0039 N=cell(1,nn); 0040 for t=1:nn 0041 step=get(S{t},'id'); 0042 N{t}.step=step; 0043 N{t}.pd=ProbDistance2PoseBelowThreshold(F,step,matchArea,@GetRelativeDistance2Pose); 0044 end 0045 end 0046 |