2D Laser Ray Tracing for the Simulation of Laser Perception
Zhen Song
Center for Self-Organizing and Intelligent Systems (CSOIS)
Department of Electrical and Computer Engineering
College of Engineering, Utah State University
4160 Old Main Hill, Logan Utah 84322-4160, USA
E-mail: [email protected] Web: http://www.csois.usu.edu/
Date: First draft on 10/27/2001. Revised on 12/27/2004
Technical Report No. : USU-CSOIS-TR-04-11
Reports are available from http://mechatronics.ece.usu.edu/reports/
2D Laser Ray Tracing for the Simulation of Laser
Perception
Zhen Song
I. I NTRODUCTION
The 2D laser scanner is an important sensor for T4 robot [1]. The 2D laser data should be carefully analyzed to identify
basic objects in parking lots and help to calibrate the odometry system by estimating the positions of landmarks. Basically,
the processing can be decomposed into segmentation, which would separate objects, and object fittings, which would find
the precise position of the objects. For some cases, the above two modules could be combined in one algorithm. Since a
cure all algorithm is difficult and unnecessary, the task would be done by a set of algorithms.
This article describe the implementation of the algorithms, including the details of simulation, plane and corner fit, and
proposed segmentation method. Part of the work was included in [1], [2] and [3].
II. S IMULATION
The aim of simulation is to compare different methods analytically. So far, the 2D laser scanner can not provide 3D
image data, simulation is mandatary. Even if these data are available later, simulation would still be useful for algorithm
testing for the extreme cases by quantity approach. All the simulations are done in Matlab.
Each object is represent as many triangles, since it is easy to check if a triangle has a internal intersection with a light
ray.
To calculate the intersection, we have the light ray
x = lt + x0
y = mt + y0
z = nt + z0
Vector [l, m, n] is the direction of the ray from 2D laser scanner to targets. Based on the coordination system in figure 2,
[l, m, n] = [cos(β) sin(α), sin(β), − cos(α) cos(β)] Parameter t is proportional to the distance from the laser scanner to
certain point in the ray. In simulation, the program find set {ti } corresponding to intersection with the ray and each object,
then The minimum t0 = min({ti }) is the solution, since other points are behind this one and can not be “seen” by 2D
laser scanner.
The formula of a plane is Ax + By + Cz + D = 0. Thus the intersection point is the one on the ray with
Ax0 + By0 + Cz0 + D
t0 = −
Al + Bm + Cn
This method is described in [4]
Similarly, for a pole described by (x − x0 )2 + (y − y0 )2 = r2 , the intersection is [t0 cos(β) sin(α), t0 sin(β), h −
t0 cos(α) cos(β)], where t0 satisfies the constraint (t0 sin(α) cos(β) − x0 )2 + (t0 sin(α) sin(β) − y0 )2 = r2 , or t20 sin(α)2 +
f t0 (−2 sin(α)
cos(β)x0 − 2 sin(α) sin(β)y0 ) + x20 + y02 − r2 = 0. This fits the format at20 + bt0 + c = 0, and the minimum
√
−b− b2 −4ac
t0 =
, which corresponds to the nearest intersection.
2a
The objects we are interest so far can be represented as combination of many small triangles and poles. For a pole,
as far as b2 − 4ac ≥ 0, the point associated with t0 is guaranteed to lie on the pole. For the case of triangles, we need
to check if the intersection point is inside the triangle and one the plane. For non singular case (none of A, B, C in the
plane function could be 0), a method of linear algebra [4] works. Unfortunately, the plane is singular in our simulation.
Here a new method is proposed.
See figure 1, for case (1) and (2), the point is on the same plane with the triangle. If the point is inside the triangle
of case (1), 6 AP B + 6 BP C + 6 CP A = 2π. If the point is out of the triangle, without losing generality, assume P
is near to segment AB as case (2). Since 6 AP B inside 4AP B, 6 AP B < π. And 6 AP B = 6 BP C + 6 CP A, so
6 AP B + 6 BP C + 6 CP A < 2π. For case (3) and (4), the point is not on the same plane with 4ABC. The projection of
P on plane ABC is P 0 . Since 6 AP B < 6 AP 0 B, 6 BP C < 6 BP 0 C and 6 CP A < 6 CP 0 A, and 6 AP 0 B + 6 BP 0 C +
6 CP 0 A ≤ 2π, so 6 AP B + 6 BP C + 6 CP A < 2π. We can conclude
6
6
AP B + 6 BP C + 6 CP A = 2π
AP B + 6 BP C + 6 CP A < 2π
the point P is inside a triangle 4ABC and on the same plane.
otherwise.
Figure 2 is the coordination system of the laser scanner. Easy to proof {x, y, z} = {d cos(β) sin(α), d sin(β), h −
d cos(α) cos(β)}. Figure 3 is the side walk model can be represented in triangles only and figure 10 is the output.
Figure ?? is simulation for a lamp pole and ground. The Matlab code for side walk model is the function SimuLaser.m
and Intri.m , and the code for lamp pole model is the function SimuLaserPole.m
SimuLaser.m
function SimuLaser
world=zeros(4,3,7);
sq2=sqrt(2);
hi=0.2;
el=1; %element length
dis=1; %distance to origin
world(:,:,1)=[dis 0 0; dis+el -el 0; dis+2*el 0 0; dis+el el 0]; % bottom
world(:,:,2)=[dis 0 hi; dis+el -el hi; dis+2*el 0 hi; dis+el el hi]; % upper
world(:,:,3)=[dis,0,0; dis+el,-el,0; dis+el,-el,hi; dis,0,hi]; %front right
world(:,:,4)=[dis,0,0; dis,0,hi; dis+el,el,hi; dis+el,el,0]; %front left
world(:,:,5)=[dis+el,-el,0; dis+2*el,0,0; dis+2*el,0,hi; dis+el,-el,hi]; %behind right
world(:,:,6)=[dis+2*el,0,0; dis+el,el,0; dis+el,el,hi; dis+2*el,0,hi]; %behind left
world(:,:,7)=[-1e6,-1e6,0; 1e6,-1e6,0; 1e6,1e6,0; -1e6,1e6,0]; %ground
h=1.5; %the height of laser
MyCamera=[0 0 h];
planes=zeros(7,4); %plane parameters of the world
for cnt1=1:7
planes(cnt1,:)=[det([world(1:3,2,cnt1), world(1:3,3,cnt1), ones(3,1)]), ...
-det([world(1:3,1,cnt1), world(1:3,3,cnt1), ones(3,1)]), ...
det([world(1:3,1,cnt1), world(1:3,2,cnt1), ones(3,1)]), ...
-det([world(1:3,1,cnt1), world(1:3,2,cnt1), world(1:3,3,cnt1)])
];
%find parameters of planes
end
dat=[]; %laser data
d2r=pi/180; %degree to rad
for alpha=5*d2r:1*d2r:72*d2r %row
line=[]; %one line of laser
for beta=-40*d2r:1*d2r:40*d2r %col
cb=cos(beta);
sb=sin(beta);
ca=cos(alpha);
sa=sin(alpha);
tmin=10;
for cnt1=1:7 %each face of the world
t=-dot(planes(cnt1,:), [0, 0, h, 1])/(dot(planes(cnt1,:) , ...
[cb*sa, sb, -ca*cb, 0] ) ); %intersection point
pt=[0 0 h]+t*[cb*sa, sb, -ca*cb];
tri1=world(1:3,:,cnt1);
tri2=[world(3:4,:,cnt1); world(1,:,cnt1)];
if intri(pt,tri1) | intri(pt,tri2) %if on the face
if t<tmin & t>0
tmin=t;
end
end
end
IntPt = [cb*sa, sb , -ca*cb].*tmin +[0, 0, h];
dat=[dat; IntPt, alpha, beta]; %dat: [x,y,z,alpha,beta; x, y,...]
end %beta
end %alpha
save dat;
2
Intri.m
function ans=Intri(pt,tri)
%INTRI test if the 3D point is inside (or on) the triangle
%ANS=INTRI(PT,TRI)
% TRI=[ax, ay, az; bx, by, bz; cx, cy, cz];
% PT=[x, y, z]
% If inside the triangle, return ANS as 1; Otherwise ANS is 0.
%
% Zhen Song, Aug 20
%
[vnum, three]=size(tri);
if three˜=3 | vnum ˜=3
disp(’Tri must be 3 by 3 matrix’);
ans=0;
return;
end
vets=[];
for cnt1=1:vnum
vets=[vets; tri(cnt1,:)-pt];
end
ang=[];
for cnt1=1:vnum-1
if norm(vets(cnt1,:))*norm(vets(cnt1+1,:))˜=0
ang(cnt1)=acos( dot(vets(cnt1,:), vets(cnt1+1,:))/(norm(vets(cnt1,:))*norm(vets(cnt1+1,:)))
else
ang(cnt1)=0;
end
end
if norm(vets(1,:))*norm(vets(vnum,:))˜=0
ang(vnum)=acos( dot(vets(vnum,:), vets(1,:))/(norm(vets(vnum,:))*norm(vets(1,:))) );
else
ang(vnum)=0;
end
if abs(sum(ang)-2*pi)<1e-6
ans=1;
return;
else
ans=0;
return;
end
SimulaserPole.m
function SimuLaserPole
world=zeros(4,3,7);
sq2=sqrt(2);
hi=0.2;
el=1; %element length
dis=1; %distance to origin
x0=1;
y0=0;
r0=0.1;
world(:,:,7)=[-1e6,-1e6,0;
1e6,-1e6,0;
1e6,1e6,0;
h=1.5; %the height of laser
MyCamera=[0 0 h];
3
-1e6,1e6,0]; %ground
planes=zeros(7,4); %plane parameters of the world
planes(1,:)=[det([world(1:3,2,7), world(1:3,3,7), ones(3,1)]), ...
-det([world(1:3,1,7), world(1:3,3,7), ones(3,1)]), ...
det([world(1:3,1,7), world(1:3,2,7), ones(3,1)]), ...
-det([world(1:3,1,7), world(1:3,2,7), world(1:3,3,7)])
]; %find parameters of planes
dat=[]; %laser data
d2r=pi/180; %degree to rad
for alpha=5*d2r:1*d2r:72*d2r %row 68
line=[]; %one line of laser
for beta=-40*d2r:1*d2r:40*d2r %col 81
cb=cos(beta);
sb=sin(beta);
ca=cos(alpha);
sa=sin(alpha);
tmin=10;
%t of ground
tg=-dot(planes(1,:), [0, 0, h, 1])/(dot(planes(1,:) , [cb*sa, sb, -ca*cb, 0] ) ); %intersect
%intersection with the pole
%tˆ2*saˆ2+t(-2*sa*cb*x0-2*sa*sb*y0)+x0ˆ2+y0ˆ2-r0=0
a=saˆ2;
b=-2*sa*cb*x0-2*sa*sb*y0;
c=x0ˆ2+y0ˆ2-r0;
if bˆ2-4*a*c >=0
tc=(-b-sqrt(bˆ2-4*a*c))/(2*a);
else
tc=inf;
end
if tg<tc
IntPt = [cb*sa, sb , -ca*cb].*tg +[0, 0, h];
else
IntPt = [sa*cb, sa*sb, -ca].*tc +[0, 0, h];
end
dat=[dat; IntPt, alpha, beta]; %dat: [x,y,z,alpha,beta; x, y,...]
end %beta
end %alpha
save dat;
III. P LANE AND C ORNER F ITTING
The goal of the plane fit problem is to find the A, B, C, D, such that


x1 y1 z1 1
   
A
e1
 x2 y2 z2 1 


B   e2 
 x3 y3 z3 1  × 
 = 
 .
C
e3
..
..
.. 

 ..
.
.
.
D
e4
xn yn zn 1
let e21 + e22 + e23 + e24 to be the minimum. [U, S, V]=SVD(A); V(:,4) are the solution of [A B C D]. See Matlab program
pfit.m for details.
The corner fit problem is to find the best corner {x, y, z}, such that




D1
A1 B1 C1
 
 
 D2 
 A2 B2 C2 
e1
x




 A3 B3 C3  ×  y  +  D3  =  e2 

 .


..
.. 
 ... 
 ..
e3
z
.
.
Dn
An Bn Cn
or A × P = −D, where, e21 + e22 + e23 is the minimum. Based on the projection theorem, the solution is
P = (AT A)−1 AT (−D)
4
Fig. 1.
Fig. 2.
Check If A Point Is Inside A Triangle
2D Laser Scanner
Fig. 3.
5
Objects in Simulation
See Matlab program cfit.m for details.
The Matlab code are:
pfit.m
function [para, meanerr,ptsout,varargout] = pfit(pts,thr,varargin)
%PFIT Plane fitting
%[PARA, MEANERR]=PFIT(PTS);
%PTS points for plane fitting
%PARA parameters of the plane
%MEANERR mean error of the fitting
%
%
% Zhen Song, Aug 23, 2001
%
%function [para, meanerr,varargout] = pfit(pts,thr,varargin)
no_of_inputs = nargin;
[row col]=size(pts);
if col˜=3 | row < 4
disp(’input points must be n by 3 matrix, where n>=4’);
return;
end
[U, S, V]=svd([pts, ones(row,1)],0);
para=V(:,4)’;
if (no_of_inputs==1)
TotalErr=sum( abs( [pts, ones(row,1)] * para’), 1);
meanerr=TotalErr/row;
return;
else %2 inputs
err=[pts,ones(row,1)]*para’;
ind=find(abs(err)<thr);
pts=pts(ind,:); %if ‘thr’ is set, pick out wild values and fit again
[U, S, V]=svd([pts, ones(row,1)],0);
para=V(:,4)’;
TotalErr=sum( abs( [pts, ones(row,1)] * para’), 1);
meanerr=TotalErr/row;
ptsout=pts;
return;
end
cfit.m
function [corner, meanerr]=cfit(para)
%CFIT fit corner
%[CORNER, MEANERR]=CFIT(PARA)
%PARA is the parameters of planes for fitting, with the format [A1 B1 C1 D1; A2 B2 C2 D2]
%CORNER is the cordination of fit [x y z];
%MEANERR is the mean error for fitting. If it is too large, the corner is invalid.
%Zhen Song Aug 24, 2001
%
[row col]=size(para);
if row < 3 | col˜=4
disp(’para must be n by 4 matrix, where n is no less than 3’);
corner=[];
meanerr=[];
return;
6
end
% ThrErr=1e-10; %threashold for error
corner=[];
meanerr=[];
% if row==3 | row==4
A=para(:,1:3);
b=para(:,4);
if rank(A’*A)==3
corner=inv(A’ * A) *A’ *(-b) ; %LMS
corner=corner’;
meanerr=sum(abs(para*[corner, 1]’))/row;
return;
else
corner=[];
meanerr=[];
return;
end
return;
IV. C ORNER D ETECTION
Corner detection simulates the case that T4 uses laser scanner to detect a corner of sidewalk, which is a landmark
with known position. Use the data generated by SimuLaser.m, this program should find the nearest corner of the box
correctly, and must be robust. Here we use the box in the simulation data as the sidewalk.
The first algorithm segment data only by discriminate the hight. Figure 4, 5, 6, 7 are illustrate this algorithm.
With a simple segmentation, the program cornerdet.m does find the corner. The upper layer and the ground are separate
only the value of z, i.e. if the z value of a point is larger than certain threshold, it is believed to be on the upper layer.
Another threshold determine if the point is on the ground. Other points are separate by whether they are on the left or
right hand side of the nearest point. Figure 7 is the result.
This method is limited due to its hight based segmentation. If the ground has large slope that parts of it is even higher
than the side walk, it can’t segment correctly. How to segment correctly, or develop a corner detection algorithm that does
not need segmentation is the main challenge of the problem.
The second algorithm found the corner by the help of 2D edge detection. Figure 12 illustrate the procedure.
Firstly, we project the 3D data on 2D plane. And use “Canny” edge detector to find the outlines. Here we assume
there is only one corner which is the nearest edge point. Secondly, calculating the distance of each point, we can find the
nearest point. Transfer this point back to the 3D data, we will get the corner. Figure 13 takes a close look on the fitted
corner. We can see there is some error.
2
2
The third algorithm proposed to fit corner by the function z = ek1 (x−x0 ) +k2 (y−y0 ) +k3 . This algorithm is still under
developing, so I’ll discuss it later. Figure 19,20 illustrate the main idea.
For lamp pole fitting, I combine the edge detection and 2D circle fitting. Figure 14 is the simulation data. First, map
3D data to 2D image, like figure15. Second, use canny detector find the edge, see figure 16. Third, transfer back to 3D,
get figure 17. Four, eliminate necessary columns of data, get figure 18. Five, project it again to 2D and use circle fitting
algorithm to find the radius and origin of the lamp pole.
cornerdet.m
function [found, corner]=cornerdet(dat,col)
%corner detection for 2D laser data
%incoming data in dat, with the format of [x y z alpha beta]
%
tic
[rownum colnum]=size(dat);
if colnum˜=3 | rownum<4
disp(’dat must be n by 3 matrix, where n is greater than 3’);
found =[];
7
0.2
0.2
0.15
z
z
0.15
0.1
0.1
0.05
0.05
0
2
Fig. 4.
0
4
1
0
y
−1
2
x
−2
2
Select points between two threshold of z as edges
Fig. 5.
1
0
y
−1
2
x
−2
Separate left and right edge by the nearest point
0.2
0.2
0.15
0.15
0.1
0.1
z
z
4
0.05
0.05
4
0
0
4
3
3
2
2
1
2
0
−1
−2
1
x
2
1
y
0
−1
−2
y
Fig. 6. Three planes (left edge, right edge, upper layer) are selected for
corner fitting
8
Fig. 7.
The fitted corner (red circle)
1
x
Fig. 8.
Transfer 3D data to 2D image by function ToImg.m
Fig. 9.
2D Edge detection by Canny edge detection
2D Laser Simulation Data
0.3
0.25
0.2
z
0.15
0.1
0.05
0
−0.05
3
2
5
1
4
0
3
−1
2
−2
1
−3
y
Fig. 10.
0
x
2D Laser Simulation Data
Fig. 11.
Canny Edge Detection
corner=[];
return;
end
found=0;
corner=[];
row=rownum/col;
clear rownum, colnum;
ThrGround=[-0.01 0.01];
ThrCurb=[0.19 0.21];
ThrFit=1e-4;
IndGround=find(dat(:,3)> ThrGround(1) & dat(:,3)<ThrGround(2) );
IndCurb=find(dat(:,3) > ThrCurb(1) & dat(:,3)<ThrCurb(2) );
IndWall=find(dat(:,3) > ThrGround(2) & dat(:,3) < ThrCurb(1) );
[ParaGround,ErrGround]=pfit(dat(IndGround,:));
[ParaCurb, ErrCurb] =pfit(dat(IndCurb,:));
PtWall=dat(IndWall,:);
Norm=PtWall(:,1).ˆ2+PtWall(:,2).ˆ2+PtWall(:,3).ˆ2;
IndCorner=find(Norm == min(Norm) );
IndOneSide=find( atan(PtWall(:,2)./PtWall(:,1)) > atan( PtWall(IndCorner,2)/PtWall(IndCorner,1) ) );
IndOtherSide=find( atan(PtWall(:,2)./PtWall(:,1)) < atan( PtWall(IndCorner,2)/PtWall(IndCorner,1) )
9
Corner Detection by 2D Edge Detection
3D Data
Detected Corner
Fig. 12.
Detected Corner by The Help of Canny 2D Edge Detection
PtOne=PtWall(IndOneSide,:);
[ParaOneSide,ErrOneSide]=pfit(PtOne);
PtOther=PtWall(IndOtherSide,:);
[ParaOtherSide, ErrOtherSide]=pfit(PtOther);
para=[ParaOneSide; ParaOtherSide; ParaCurb];
[corner, meanerr]=cfit(para);
if meanerr<1e-4
found=1;
end
toc
return;
ToImg.m
function img=ToImg(dat, row ,col)
%row=41;
%col=61;
img=zeros(row,col,1);
pt=1;
10
Corner Detection by 2D Edge Detection
3D Data
Detected Corner
Fig. 13.
Closer View on The Detected Corner
Simulation Data for Pole Fitting
4.5
4
3.5
1
0.5
0
3
2.5
2
4
3
1.5
2
1
1
0
−1
0.5
−2
−3
−4
Fig. 14.
Simulation Data for Pole Fit
Fig. 15.
11
Transfer the 3D Data Into Image
Pole Fitting: After Edge Detection
1.4
1.2
1
0.8
0.6
0.4
0.2
0
0.4
0.9
0.2
0.85
0
0.8
0.75
−0.2
0.7
−0.4
Fig. 16.
Detect The Edge of The Pole Image
Fig. 17.
0.65
Transfer The Pole Edge Image Back to The 3D Data
Pole Fitting: Eliminate Stacking Points
A Piece Of Data For Corner Fitting
0.3
0.2
0.2
0.18
0.16
0.1
0.14
0.12
0
1
0.3
0.2
0.9
0.1
0.08
1.1
0.1
0.8
0
0.06
0.04
0.7
1.05
−0.1
Fig. 18.
0.05
0.6
−0.2
y
0.5
0
x
−0.05
Eliminate Two “Stacks” of The 3D Data
Fig. 19.
12
1
A Piece of Data For Corner Fit
Project a Corner to Fitted Plane
A Piece Of Data For Corner Fitting
0.2
0.08
0.06
0.18
0.16
0.04
0.14
0.02
0.12
0.1
0
−0.09
−0.02
−0.092
0.08
1.1
0.06
−0.094
−0.04
0.04
−0.096
−0.06
0.015
Fig. 20.
0.02
0.025
1.05
0.05
0
−0.08
−0.05
Project The Corner Data to The Fitted Plane
Fig. 21.
1
A Piece of Data For Corner Fit
TheMax=max(dat(:,3));
TheMin=min(dat(:,3));
for cnt1=1:row
for cnt2=1:col
img(cnt1,cnt2)=(dat(pt,3)-TheMin)*(1/(TheMax-TheMin));
pt=pt+1;
end
end
TheMax
TheMin
return;
The last two items (in figure 4,5,6,7,8,9) shows how to detect 2D edge by transfer the 3D data on 2D plane. There
are already many corner detection algorithms available, e.g. Canny, Laplace. etc. All of them are based 2D image data,
instead of 3D laser data. Some methods, like Canny, use 3 by 3 masks to detect edge in binary image. We might be able
to expand the ability of these 2D methods by same idea of mask and plane or corner fitting.
R EFERENCES
[1] Z. Song, K. Moore, Y. Chen, and V. Bahla, “Two-dimensional laser servoing for precision motion control of an odv robotic license plate recognition
system,” in in Proc. of SPIE Conf. on Aerospace/Defense Sensing, Simulation, and Controls, 2003.
[2] Z. Song, Y. Chen, L. Ma, and Y. Chung, “Some sensing and perception techniques for an omnidirectional ground vehicle with a laser scanner,” in
in Proc. of IEEE International Symposium on Intelligent Control (ISIC), 2003.
[3] Z. Song, “Some localization strategies for mobile robots,” Master’s thesis, Utah State University, 2003.
[4] P. Shirley, Realistic Ray Tracing. A K Peters, 2000.
13