Institut de Robòtica i Informàtica Industrial

ProbDistance2PoseDataAboveThreshold

PURPOSE ^

p=ProbDistance2PoseDataAboveThreshold(PD,F,t,pt)

SYNOPSIS ^

function [sLow sUp]=ProbDistance2PoseDataAboveThreshold(PD,F,t,pt)

DESCRIPTION ^

 p=ProbDistance2PoseDataAboveThreshold(PD,F,t,pt)

 Computes the probability of the relative displacement between the 
 current robot's pose and a given PoseData, PD, to be lower than 't'.

 Note that the threshold 't' has one entry for each dimension of the
 pose space (this is v_r in the paper).

 This function perform a short-circuit evaluation of the conditions 
 for all the dimensions of the relative displacement. 

 The probability is estimated as a range [sLow sUp].
 The evaluation is performed while sUp is above the given
 threshold 'pt': as soon sUp is below 'pt' for any dimension, we know for
 sure that the tested pose is too far (at least in that dimension) from
 the current pose and it does not need to be used for sensor registration.
 If after all the evaluations sLow is above 'st' all the poses in the
 PoseData object are neighbours of the current pose (nor further
 processing is needed). If sUp is below 'pt' none of the poses represented
 in PoseData are neighbours. If 'pt' is inside the range [sLow,sUp], the
 elements in PoseData need to be further refined.

 For the special case of orientations, intervals in PoseData larger than 
 2*pi are directly produce [sLow sUp]=[0,1] meaning that they have to be
 further refined.

 Note that for PoseData objets including a single pose sLow=sUp.

 This function implements equations in section VI-A in the paper. It
 is hard to follow because interval expressions are 
 re-ordered to make them more efficient and to avoid overestimation 
 (a problem typical of interval arithmtics).

 See also ProbDistance2PoseBelowThreshold.

CROSS-REFERENCE INFORMATION ^

This function calls:
  • get Get function for robots.
  • get Get function for trajectories.
  • get Get function for EKF filters.
  • GetPoseCovariance Marginal covariance of a given pose.
  • GetPoseMean Mean estimation of a given pose.
  • 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.
  • GetPoseCovariance Covariance estimation of a given pose.
  • GetPoseMean Mean estimation of a given pose.
  • get Get function for TRO filters.
  • get Get function for BTrees.
  • get Get function for PoseData objects.
  • get Get function for Gaussians.
  • pi2pi Moves an angle to the -pi,pi range.
  • Diameter Diameter of the intervals in an interval matrix.
  • Intersection Intersection between interval matrices.
  • Interval Interval constructor
  • Lower Lower limit of an interval matrix.
  • Offset Offsets an interval matrix.
  • Scale Scales an interval matrix.
  • Upper Upper limit of an interval matrix.
  • diag Extracts the diagonal of an interval matrix.
  • pi2pi Forces an angular interval to be in [-pi,pi].
  • sqrt Element-wise square root of an interval matrix (operator: .^0.5).
  • get Get function for poses.
  • get Generic get for relative positioning sensors
  • get Generic get for sensors
This function is called by:

SOURCE CODE ^

