LiDAR simulator
MARV216 project 2012
Ilkka Korpela
Inclined
ground
House
Tree crown
House
LiDAR in a nutshell
LiDAR is flown across the scene
along a trajectory.
tx (X, Y, Z, , , )
tx is a time-point when LiDAR is “fired”
Scan angle 
Target position:
Range
[X,Y,Z] = [X,Y,Z]LiDAR + range ∙ [i, j, k]
==
[X,Y,Z] = p + t * dir
dir = [i,j,k] = f() ∙ g(, , )
Target (X, Y, Z)
(Mirror moves inside the LiDAR, and the
whole LiDAR rotates in 3D).
A real LiDAR can give
raise to and detect
multiple reflections per
pulse transmitted.
Divergence, contains e.g.
1/e2 of the energy
If we send just a single
ray, the divergence is zero
(not 0.3-1.0 mrad as in
real LiDARs).
We can send many per
pulse – call them sub-rays
of a pulse.
Illuminated
area
The ”photon
clump” (pulse).
The leaf (surface)
normal
A sub-ray in a pulse stretches out from LiDAR
position p in direction dir, as far as t says.
[XYZ]hit = p + t ∙ dir
How to define what was hit by the ray?
This is an intersection problem.
A ray – object intersection p.
If the objects comprise 3D
planes, solution for ray-plane
intersection exists and is rather
fast to compute.
3D planes have infinite area. We
will use 3D plane polygons.
They need a class.
For trees, we will use a
function that tells the radius
of the crown envelope, and
another function that tells
the diameter of the stem, for
a given height.
Ray-object intersection is
solved by iteration
(numerically).
Trees will be objects.
How strong reflection?
(How much backscattered
photons to expect at the LiDAR?)
a
Planar (man-made) objects have some
reflectance at the wavelegth of LiDAR (0−1).
It tells (roughly) the proportion light that
scatters.
The indidence angle (a) is important. We assume
that backscatter is reduced by cos(a)**pow. pow
and reflectance are both defined for each
surface material.
Long distance reduces power per unit area
(radiance) at the sensor to the second power.
How strong reflection?
(How much backscattered
photons to expect at the LiDAR?)
A crown has scatters that
potentially produce an echo
(measurable reflection).
Single needle – not enough.
Single leaf – maybe.
Shoot – probably.
Scatterer density [kg/m3]:
- Top-down gradient.
- Clumbs (shoots/branches)
- Foliage at outer hull
Incidence angle – meaningfull?
SKELETON
1. Read the inputs
Scene
1.1. Planar polygons (ground, walls, roof planes)
1.2. Trees
Params 1.3. Flying (Campaign) parameters
1.4. Sensor (LiDAR) parameters
2. Compute the needed trajectory for the whole duration of the flight, for each
moment the LiDAR is fired (transmitted). Let the LiDAR sway.
3. Start the flight, from the moment LiDAR operation begings (time==0). Compute
the mirror angle. Compute the dir-vector of the pulse, and actually dir-vectors of all
N sub-rays. They all start from point p, the position of the LiDAR.
4. Transmit a pulse (made of N sub-rays)
for all sub-rays:
for all planar polygons:
Check intersection
Choose the plane object with shortest distance
for all trees:
Check intersection
Choose tree object with shortest distance
Store the sub-ray result (xyz, distance, reflection strength) to list
Let time grow one unit, and goto 3, unless time is over.
5. Finally write the results (for subrays) to a file.
Wall,0.25,3
10,10,2
10,30,2
10,30,6
10,10,6
10,10,2
Wall,0.25,3
10,10,2
16,10,2
16,10,6
10,10,6
10,10,2
Wall,0.25,3
16,10,2
16,30,2
16,30,6
16,10,6
16,10,2
Wall,0.25,3
10,30,2
10,30,6
16,30,6
16,30,2
10,30,2
Roof,0.18,2
9,9,5.8
9,31,5.8
13,31,10
13,9,10
9,9,5.8
...
1.1 Planar 3D polygons - scene.csv
”name”, reflectance, power
x,y,z
x,y,z
x,y,z,
...
class PolygonSurface:
def __init__(self):
self.Vertices = []
self.refl = None
self.pow = None
self.Box = None
self.A = None
self.B = None
self.C = None
What else?
- Surface normal vector (n = [A,B,C]) is needed
when computing ray-plane intersections
- A bounding box will speed up intersection
calculation calculations
[A,B,C]
Wall,0.25,3
10,10,2
10,30,2
10,30,6
10,10,6
10,10,2
Wall,0.25,3
10,10,2
16,10,2
16,10,6
10,10,6
10,10,2
Wall,0.25,3
16,10,2
16,30,2
16,30,6
16,10,6
16,10,2
Wall,0.25,3
10,30,2
10,30,6
16,30,6
16,30,2
10,30,2
Roof,0.18,2
9,9,5.8
9,31,5.8
13,31,10
13,9,10
9,9,5.8
...
1.1 Planar 3D polygons - reading scene.csv
f = open("scene.csv","r")
Surfaces = [] # To hold the PolygonSurface objects
for line in f:
tinylist = line.split(",")
try:
float(tinylist[0]) # Error for float("Wall")
newpolyline = True
except:
newpolyline = False
if (newpolyline):
Surfaces.append(PolygonSurface())
index = len(Surfaces)-1
Surfaces[index].refl = float(tinylist[1])
Surfaces[index].pow = float(tinylist[2])
else:
x = float(tinylist[0])
y = float(tinylist[1])
z = float(tinylist[2])
Surfaces[index].Vertices.append(Point3D(x,y,z))
f.close()
1.1 Planar 3D polygons - Bounding Box
class BoundingBox:
"""mins and maxs of
def __init__(self,
self.min = mini #
self.max = maxi #
x,y,z that make a non-rotated 3D bounding box"""
mini, maxi):
Point3D object
Point3D object
Include a method in class PolygonSurface:
def SetBox(self):
"""Search for the (min,max) of x,y,z (2 x 3) and store to bounding box"""
mini = Point3D(1E200,1E200, 1E200)
maxi = Point3D(-1E200,-1E200, -1E200)
for i in self.Vertices:
mini.x = min(i.x, mini.x)
mini.y = min(i.y, mini.y)
mini.z = min(i.z, mini.z)
maxi.x = max(i.x, maxi.x)
maxi.y = max(i.y, maxi.y)
maxi.z = max(i.z, maxi.z)
# Make the boundaries of the Box expand 1 m
# in all directions (to be on the safe side)
mini.x -= 1; mini.y -= 1; mini.z -= 1
maxi.x += 1; maxi.y += 1; maxi.z += 1
# Create a BoundingBox object instance and assign to
# attribute Box
self.Box = BoundingBox(mini,maxi)
1.1 Planar 3D polygons - Surface normal vector
Include a method in class PolygonSurface:
def SolveParams(self):
# Solves surface normal vector (A,B,C) from a triangle in the surface.
# See MARV216_project.pdf. Solves linear equations.
# Assign four 3 x 3 matrices. numpy-package is required..
D = numpy.zeros([3,3]); A = numpy.zeros([3,3])
B = numpy.zeros([3,3]); C = numpy.zeros([3,3])
j = -1
for k in range(3):
i = self.Vertices[k]
[A,B,C]
# Take a Point3D in index [k], fill matrices
j += 1
D[j][0] = i.x; D[j][1] = i.y; D[j][2] = i.z
A[j][0] = 1;
A[j][1] = i.y; A[j][2] = i.z
B[j][0] = i.x; B[j][1] = 1;
B[j][2] = i.z
C[j][0] = i.x; C[j][1] = i.y; C[j][2] = 1
d = -1.0
# Compute the determinants of the matrices A-D
D = numpy.linalg.det(D); A = numpy.linalg.det(A)
B = numpy.linalg.det(B); C = numpy.linalg.det(C)
# Normalize the normal vector [A,B,C] to len(one), assign to attributes
self.A = -d/D*A / math.sqrt((-d/D*A)**2+(-d/D*B)**2+(-d/D*C)**2)
self.B = -d/D*B / math.sqrt((-d/D*A)**2+(-d/D*B)**2+(-d/D*C)**2)
self.C = -d/D*C / math.sqrt((-d/D*A)**2+(-d/D*B)**2+(-d/D*C)**2)
print self.A, self.B, self.C
1.1 Planar 3D polygons
Objects (class PolygonSurface)
Stored in list Surfaces
Attributes:
Vertices[i].x (list of Point3D's)
refl
pow
A,B,C
Box (BoundingBox object,
.mini.Point3D, .maxi.Point3D)
Methods:
Surfaces[index].setBox()
Surfaces[index].solveParams()
1.2 Trees in the scene - Trees.csv
id,x,y,z,h,d,hc,sp
1,35,30,1,19,21,6,Conifer
2,37,45,1,24,28,10,Broadleaved
...
class Tree:
# Everything needed to make a tree.
def __init__(self, num, pos, h, dbh, hc, sp):
self.num = num # Some identifier
self.pos = pos # Point3D, tree base
self.h
= h
# Height, m
self.dbh = dbh # Trunk diameter at 1.3-m height, m
self.hc = hc # Height of the base of living crown
self.sp = sp # Species "Conifer" or "Broadleaved"
1.2 Trees in the scene - reading Trees.csv
id,x,y,z,h,d,hc,sp
1,35,30,1,19,21,6,Conifer
2,37,45,1,24,28,10,Broadleaved
...
infile = open("trees.csv",'r')
Trees = []
i = int ; f = float ; s = str
FirstLine = infile.readline()
print FirstLine[0:len(FirstLine)-2]
#
#
#
#
#
Open the file for reading
List will hold the tree objects
Type conversion functions, shortened
Read the header line
Print it, strip the last "\n"
for line in infile:
# Loop the remaining lines
L = line.split(",")
# Split where there is a comma to list L
treepos = Point3D(f(L[1]),f(L[2]),f(L[3]))
# Call the constructor for a Tree(), convert types on-the-fly.
Trees.append(Tree(i(L[0]), treepos ,f(L[4]),f(L[5]),f(L[6]),s(L[7][0:4]) ))
# When done, close the file
infile.close()
1.2 Trees
As objects (class Tree)
Stored in list Trees
Attributes:
.num
.pos
.h
.dbh
.hc
.sp
Methods:
None, but we may create some later....
1.3 Parameters - flying related
class Campaign:
def __init__(self):
self.Startpoint = None
self.Direction = Point3D(0,1,0)
self.Duration = None
self.Flyingspeed = None
Startpoint will be a Point3D that defined the xyz-point when scanning commences.
Direction, we keep this fixed to North (j==1), keeping the altitude (k ==0)
Duration, tells how many seconds we will scan the scene.
Flyingspeed, is groundspeed (though we never know it in advance, due to winds), m/s.
No methods, just a storage class.
1.4 Parameters - Sensor (LiDAR) related
class LiDAR:
def __init__(self):
self.PRF = None
self.Divergence = None
self.ScanFreq = None
self.MaxScanAngle = None
self.NumOfRays = None
#
#
#
#
#
Hz, the pulse repetition frequency
radians, contains 67% energy
Hz, mirror does this many "zigzags" per second
radians, the max zenith angle of the mirror
N, How many sub-rays simulate one pulse
PRF is a crucial parameter in LIDAR, nowadays 25kHz...300kHz.
Divergence, our scene is just 50 x 50 m, so we will fly low, divergence 1...4 mrad?
ScanFreq, typically 20..100 Hz. with PRF, speed, and height define point pattern.
MaxScanAngle, we assume linear (in angle) movement of mirror, swath width!
NumOfRays, The more, the more realistic, and slower...
No methods, just a storage class.
1.3 + 1.4 Parameters - reading from a file
Duration=2.0
Flyingspeed=65.0
Startpoint=25,0,125
Direction=0,1,0
PRF=200000
Divergence=0.001
ScanFreq=50
MaxScanAngle=18
NumOfRays=20
1.3 + 1.4 Parameters - from file
# Parameters - reading from file
c = Campaign() # Construct a Campaign -object
l = LiDAR()
# Construct a LiDAR
-object
Keys =["Duration", "Flyingspeed", "Startpoint", "Direction",\
"PRF","Divergence", "ScanFreq", "MaxScanAngle", "NumOfRays"]
f = open("parameters.txt", "r")
fl = float
for line in f:
a = line.split("=")
if a[0] == Keys[0]: c.Duration = fl(a[1])
if a[0] == Keys[1]: c.Flyingspeed = fl(a[1])
if a[0] == Keys[2]:
b = a[1].split(",")
c.Startpoint = Point3D(fl(b[0]), fl(b[1]), fl(b[2]))
if a[0] == Keys[3]:
b = a[1].split(",")
c.Startpoint = Point3D(fl(b[0]), fl(b[1]), fl(b[2]))
if a[0] == Keys[4]: l.PRF = fl(a[1])
if a[0] == Keys[5]: l.Divergence = fl(a[1])
if a[0] == Keys[6]: l.ScanFreq = fl(a[1])
if a[0] == Keys[7]: l.MaxScanAngle = math.radians(fl(a[1]))
if a[0] == Keys[8]: l.NumOfRays = int(a[1])
Other basic stuff of "input nature"
class Point2D:
# A simple class for storing xy-points/vectors
def __init__(self,x,y):
self.x = float(x)
self.y = float(y)
def norm(self): # The length
return math.sqrt(self.x**2+self.y**2)
class Point3D:
# A simple class for xyz-points/vectors
def __init__(self,x,y,z):
self.x = x
self.y = y
self.z = z
def norm(self):
return math.sqrt(self.x**2+self.y**2+self.z**2)
def normalize(self): # IN PLACE operation, make norm == 1
length = self.norm()
self.x = self.x / length
self.y = self.y / length
self.z = self.z / length
Other basic stuff of "input nature"
Functions
def _3Dnorm(x,y,z):
return math.sqrt(x**2+ y**2 + z**2)
def vector_angle(v1, v2):
"""acute angle between 3Dvectors v1 and v2"""
a = (v1.x*v2.x+v1.y*v2.y+v1.z*v2.z)
b = (v1.norm()*v2.norm())
return math.acos(a/b)
def sign(a):
"""sign of
if a < 0:
if a > 0:
if a == 0:
a"""
return -1
return 1
return 1
def DotProduct(x,y):
"""Gives the dot product (inner product)"""
Dot = 0.0
for i in range(len(x)):
Dot += x[i]*y[i]
return Dot
Other basic stuff of "input nature"
def InsidePolygon(P, Px):
""" Says if Point2D Px is inside P consisting of N Point2D's"""
counter = 0
i = 0
xinters = 0.0
N = len(P)
p1 = Point2D(P[0].x,P[0].y)
for i in range (1, N+1,1):
p2 = Point2D(P[i%N].x,P[i%N].y)
if (Px.y > min(p1.y, p2.y)):
if (Px.y <= max(p1.y, p2.y)):
if (Px.x <= max(p1.x, p2.x)):
if (p1.y != p2.y):
xinters = (Px.y-p1.y)*(p2.x-p1.x)/(p2.y-p1.y)+p1.x
if ((p1.x == p2.x) or (Px.x <= xinters)):
counter = counter + 1
p1 = p2
if (counter%2 == 0):
InsidePolygon = False
else:
InsidePolygon = True
return InsidePolygon
SKELETON
1. Read the inputs
Scene
1.1. Planar polygons (ground, walls, roof planes)
1.2. Trees
Params 1.3. Flying (Campaign) parameters
1.4. Sensor (LiDAR) parameters
2. Compute the needed trajectory for the whole duration of the flight, for each
moment the LiDAR is fired (transmitted). Let the LiDAR sway.
3. Start the flight, from the moment LiDAR operation begings (time==0). Compute
the mirror angle. Compute the dir-vector of the pulse, and actually dir-vectors of all
N sub-rays. They all start from point p, the position of the LiDAR.
4. Transmit a pulse (made of N sub-rays)
for all sub-rays:
for all planar polygons:
Check intersection
Choose the plane object with shortest distance
for all trees:
Check intersection
Choose tree object with shortest distance
Store the sub-ray result (xyz, distance, reflection strength) to list
Let time grow one unit, and goto 3, unless time is over.
5. Finally write the results (for subrays) to a file.
2. Compute the needed trajectory
class Trajectory:
def __init__(self, time, x, y, z, omega, phi, kappa):
self.time = time
# Time runs from 0,1,2... in ticks
self.x = x
# x of LiDAR
self.y = y
# y of LiDAR
self.z = z
# z of LiDAR
self.omega = omega
# LiDAR's rotation over x axis, radians
self.phi = phi
# LiDAR's rotation over y axis, radians
self.kappa = kappa
# LiDAR's rotation over z axis, radians
The pulse-ray is p + t * dir
p = [x,y,z] at time point time
t tells how far the ray goes from p
and dir is the 3D direction [i,j,k].
omega, phi and kappa are needed
as well as mirror angle to compute
dir (we will do that later).
2. The needed trajectory
The needed number of trajectory objects is
l.PRF * c.Duration
One object is perhaps ~ 60 bytes of storage (one int
and 6 floats). So if PRF is 200 kHz and duration is 10
seconds, the storage need is ~133 MegaBytes.
So, for a short flying we can create the trajectory in
advance, store the objects in a list and need not to
create them on-the-fly.
2. The needed trajectory
def ComputeTrajectory(C, L):
# C and L are Campaign and LiDAR objects
t = 0
# time at the beginning, integer 'tick'
TRJ_LIST = [] # List to hold the trajectory objects
Duration = C.Duration * L.PRF # In number of pulses ('ticks')
Tlapse = 1.0/L.PRF
# Time between pulses, seconds
omega = 0; phi = 0; kappa =0 # Aircraft is perfectly aligned
while (t < Duration ):
# The position at time point t.
x = C.Startpoint.x + C.Flyingspeed * Tlapse * t * C.Direction.x
y = C.Startpoint.y + C.Flyingspeed * Tlapse * t * C.Direction.y
z = C.Startpoint.z + C.Flyingspeed * Tlapse * t * C.Direction.z
# Attitude of the plane/sensor, let it float, radians
omega += (random.random()-.5) * 0.0001
phi +=
(random.random()-.5) * 0.0001
# Keeping kappa unchanged.
# If omega and phi wonder too far, limit to +/- 0.05 radians
if abs(omega) > 0.05: omega = sign(omega)*0.05
if abs(phi) > 0.05:
phi
=
sign(phi)*0.05
# Construct a Trajectory object, put it to the list
TRJ_LIST.append(Trajectory(t,x,y,z,omega,phi,kappa))
t +=1 # increment time, by one pulse ('tick')
# Finally, the whole trajectory is known, return the list.
return TRJ_LIST
2. The needed trajectory
Examples of omega(t), phi(t) for a case with t=0...14999 (pulses).
y-axis has the rotation, in radians.
SKELETON
1. Read the inputs
Scene
1.1. Planar polygons (ground, walls, roof planes)
1.2. Trees
Params 1.3. Flying (Campaign) parameters
1.4. Sensor (LiDAR) parameters
2. Compute the needed trajectory for the whole duration of the flight, for each
moment the LiDAR is fired (transmitted). Let the LiDAR sway.
3. Start the flight, from the moment LiDAR operation begings (time==0). Compute
the mirror angle. Compute the dir-vector of the pulse, and actually dir-vectors of all
N sub-rays. They all start from point p, the position of the LiDAR.
4. Transmit a pulse (made of N sub-rays)
for all sub-rays:
for all planar polygons:
Check intersection
Choose the plane object with shortest distance
for all trees:
Check intersection
Choose tree object with shortest distance
Store the sub-ray result (xyz, distance, reflection strength) to list
Let time grow one unit, and goto 3, unless time is over.
5. Finally write the results (for subrays) to a file.
What do we have now?
classes:
Point2D
Point3D
BoundingBox
PolygonSurface
Tree
Campaign
LiDAR
Trajectory
important storage variables:
list with SurfacePolygon_objs
list with Tree_objs
list with Trajectory_objs
identifier with Campaign_obj
identifier with LiDAR_obj
functions:
_3Dnorm((x,y,z))
vector_angle(vectorA,vectorB)
sign(number)
DotProduct(vectorA, vectorB)
InsidePolygon(ListOfPoints2Ds, Point2D)
ComputeTrajectory(Campaign_obj, LiDAR_obj)
3. Start the flight
def main():
# "Houston - Preparing for takeoff".
Echoes = [] # This list will hold the observations
# Read the parameters and construct Campaign and LiDAR objects
c, l = ReadParameterFile("parameters.txt")
TRJ_list = ComputeTrajectory(c, l) # Trajectory - whole flight
S = readsurfaces("Scene.csv")
# Read planar objects to list S
T = readtrees("Trees.csv")
# Trees to list T
# "HOUSTON - PREPARING FOR SCANNING", Mirror angle == Angle
# increment == move mirror this much between pulses, radians
n = 0; Angle = 0.0;
PulsesPerMirrorCycle = l.PRF / l.ScanFreq
increment = (l.MaxScanAngle*4.0)/ PulsesPerMirrorCycle
for TRJ in TRJ_list: # START FLYING WITH LIDAR ON....
n +=1
# Pulse counter
if Angle > 0 and Angle > l.MaxScanAngle: increment = -increment
if Angle < 0 and Angle < -l.MaxScanAngle: increment = -increment
Angle += increment # Update mirror angle, in zigzag pattern
# A. Get a bundle of sub-rays for this Pulse
RayList = computeSubRays(TRJ, Angle,l )
# B. For each sub-ray, find the closest object (range)
# C. Store the closest reflection
# D. Shoot the next Pulse.
# When DONE, Store the observations.
3. Compute dir-vectors for all sub-rays in a pulse
in main(), we had a function call for pulse
RayList = computeSubRays(TRJ, Angle,
l )
Now, TRJ (Trajectory object) has the position (x,y,z) and attitude (omega,
phi, kappa) of the LiDAR for this pulse. Angle holds the mirror angle. l is
the LiDAR object.
We must compute l.NumOfRays sub-rays, filling the angle l.Divergence.
3. Compute dir-vectors for all sub-rays in a pulse
def computeSubRays(TRJ, ScanAngle, LiDAR):
"""Returns a ray pencil of (i,j,k) vectors making the pulse"""
COS = math.cos; SIN = math.sin; TAN = math.tan
RayList = []
for case in range(LiDAR.NumOfRays):
# Assume mirror points down, vector = [0,0,1]
Omega = random.gauss(0,l.Divergence) # shake over x-axis
Phi = random.gauss(0, l.Divergence) # shake over y-axis
# kappa == 0 this means that sin(kappa)==0 and cos(kappa)==1
# 3x3 Rotation matrix becomes
r11= COS(Phi);
r12=0;
r13= SIN(Phi)
r21= SIN(Omega)*SIN(Phi); r22=COS(Omega); r23=-SIN(Omega)*COS(Phi)
r31=-COS(Omega)*SIN(Phi); r32=SIN(Omega); r33= COS(Omega)*COS(Phi)
# Rotate mirror [i,j,k]=R*[0,0,1]
i = r11 * 0 + r12 * 0 + r13 * 1
j = r21 * 0 + r22 * 0 + r23 * 1
k = r31 * 0 + r32 * 0 + r33 * 1
3. Compute dir-vectors for all sub-rays in a pulse
Example:
1000 sub-rays
l.Divergence = 0.0003 (3 milliradians)
[i,j,k] Pattern 100 m away, hits the origin (fig below).
Sdev(x), Sdev(y) = 3 cm.
419 out of 1000 are within radius of 3 cm.
Footprint means 67% of energy (670 s.b. within).
=> Divide Divergence with SQRT(2)
random.gauss(0,l.Divergence/1.4142)
So, we fill the footprint with subrays having the mirror pointing
down - we 'shaked' the (0,0,1)
vector to produce an energy
distribution.
Next we must apply rotation of
the mirror to all sub-rays.
so... Apply rotation of the mirror to all sub-rays.
def compute_SubRays(TRJ, ScanAngle, LiDAR):
...(continues)...
# Mirror is at ScanAngle,
# ScanAngle corrsponds to rotation about the y-axis (Phi)
Phi = ScanAngle
r11 = COS(Phi); r12 = 0; r13 = SIN(Phi)
r21 = 0;
r22 = 1; r23 = 0
r31 = -SIN(Phi); r32 = 0; r33 = COS(Phi)
ii = r11 * i + r12 * j + r13 * k
jj = r21 * i + r22 * j + r23 * k
kk = r31 * i + r32 * j + r33 * k
# Vector [i,j,k] now points to where
# "the mirror says", i.e. to [ii,jj,kk]
3. Compute dir-vectors for all sub-rays in a pulse
The pulse now departs the LiDAR in
known direction [ii,jj,kk], with respect
to the scanangle.
The example below had scanangle of
0.1radians, scanning distance of
distance 100 m. The pattern lies on
the X-axis, ~10 m to the right.
Next, we must rotate the whole
LiDAR that is attached to the
aircraft that rotates in 3D.
omega, phi, kappa rotation
angles are stoted in the
Trajectory object for that pulse.
def compute_SubRays(TRJ, ScanAngle, LiDAR):
...(continues)...
# Next we rotate [ii,jj,kk] to the world coordinate system
# Trajectory object TRJ holds the attitude of the airplane.
Phi = TRJ.phi; Omega = TRJ.omega; Kappa = TRJ.kappa
# Orthogonal 3x3 Rotation matrix
r11 = COS(Phi)*COS(Kappa); r12=-COS(Phi)*SIN(Kappa); r13=SIN(Phi)
r21 = COS(Omega)*SIN(Kappa)+SIN(Omega)*SIN(Phi)*COS(Kappa)
r22 = COS(Omega)*COS(Kappa)-SIN(Omega)*SIN(Phi)*SIN(Kappa)
r23 =-SIN(Omega)*COS(Phi)
r31 = SIN(Omega)*SIN(Kappa)-COS(Omega)*SIN(Phi)*COS(Kappa)
r32 = SIN(Omega)*COS(Kappa)+COS(Omega)*SIN(Phi)*SIN(Kappa)
r33 = COS(Omega) * COS(Phi)
# The [ii,jj,kk] is rotated and assigned back to [i,j,k]
i = r11 * ii + r12 * jj + r13 * kk
j = r21 * ii + r22 * jj + r23 * kk
k = r31 * ii + r32 * jj + r33 * kk
# Normalize it to length one
length = _3Dnorm(i,j,k)
i = i / length ; j = j / length; k = k / length
# Store this sub-ray to RayList as a tuple, negative vector
RayList.append((-i,-j,-k))
3. Compute dir-vectors for all sub-rays in a pulse
Raylist
will hold l.NumOfRays tuples
of floats (i,j,k), i.e. the
direction (dir) vectors of the
sub-rays, making a pulse:
p + t*dir
The list is returned back to
the main().
Now we have for this pulse,
the position p, and the dirvectors of all sub-rays.
It s possible to cast them
down and start to look for
intersections.
4. Transmit a pulse, sub-ray by sub-ray
def main():
# "Houston - Preparing for takeoff".
Echoes = [] # This list will hold the observations
# Read the parameters and construct Campaign and LiDAR objects
c, l = ReadParameterFile("parameters.txt")
TRJ_list = ComputeTrajectory(c, l) # Trajectory - whole flight
S = readsurfaces("Scene.csv")
# Read planar objects to list S
T = readtrees("Trees.csv")
# Trees to list T
# "HOUSTON - PREPARING FOR SCANNING", Mirror angle == Angle
# increment == move mirror this much between pulses, radians
n = 0; Angle = 0.0;
PulsesPerMirrorCycle = l.PRF / l.ScanFreq
increment = (l.MaxScanAngle*4.0)/ PulsesPerMirrorCycle
for TRJ in TRJ_list: # START FLYING WITH LIDAR ON....
n +=1
# Pulse counter
if Angle > 0 and Angle > l.MaxScanAngle: increment = -increment
if Angle < 0 and Angle < -l.MaxScanAngle: increment = -increment
Angle += increment # Update mirror angle, in zigzag pattern
# A. Get a bundle of sub-rays for this Pulse
RayList = compute_SubRays(TRJ, Angle,l )
# B. For each sub-ray, find the closest object (range)
for case in range(len(RayList)):
Object = FindClosest(n, case, RayList[case], c, l, TRJ)
# C. Store the closest reflection
# D. Shoot the next Pulse.
# When DONE, Store the observations.
4. A sub-ray in a pulse - find scane object
def FindClosest(n, case, Ray, S, T, TRJ):
# Pulse-#, Sub-ray-#, Rays' dir vector (tuple), Surfaces, Trees, trajectory
i, j, k = Ray
DistMin = TRJ.z + 1E5
Data = None
# Assign the values from the tuple to i,j,k
# Look for the minimum, candidate to large value
# The return value.
for s in S:
# Loop surfaces S
ValueRet = SurfaceIntersect(n, case, TRJ.x, TRJ.y, TRJ.z, i, j, k, s)
if ValueRet != None:
# We have an intersection for Surface s in S
if ValueRet[4] < DistMin: # Check the distance
DistMin = ValueRet[4]
# New minimum was found
Data = ValueRet
# Surfaces now checked, Data has the minimum case, or None
for t in T: # Now start to check trees in list T
CHit, THit = TreeIntersect(n, case, TRJ.x, TRJ.y, TRJ.z, i,j,k, t)
if CHit[0] < DistMin: # Check the potential crown hit
DistMin = CHit[0] # Set the new minimum
Data = [n, case, "Crown",CHit[4],CHit[0],0,CHit[1], CHit[2], CHit[3],i,j,k]
if THit[0] < DistMin: # There is a potential crown hit
DistMin = THit[0]
Data = [n, case, "Trunk",THit[4],THit[0],0,THit[1], THit[2], THit[3],i,j,k]
return Data
# Trees: [pulse-#, ray-#, Type, distance, reflection, distance, 0, x,y,z, i,j,k]
# Surfaces:[pulse-¤, ray-#, Type, Reflectance, distance, angle, x,y,z, i,j,k]
4. A sub-ray in a pulse - checking intersection
SurfaceIntersect(n, case, TRJ.x, TRJ.y, TRJ.z, i,j,k, s)
TreeIntersect(n, case, TRJ.x, TRJ.y, TRJ.z, i,j,k, t)
ray
s
s
t
t
def SurfaceIntersect(nc, case, x, y, z, i, j, k, Surf):
# nc = pulse #, case = sub-ray #. (x, y, z) = LiDAR position,
# (i,j,k) = direction vector of the sub-ray.
# Surf is a SurfacePolygon -object.
# Ray = (x,y,z) + t * (i,j,k).
# Surface is defined by normal vector n =[A,B,C] and one point, p1.
# Hit is at distance t = ((p1 - (x,y,z)) .dot. n) / ( (i,j,k) .dot. n )
# Note! Here Points/Vectors are tuples [0]==x, [1]==y, [2]==z.
n = (Surf.A, Surf.B, Surf.C)
P1 = (Surf.Pline[0].x,Surf.Pline[0].y, Surf.Pline[0].z)
P = (x,y,z)
Dir = (i,j,k)
Divisor = DotProduct(Dir, n)
P1P = (P1[0]-P[0],P1[1]-P[1],P1[2]-P[2])
Numerator = DotProduct(P1P, n)
t = 0
try :
t = Numerator / Divisor
except:
pass
# We have t solved, make a Point3D (endpoint of ray) at distance t.
test = Point3D(x+i*t, y+j*t, z+k*t)
# BOUNDING-BOX CHECK
if not ((test.x > Surf.Box.min.x and test.x < Surf.Box.max.x) \
and (test.y > Surf.Box.min.y and test.y < Surf.Box.max.y) \
and (test.z > Surf.Box.min.z and test.z < Surf.Box.max.z)):
return None
# TO BE CONTINUED....
... SurfaceIntersect(nc, case, x, y, z, i, j, k, Surf) .. continues
# It is in the Box. Attitude of the surface defines inclusion testing.
# if i ~ 1, surface is ¨aligned with yz-plane. if j ~ 1 it is
# aligned with xz-plane. if k ~ 1 it is aligned with xy-plane.
P = [] # This list holds the 2D test polygon (with Point2D's)
if abs(Surf.A) > 0.9 : # go for the yz-plane option
Px = Point2D(test.y, test.z)
Type = "A"
for m in Surf.Pline:
P.append(Point2D(m.y,m.z))
elif abs(Surf.B) > 0.9: # xz-plane
Px = Point2D(test.x, test.z)
Type = "B"
for m in Surf.Pline:
P.append(Point2D(m.x,m.z))
else: # xy-plane
Px = Point2D(test.x, test.y)
Type = "C"
for m in Surf.Pline:
P.append(Point2D(m.x,m.y))
if (InsidePolygon(P, Px)): # Is Px inside P?
# It is inside, compute angles for both directions of surface normal.
va1 = vector_angle(Point3D(Surf.A,Surf.B,Surf.C), Point3D(i,j,k))
va2 = vector_angle(Point3D(-Surf.A,-Surf.B,-Surf.C), Point3D(i,j,k))
# Return pulse-#, ray-#, Type, reflectance, ditance, angle, hit-point, dir
return [nc,case,Type,Surf.Refl,t,min(va1,va2),test.x,test.y,test.z,i,j,k]
else:
return None # Ray did not hit the surface.
def TreeIntersect(nc, case, x, y, z, i, j, k, t):
# (x,y,z) = LiDAR position (i,j,k) = ray dir-vector, t = tree
treetop = Point3D(t.pos.x, t.pos.y, t.pos.z + t.h)
treebase = Point3D(t.pos.x, t.pos.y, t.pos.z)
crownbase = Point3D(t.pos.x, t.pos.y, t.pos.z + t.hc)
# Ray: (x,y,z) + d * (i,j,k) , solve d for treetop and crownbase z
d_top = (treetop.z-z)/k
# distance | z = treetop.z (apex)
d_base = (treebase.z-z)/k
# distance | z = treebase.z (base)
d_cbase = (crownbase.z-z)/k # distance | z = crownbase.z (base, butt)
# Reflectance of needles
if t.sp == "Coni": refl = 0.35
if t.sp == "Broa": refl = 0.40
# These tuples are the return from this function
CrownHit = (); TrunkHit = ()
# End-point (xt,yt,zt) of the ray, at distance d_top
xt, yt, zt = x + d_top * i, y + d_top * j, z + d_top
# Endpoint's horizontal distance from trunk
d_trunk = math.sqrt((xt-t.pos.x)**2+(yt-t.pos.y)**2)
# Check if we are close enough to continue, a circle
angle = vector_angle(Point3D(0,0,1), Point3D(i,j,k))
limit = math.sin(angle)*t.h + 1.0
if d_trunk > (t.crown_radius(crownbase.z) + limit):
return (1E20,0,0,0,0), (1E20,0,0,0,0)
# The ray
(zt == treetop.z)
* k
around treetop
can not intersect
TreeIntersect() continues...
# The point was inside the test circle, continue with CROWN intersection testing
losses = 0; crownlength = treetop.z-crownbase.z
z_test = zt
while (z_test > crownbase.z): # Loop heights between top and crown base
cr = t.crown_radius(z_test) # Apply the crown model to get crown radius
d_end = (z_test-z)/k
# The end of the ray, solve distance from z_test
xt, yt, zt = x + d_end * i, y + d_end * j, z + d_end * k # the endpoint xyz
d_trunk = math.sqrt((xt-t.pos.x)**2+(yt-t.pos.y)**2)
# distance to trunk
if d_trunk < cr: # if distance is less than crown radius, we are inside
# The new variables (ksii, rc, theta) to test if we get an observation
ksii = (treetop.z-z_test)/crownlength
# Rel. dist. from top
rc = 1 - d_trunk/cr
# Rel. horizontal distance
theta = math.atan2((xt-t.pos.x),(yt-t.pos.y)) # Azimuth -Pi...+Pi
rand = random.random()*TO_RADIANS(60)
# Random rotation 0...60 deg.
thetaf = abs(math.cos(6*phi+rand))
# The branches component
heightf = 1.0
if ksii > .5: heightf = 1 - ksii
# The vertical component
radiusf = 0.2
if rc > 0.25: radiusf = 1-rc+0.05
# Horizontal component
target = heightf*thetaf*radiusf
if (target-losses) > 0.05 :
# Yes!, tuple with dist, x, y, z and strength of backscatter
CrownHit = (d_end, xt, yt, zt,(target-losses)
break # break the while-loop since we have an obs from crown
losses += target
z_test -= 0.30 # Still looping, set the endpoint down 30 cm for next test
TreeIntersect() continues...
# ---- The trunk (cone) below a crown - POOR SOLUTION ---basediam = (1/((t.h-1.3)/t.h)) * t.dbh/100.0
# Parameters for the trunk radius-model (same as the crown model)
tc = 1.0 ; tb = basediam ; ta = 0.0
# The horizontal distances at tree base and crown base
r_base = math.sqrt((x+d_base*i-t.pos.x)**2+(y+d_base*j-t.pos.y)**2)
r_cbase = math.sqrt((x+d_cbase*i-t.pos.x)**2+(y+d_cbase*j-t.pos.y)**2)
# For a reasonable candidate, lets try "intersection"
if r_base < 5 and r_cbase < 5:
d_test = d_base
# d_base = distance to tree base along ray
while d_test > d_cbase:
# Loop from tree base to crown base
z_test = z + d_test*k;
# Solve z from distance along LiDAR ray
ksii = (treetop.z-z_test)/t.h # Current Rel. height, 0 top, 1 = base
r_trunk = math.sqrt((x+d_test*i-t.pos.x)**2+(y+d_test*j-t.pos.y)**2)
r_true = ta + (ksii**tc) * tb
if abs(r_trunk-r_true) < 0.025:
print "Trunk at h:", z_test-treebase.z, "this close", abs(r_trunk-r_true)
TrunkHit = (d_test, x+d_test*i, y+d_test*j, z+d_test*k,0.3-losses)
break
d_test -= 0.05 # Move at small steps
if CrownHit == () : CrownHit = (1E20,0,0,0,0) # Return a large distance
if TrunkHit == () : TrunkHit = (1E20,0,0,0,0) # and zeroes, if no observation
return list(CrownHit), list(TrunkHit)
4. A sub-ray - checking intersection - return values
SurfaceIntersect(n, case, TRJ.x, TRJ.y, TRJ.z, i,j,k, s)
If there is a hit, returns a list, with
[pulse_number,
ray-number,
Type,
Surf.Refl,
t,
angle,
xt,yt,zt
i,j,k]
0...duration*PRF
0...NumOfRays-1
A, B, C depending in plane orientation
0..1 of the surface
Distance to the intersection point
Incidence angle of the pulse and the ray
Point of intersection
The ray's direction vector
TreeIntersect(n, case, TRJ.x, TRJ.y, TRJ.z, i,j,k, t)
returns list(CrownHit), list(TrunkHit)
If there are observations:
(distance, x,y,z, strength of reflection)
(distance, x,y,z, strength of reflection)
If no observation:
(1E20,0,0,0,0)
(1E20,0,0,0,0)
In TreeIntersect() unseen method for class Tree
class Tree:
def __init__(self, num, pos, h, dbh, hc, sp):
self.num = num
self.pos = pos
self.h
= h
self.dbh = dbh
self.hc = hc
self.sp = sp
def crownradius(self,z):
z_cbase = self.pos.z + self.hc # z at crown base
z_top = self.pos.z + self.h
# z at top
if not((z_cbase <= z) and (z <= z_top)):
return 0.0
# z is outside z-range of crown
else:
crown_l = z_top - z_cbase
# length of the crown
ksii = (z_top-z)/float(crown_l) # 0 = top, 1= crown base
a = 0.25
# Constant, flat top
if self.sp == "coni":
# Select parameters f(species)
b = 15*self.dbh/100.0
# cm to m conversion
c = 0.7
if self.sp == "broa":
b = 20*self.dbh/100.0
c = 0.5
return a + b*ksii**c
# Compute the radius. return it
4. Where are we now? - We are DONE!
def main():
# "Houston - Preparing for takeoff".
Echoes = [] # This list will hold the observations
# Read the parameters and construct Campaign and LiDAR objects
c, l = ReadParameterFile("parameters.txt")
TRJ_list = ComputeTrajectory(c, l) # Trajectory - whole flight
S = readsurfaces("Scene.csv")
# Read planar objects to list S
T = readtrees("Trees.csv")
# Trees to list T
# "HOUSTON - PREPARING FOR SCANNING", Mirror angle == Angle
# increment == move mirror this much between pulses, radians
n = 0; Angle = 0.0;
PulsesPerMirrorCycle = l.PRF / l.ScanFreq
increment = (l.MaxScanAngle*4.0)/ PulsesPerMirrorCycle
for TRJ in TRJ_list: # START FLYING WITH LIDAR ON....
n +=1
# Pulse counter
if Angle > 0 and Angle > l.MaxScanAngle: increment = -increment
if Angle < 0 and Angle < -l.MaxScanAngle: increment = -increment
Angle += increment # Update mirror angle, in zigzag pattern
# Get a bundle of sub-rays for this Pulse
RayList = compute_SubRays(TRJ, Angle,l )
# For each sub-ray, find the closest object with a hit
for case in range(len(RayList)):
Object = FindClosest(n, case, RayList[case], c, l, TRJ)
We have the closest object hit by the ray, for each ray. Alternatively.
tree [pulse-#, ray-#, Type, distance, reflection, distance, 0, x,y,z, i,j,k]
plane [pulse-#, ray-#, Type, Reflectance, distance, angle, x,y,z, i,j,k]
STORE THEM TO A LIST OBSERVATIONS FOR OUTPUT AT THE END.
What do we have now?
Functions are used for abstraction - hiding away
implementation details. Classes are used for that
too, although not as much.
Functions:
Classes:
Point2D
Point3D
BoundingBox
PolygonSurface
Tree
Campaign
LiDAR
Trajectory
_3Dnorm((x,y,z))
vector_angle(vectorA, vectorB)
important storage variables:
sign(number)
list with SurfacePolygon_objs
DotProduct(vectorA, vectorB)
list with Tree_objs
InsidePolygon(VertexList, Point2D)
list with Trajectory_objs
readParameterFile("parameters.txt")
identifier with Campaign_obj
readsurfaces("Scene.csv")
identifier with LiDAR_obj
readtrees("Trees.csv")
list with observations, Echoes
ComputeTrajectory(Campaign, LiDAR)
Compute_SubRays(Trajectory, MirrorAngle, LiDAR )
FindClosest(Pulse#, Ray#, (i,j,k), Campaign, LiDAR, Trajectory)
SurfaceIntersect(Pulse#, Ray#, TRJ.x, TRJ.y, TRJ.z, i, j, k, SurfacePolygon)
TreeIntersect(Pulse#, Ray#, TRJ.x, TRJ.y, TRJ.z, i,j,k, Tree)
main()
INPUT-phase
Create the trajectory
c, l= ReadParameterFile()
S = ReadSurfaces()
T = ReadTrees()
TRJ_LIST = ComputeTrajectory()
Fly in a while loop, let time increase at steps of PRF, till duration of flight is reached:
Set mirror Angle for each time point
Transmit:
Get the Ray bundle
Raylist = Compute_SubRays ()
For each sub-ray:
Hit = FindClosestObject():
Loop surfaces for a hit, keep track of closest surfhitcand = SurfaceIntersect()
Loop crowns and later trunks for a hit.
treehitcand = TreeIntersect()
Store the Hit to a DataList
After transmission, increase time
After Flying, write the DataList to a file
VISUALIZE
Handling the large amount of code
Simulator
LiDAR_Classes.py
LiDAR_Functions_IO.py
LiDAR_Functions_Intersect.py
LiDAR_Functions_Misc.py
LiDAR_Main.py
class definitions
file I/O functions
ray-target intersection functions
Other functions
import statements and the main()
The py-files need the import statements, e.g. if a class is used in a function
in a code module, import LiDAR_Classes must be added in tha code module.
numpy package must be installed (for class PolygonSurface).
input files
params.txt
scene.csv
trees.csv
Output files
lidar.csv (data for all ray's with a hit, header row)
Visualization
graph.py
jetColorMAP.csv (defines 64 nice colors (HSV-colors))
Visualization
graph.py