悉尼大学SLAM实验数据MATLAB版

悉尼大学SLAM实验数据MATLAB版。

function x=detectTrees（RR） 
% laser range  RR is a [1x361] matrix in meters!!!!.
% return x a 3xn matrix where
% x（1i） distance to i-landmark center
% x（2i） angle of i-landmark center （in radians）
% x（3i） i-landmark diameter
% Jos? ACFR. 1999.



% --------------------------------
M11 = 75 ;
M10 =  1 ;
daa = 5*pi/306 ;
M2=1.5 ; 	
M2a = 10*pi/360 ;
M3=3 ;		% *************** 
M5=1 ; % 1 mt   % *************** 
daMin2 = 2*pi/360 ;
% --------------------------------
global AAr; AA = AAr ;
ii1=Find16X（RRL1 = length（ii1） ; 
if L1<1 x=[] ; return ; end ;

R1 = RR（ii1） ; A1 = AA（ii1） ;

ii2 = find（  （abs（diff（R1））>M2）|（ diff（A1）>M2a） ） ;

L2= length（ii2）+1 ;
%ii2 = [1 ii2+1 ii1（L1） ] ;
ii2u = int16（[ ii2 L1 ]） ;
ii2  = int16（[1 ii2+1 ]） ;

%ii2  size（R1） 

R2  = R1（ii2 ） ; A2  = A1（ii2 ） ; 
A2u = A1（ii2u） ; R2u = R1（ii2u） ;

x2  =  R2.*cos（A2 ） ; y2 =  R2.*sin（A2 ） ;
x2u = R2u.*cos（A2u） ;y2u = R2u.*sin（A2u） ;

flag=zeros（1L2） ;

L3=0 ;M3c= M3*M3 ;
if L2>1 
 L2m=L2-1 ;
 dx2 = x2（2:L2）-x2u（1:L2m） ;  dy2 = y2（2:L2）-y2u（1:L2m） ; 
 %dl2 = sqrt（ dx2.*dx2+dy2.*dy2 ） ;	%dont use SQRT
 dl2 = （ dx2.*dx2+dy2.*dy2 ） ;
 ii3 = find（dl2 if L3>0 flag（ii3）=1 ; flag（ii3+1）=1 ; end ;

if L2>2 
 L2m=L2-2 ;
 dx2 = x2（3:L2）-x2u（1:L2m） ;  dy2 = y2（3:L2）-y2u（1:L2m） ; 
 dl2 = （ dx2.*dx2+dy2.*dy2 ） ;
 ii3 = find（dl2 if L3b>0 flag（ii3）=1 ; flag（ii3+2）=1 ; L3=L3+L3b ; end ;

if L2>3 % OJOOOOOOOOO
 L2m=L2-3 ;
 dx2 = x2（4:L2）-x2u（1:L2m） ;  dy2 = y2（4:L2）-y2u（1:L2m） ; 
 dl2 = （ dx2.*dx2+dy2.*dy2 ） ;
 ii3 = find（dl2 if L3b>0 flag（ii3）=1 ; flag（ii3+3）=1 ;L3=L3+L3b ; end ;

end ;end ;end ;

if L2>1 
 ii3 = [1:L2-1] ;
 ii3 = find（ （A2（ii3+1）-A2u（ii3））jects close （in angle） from viewpoint.
 L3b=length（ii3） ;
 if L3b>0
    ff = （R2（ii3+1）>R2u（ii3）） ;		% which object is in the back?
	 ii3=ii3+ff	;
	 flag（ii3）=1	;		% mark them for the deletion
	 L3=L3+L3b	;
 end ;
 iixx=ii3 ;
end ;

if L3>0
	ii3=Find16X（flag==0） ;
   L3=length（ii3） ;
   ii4 = double（ii2（ii3）） ; ii4u = double（ii2u（ii3）） ;
	R4  = R2（ii3）  ; R4u  = R2u（ii3）  ;
	A4  = A2（ii3）  ; A4u  = A2u（ii3）  ;
	x4  = x2（ii3）  ; y4   = y2（ii3）   ;
	x4u = x2u（ii3） ; y4u  = y2u（ii3）  ;
else
   ii4 = double（ii2） ; ii4u = double（ii2u）;
	R4  = R2  ; R4u  = R2u ;
	A4  = A2  ; A4u  = A2u  ;
	x4  = x2  ; y4   = y2   ;
	x4u = x2u ; y4u  = y2u  ;
end ;

%dA2=diff（A2） ; dD2 = dA2.*R2（1:L2-1）;
dx2 = x4-x4u ;  dy2 = y4-y4u ; 
dl2 = （ dx2.*dx2+dy2.*dy2 ） ;
ii5 = Find16X（dl2<（M5*M5）） ; L5 = length（ii5） ; if L5<1 x=[] ; return ; end ;

R5 = R4（ii5） ; R5u = R4u（ii5） ;	A5 = A4（ii5） ; A5u = A4u（ii5） ; ii4=ii4（ii5） ;ii4u=ii4u（ii5） ;

ii5=Find16X（ （R5>M10）&（A5>daa）&（A5u<（pi-daa）） ） ;
L5 = length（ii5） ; if L5<1 x=[] ; return ; end ;
R5 = R5（ii5） ; R5u = R5u（ii5） ;	A5 = A5（ii5） ; A5u = A5u（ii5） ; ii4=ii4（ii5） ;ii4u=ii4u（ii5） ;
dL5 = （A5u+pi

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        3666  2001-04-29 15:10  detectTreesI16.m
     文件     5263032  2000-11-01 07:12  aa3_lsr2.mat
     文件     1239216  2001-04-29 15:05  aa3_dr.mat
     文件        1624  2001-04-29 15:56  info.txt
     文件        1746  2001-04-29 15:09  viewLsr.m
     文件       89624  2001-04-29 15:24  aa3_gpsx.mat
     文件      391834  2001-04-29 15:53  car.bmp
     文件       25088  2001-04-29 15:51  car.vsd