0001 function [sLow sUp]=ProbDistance2PoseDataAboveThreshold(PD,F,t,pt)
0002 % p=ProbDistance2PoseDataAboveThreshold(PD,F,t,pt)
0003 %
0004 % Computes the probability of the relative displacement between the
0005 % current robot's pose and a given PoseData, PD, to be lower than 't'.
0006 %
0007 % Note that the threshold 't' has one entry for each dimension of the
0008 % pose space (this is v_r in the paper).
0009 %
0010 % This function perform a short-circuit evaluation of the conditions
0011 % for all the dimensions of the relative displacement.
0012 %
0013 % The probability is estimated as a range [sLow sUp].
0014 % The evaluation is performed while sUp is above the given
0015 % threshold 'pt': as soon sUp is below 'pt' for any dimension, we know for
0016 % sure that the tested pose is too far (at least in that dimension) from
0017 % the current pose and it does not need to be used for sensor registration.
0018 % If after all the evaluations sLow is above 'st' all the poses in the
0019 % PoseData object are neighbours of the current pose (nor further
0020 % processing is needed). If sUp is below 'pt' none of the poses represented
0021 % in PoseData are neighbours. If 'pt' is inside the range [sLow,sUp], the
0022 % elements in PoseData need to be further refined.
0023 %
0024 % For the special case of orientations, intervals in PoseData larger than
0025 % 2*pi are directly produce [sLow sUp]=[0,1] meaning that they have to be
0026 % further refined.
0027 %
0028 % Note that for PoseData objets including a single pose sLow=sUp.
0029 %
0030 % This function implements equations in section VI-A in the paper. It
0031 % is hard to follow because interval expressions are
0032 % re-ordered to make them more efficient and to avoid overestimation
0033 % (a problem typical of interval arithmtics).
0034 %
0035 % See also ProbDistance2PoseBelowThreshold.
0036 
0037   if ~PD.isInterval
0038     sUp=ProbDistance2PoseBelowThreshold(F,PD.id,t,@GetRelativeDistance2Pose);
0039     sLow=sUp;
0040   else
0041     dim=get(F,'dim');
0042  
0043     nSteps=get(F,'nSteps');
0044     PoseN=GetPoseMean(F,nSteps);
0045     
0046     o1=PoseN(3);
0047     c1=cos(o1);
0048     s1=sin(o1);
0049     
0050     R=[[ c1  s1 0];
0051        [-s1  c1 0];
0052        [  0   0 1]];
0053      
0054     dx=(PD.m(1)-PoseN(1));
0055     dy=(PD.m(2)-PoseN(2));
0056 
0057     dx2=dx.^2;
0058     dy2=dy.^2;
0059     dxdy=dx.*dy;
0060 
0061     S11=GetPoseCovariance(F,nSteps);
0062 
0063     FF=get(F,'F');
0064     Fa=FF(3,1);
0065     Fb=FF(3,2);
0066 
0067     T1=zeros(dim);
0068     T1(1,3)=s1;
0069     T1(2,3)=c1;
0070 
0071     T2=zeros(dim);
0072     T2(1,3)=-c1;
0073     T2(2,3)= s1;
0074 
0075     A1=diag(T1*S11*T1');
0076     A2=diag(T2*S11*T2');
0077     A3=2*diag(T1*S11*T2');
0078     A4=2*diag(R*S11*T1');
0079     A5=2*diag(R*S11*T2');
0080     A6=diag(R*S11*R');
0081 
0082     s1c1=s1*c1;
0083     s12=s1^2;
0084     c12=c1^2;
0085     
0086     positive=Interval(1e-100,inf);
0087     
0088     PD_CS_1_3=PD.CS(1,3);
0089     PD_CS_2_3=PD.CS(2,3);
0090     ct1=Scale(PD_CS_1_3,s1c1);
0091     B1_1=-ct1+Scale(PD_CS_2_3,-s12);
0092     
0093     ct2=Scale(PD_CS_2_3,s1c1);
0094     B2_1= ct2+Scale(PD_CS_1_3,c12);
0095     
0096     PD_CS_1_2=PD.CS(1,2);
0097     PD_CS_2_1=PD.CS(2,1);
0098     PD_CS_1_1=PD.CS(1,1);
0099     PD_CS_2_2=PD.CS(2,2);
0100     ct3=Scale(PD_CS_1_2+PD_CS_2_1,s1c1);
0101     B3_1=Scale(PD_CS_1_1,c12)+Scale(PD_CS_1_3, Fb*s1c1+Fa*c12)+ct3+Scale(PD_CS_2_2,s12)+Scale(PD_CS_2_3,Fb*s12+Fa*s1c1);
0102 
0103     PD_S_1_1=PD.S(1,1);
0104     PD_S_2_2=PD.S(2,2);
0105     ct4=Scale(PD.S(1,2),2*s1c1);
0106     C1_1=Scale(PD_S_1_1,c12)+ct4+Scale(PD_S_2_2,s12);
0107     
0108     ct5=Offset(Scale(B3_1,-2)+C1_1,A6(1));
0109     ct6=Offset(Scale(B1_1,2),A4(1));
0110     ct7=Offset(Scale(B2_1,2),A5(1));
0111     S1=Scale(dx2,A1(1))+Scale(dy2,A2(1))+Scale(dxdy,A3(1))+ct6.*dx+ct7.*dy+ct5;
0112     Sx=Intersection(S1,positive);
0113 
0114     m_1=Scale(dx, c1)+Scale(dy,s1);
0115 
0116     rtU=( t(1)-m_1)/sqrt(2*Sx);
0117     rtL=(-t(1)-m_1)/sqrt(2*Sx);
0118     sUp =min(0.5*(erf(Upper(rtU))-erf(Lower(rtL))),1);
0119     sLow=max(0.5*(erf(Lower(rtU))-erf(Upper(rtL))),0);
0120     
0121     if (sUp>pt)
0122       B1_2= ct1+Scale(PD_CS_2_3,-c12);
0123 
0124       B2_2=-ct2+Scale(PD_CS_1_3,s12);
0125 
0126       B3_2=Scale(PD_CS_1_1,s12)+Scale(PD_CS_1_3,-(Fb*s1c1-Fa*s12))-ct3+Scale(PD_CS_2_2,c12)+Scale(PD_CS_2_3,Fb*c12-Fa*s1c1);
0127 
0128       C1_2=Scale(PD_S_1_1,s12)-ct4+Scale(PD_S_2_2,c12);
0129 
0130       ct8=Offset(Scale(B3_2,-2)+C1_2,A6(2));
0131       ct9=Offset(Scale(B1_2,2),A4(2));
0132       ct10=Offset(Scale(B2_2,2),A5(2));
0133       S2=Scale(dx2,A1(2))+Scale(dy2,A2(2))+Scale(dxdy,A3(2))+ct9.*dx+ct10.*dy+ct8;
0134       Sy=Intersection(S2,positive);
0135  
0136       m_2=Scale(dx,-s1)+Scale(dy,c1);
0137 
0138       rtU=( t(2)-m_2)/sqrt(2*Sy);
0139       rtL=(-t(2)-m_2)/sqrt(2*Sy);
0140       sUp =min(min(0.5*(erf(Upper(rtU))-erf(Lower(rtL))),1),sUp);
0141       sLow=min(max(0.5*(erf(Lower(rtU))-erf(Upper(rtL))),0),sLow);
0142       
0143       if (sUp>pt)
0144         if Diameter(PD.m(3))<=2*pi
0145           S3=Offset(Scale(dx2,A1(3))+Scale(dy2,A2(3))+Scale(dxdy,A3(3))+Scale(dx,A4(3))+Scale(dy,A5(3))+Scale(PD.CS(3,3),-2)+PD.S(3,3),A6(3));
0146           So=Intersection(S3,positive);
0147   
0148           do=pi2pi(PD.m(3)-PoseN(3));
0149           
0150           rtU=( t(3)-do)/sqrt(2*So);
0151           rtL=(-t(3)-do)/sqrt(2*So);
0152           sUp =min(min(0.5*(erf(Upper(rtU))-erf(Lower(rtL))),1),sUp);
0153           sLow=min(max(0.5*(erf(Lower(rtU))-erf(Upper(rtL))),0),sLow);
0154         else
0155           sUp=1;
0156           sLow=0;
0157         end
0158       end
0159     end
0160   end
0161


Institut de Robòtica i Informàtica Industrial

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