![]() |
UpdateTreePURPOSE
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:
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 |