Institut de Robòtica i Informàtica Industrial

TreeDetectLoops

PURPOSE ^

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:
  • get Get function for robots.
  • get Get function for trajectories.
  • size Number of poses in a trajectory.
  • get Get function for EKF filters.
  • GetRelativeDistance2Pose Relative displacement between the current pose and a previous one.
  • ProbDistance2PoseBelowThreshold Checks if pose 'step' is close enought to the current pose.
  • get Generic get function for filters.
  • size Size of the state estimated in the filter
  • get Get function for TRO filters.
  • SearchInTree Search nearest-neighbours using a Btree
  • get Get function for BTrees.
  • get Get function for PoseData objects.
  • get Get function for Gaussians.
  • size Size (rows/columns) of an interval matrix.
  • get Get function for poses.
  • size Number of parameters of the pose.
  • get Generic get for relative positioning sensors
  • get Generic get for sensors
  • size Number of readings stored in the Sensor.
This function is called by:
  • Simulation Simulates a robot performing Pose SLAM.

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


Institut de Robòtica i Informàtica Industrial

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