Institut de Robòtica i Informàtica Industrial

UpdateTree

PURPOSE ^

Updates the information in a BTree.

SYNOPSIS ^

function T=UpdateTree(T,F,pd)

DESCRIPTION ^

 Updates the information in a BTree.

 Updates the information stored in the leaves and the internal nodes of a
 BTree. 

 This function is used after loop closures, when the information for all
 poses are likely to signifiantly change and it is much faster that
 re-building the tree from scratch.

 This is basically an iterative implementation of the recursive pseudo-code
 given in the paper.

 Parameters:
   - T The BTree.
   - F The filter
   - pd The set with the PoseData (mean, covariance and cross covariance
     with the current pose) for each pose.

 See also BTree.

CROSS-REFERENCE INFORMATION ^

This function calls:
  • get Get function for robots.
  • get Get function for trajectories.
  • get Get function for EKF filters.
  • Step2State Returns the state correspoinding to a step.
  • get Generic get function for filters.
  • get Get function for TRO filters.
  • get Get function for BTrees.
  • Union Union of two PoseData objects.
  • get Get function for PoseData objects.
  • get Get function for Gaussians.
  • Union Iout=Union(d1,d2)
  • Union Hull of two interval matrices.
  • get Get function for poses.
  • get Generic get for relative positioning sensors
  • get Generic get for sensors
This function is called by:
  • Simulation Simulates a robot performing Pose SLAM.

SOURCE CODE ^

0001 function T=UpdateTree(T,F,pd)
0002 % Updates the information in a BTree.
0003 %
0004 % Updates the information stored in the leaves and the internal nodes of a
0005 % BTree.
0006 %
0007 % This function is used after loop closures, when the information for all
0008 % poses are likely to signifiantly change and it is much faster that
0009 % re-building the tree from scratch.
0010 %
0011 % This is basically an iterative implementation of the recursive pseudo-code
0012 % given in the paper.
0013 %
0014 % Parameters:
0015 %   - T The BTree.
0016 %   - F The filter
0017 %   - pd The set with the PoseData (mean, covariance and cross covariance
0018 %     with the current pose) for each pose.
0019 %
0020 % See also BTree.
0021 
0022   c=T.root;
0023   s=0;
0024   done=(c==0);
0025   while ~done
0026     if T.data{c}.heigth==1
0027       % for leafs there is no need to go to
0028       % lower levels
0029       s=2;
0030     end
0031     switch s
0032       case 0
0033         % right tree is still to be udpated -> proceed
0034         c=T.data{c}.right;
0035         s=0;
0036       case 1
0037         % left tree is still to be updated -> proceed
0038         c=T.data{c}.left;
0039         s=0;
0040       case 2
0041         % right and left trees are updated -> update the current
0042         % node and go up
0043         if T.data{c}.heigth==1
0044           T.data{c}.element=pd{Step2State(F,get(T.data{c}.element,'id'))};
0045         else
0046           l=T.data{c}.left;
0047           r=T.data{c}.right;
0048           T.data{c}.element=Union(T.data{l}.element,T.data{r}.element);
0049         end
0050 
0051         % The current sub-tree is compleated, proceed with the
0052         % upper levels.
0053         % if the current sub-tree is the right branch of the upper
0054         % level s is set to 1 to proceed with the left branch.
0055         % if the current sub-tree is the left branch, we can proceed
0056         % to update the parent node (we move to the parent node
0057         % with s=2)
0058         p=T.data{c}.parent;
0059         if p==0
0060           done=true;
0061         else
0062           if T.data{p}.right==c
0063             s=1;
0064           else
0065             % if must be that T.data{p}.left==c
0066             s=2;
0067           end
0068           c=p;
0069         end
0070     end
0071   end
0072


Institut de Robòtica i Informàtica Industrial

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