Institut de Robòtica i Informàtica Industrial

DetectLoops

PURPOSE ^

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: This function is called by:
  • Simulation Simulates a robot performing Pose SLAM.

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


Institut de Robòtica i Informàtica Industrial

Generated on Fri 24-Jul-2009 12:32:50 by m2html © 2003