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