lidar simulator marv216 project 2012 ilkka korpela tree crown inclined ground house

LiDAR simulatorMARV216 project 2012

Ilkka Korpela

Tree crownInclined ground House

House

LiDAR is flown across the scene along a trajectory.

Target (X, Y, Z)

Range

Scan angle

tx (X, Y, Z, , , )

tx is a time-point when LiDAR is “fired”

Target position:

[X,Y,Z] = [X,Y,Z]LiDAR + range [i, j, k]∙==

[X,Y,Z] = p + t * dir

dir = [i,j,k] = f() g(∙ , , )

(Mirror moves inside the LiDAR, and the whole LiDAR rotates in 3D).

LiDAR in a nutshell

The ”photon clump” (pulse).

Illuminated area

Divergence, contains e.g. 1/e2 of the energy

A real LiDAR can give raise to and detect multiple reflections per pulse transmitted.

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-raysof a 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?)

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.

a

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. TreesParams 1.3. Flying (Campaign) parameters

1.4. Sensor (LiDAR) parameters2. 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 distancefor all trees:

Check intersectionChoose tree object with shortest distanceStore 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,310,10,210,30,210,30,610,10,610,10,2Wall,0.25,310,10,216,10,216,10,610,10,610,10,2Wall,0.25,316,10,216,30,216,30,616,10,616,10,2Wall,0.25,310,30,210,30,616,30,616,30,210,30,2Roof,0.18,29,9,5.89,31,5.813,31,1013,9,109,9,5.8...

1.1 Planar 3D polygons - scene.csv

”name”, reflectance, powerx,y,zx,y,zx,y,z,...

class PolygonSurface: def __init__(self):

self.Vertices = []self.refl = Noneself.pow = Noneself.Box = Noneself.A = Noneself.B = Noneself.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]

f = open("scene.csv","r")Surfaces = [] # To hold the PolygonSurface objectsfor 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 - reading scene.csvWall,0.25,310,10,210,30,210,30,610,10,610,10,2Wall,0.25,310,10,216,10,216,10,610,10,610,10,2Wall,0.25,316,10,216,30,216,30,616,10,616,10,2Wall,0.25,310,30,210,30,616,30,616,30,210,30,2Roof,0.18,29,9,5.89,31,5.813,31,1013,9,109,9,5.8...

class BoundingBox: """mins and maxs of x,y,z that make a non-rotated 3D bounding box""" def __init__(self, mini, maxi): self.min = mini # Point3D object self.max = maxi # 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 - Bounding Box

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] # 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 - Surface normal vector

[A,B,C]

1.1 Planar 3D polygons

Objects (class PolygonSurface)Stored in list SurfacesAttributes:

Vertices[i].x (list of Point3D's)reflpowA,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,sp1,35,30,1,19,21,6,Conifer2,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,sp1,35,30,1,19,21,6,Conifer2,37,45,1,24,28,10,Broadleaved...

infile = open("trees.csv",'r') # Open the file for reading Trees = [] # List will hold the tree objectsi = int ; f = float ; s = str # Type conversion functions, shortenedFirstLine = infile.readline() # Read the header line print FirstLine[0:len(FirstLine)-2] # 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 TreesAttributes:

.num

.pos

.h

.dbh

.hc

.spMethods:

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 # Hz, the pulse repetition frequency self.Divergence = None # radians, contains 67% energy self.ScanFreq = None # Hz, mirror does this many "zigzags" per second self.MaxScanAngle = None # radians, the max zenith angle of the mirror self.NumOfRays = None # 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.0Flyingspeed=65.0 Startpoint=25,0,125Direction=0,1,0PRF=200000Divergence=0.001ScanFreq=50MaxScanAngle=18NumOfRays=20

# Parameters - reading from filec = Campaign() # Construct a Campaign -objectl = LiDAR() # Construct a LiDAR -objectKeys =["Duration", "Flyingspeed", "Startpoint", "Direction",\ "PRF","Divergence", "ScanFreq", "MaxScanAngle", "NumOfRays"]f = open("parameters.txt", "r")fl = floatfor 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])

1.3 + 1.4 Parameters - from file

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 a""" if a < 0: return -1 if a > 0: return 1 if a == 0: 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








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 timet tells how far the ray goes from pand 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.


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


Examples of omega(t), phi(t) for a case with t=0...14999 (pulses).y-axis has the rotation, in radians.

SKELETON








What do we have now?

classes:Point2DPoint3DBoundingBoxPolygonSurfaceTreeCampaignLiDARTrajectory

functions:_3Dnorm((x,y,z))vector_angle(vectorA,vectorB)sign(number)DotProduct(vectorA, vectorB)InsidePolygon(ListOfPoints2Ds, Point2D)ComputeTrajectory(Campaign_obj, LiDAR_obj)

important storage variables:

list with SurfacePolygon_objslist with Tree_objslist with Trajectory_objsidentifier with Campaign_objidentifier with LiDAR_obj

3. Start the flightdef 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.

in main(), we had a function call for pulse

RayList = computeSubRays(TRJ, Angle, l )

3. Compute dir-vectors for all sub-rays in a pulse

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.

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



Example: 1000 sub-raysl.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 sub-rays 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.

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]

so... Apply rotation of the mirror to all sub-rays.


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))


Raylist

will hold l.NumOfRays tuplesof 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 dir-vectors of all sub-rays. It s possible to cast them down and start to look for intersections.

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. Transmit a pulse, sub-ray by sub-ray

def FindClosest(n, case, Ray, S, T, TRJ): # Pulse-#, Sub-ray-#, Rays' dir vector (tuple), Surfaces, Trees, trajectory

i, j, k = Ray # Assign the values from the tuple to i,j,k DistMin = TRJ.z + 1E5 # Look for the minimum, candidate to large value Data = None # 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 - find scane object

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)

4. A sub-ray in a pulse - checking intersection

s

s

ray

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 (zt == treetop.z) xt, yt, zt = x + d_top * i, y + d_top * j, z + d_top * k # 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 around treetop 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 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)

SurfaceIntersect(n, case, TRJ.x, TRJ.y, TRJ.z, i,j,k, s)

If there is a hit, returns a list, with

[pulse_number, 0...duration*PRF ray-number, 0...NumOfRays-1 Type, A, B, C depending in plane orientation Surf.Refl, 0..1 of the surface t, Distance to the intersection point angle, Incidence angle of the pulse and the ray xt,yt,zt Point of intersection i,j,k] 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)

4. A sub-ray - checking intersection - return values

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

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.

4. Where are we now? - We are DONE!

What do we have now?

Functions are used for abstraction - hiding awayimplementation details. Classes are used for thattoo, although not as much.

Functions:

_3Dnorm((x,y,z))vector_angle(vectorA, vectorB)sign(number)DotProduct(vectorA, vectorB)InsidePolygon(VertexList, Point2D)readParameterFile("parameters.txt") readsurfaces("Scene.csv") readtrees("Trees.csv")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()

Classes:Point2DPoint3DBoundingBoxPolygonSurfaceTreeCampaignLiDARTrajectory

important storage variables:list with SurfacePolygon_objslist with Tree_objslist with Trajectory_objsidentifier with Campaign_objidentifier with LiDAR_objlist with observations, Echoes

INPUT-phase c, l= ReadParameterFile()S = ReadSurfaces()T = ReadTrees()

Create the trajectory 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 For each sub-ray: Hit = FindClosestObject():

Loop surfaces for a hit, keep track of closest Loop crowns and later trunks for a hit.

Store the Hit to a DataList After transmission, increase timeAfter Flying, write the DataList to a file

Raylist = Compute_SubRays ()

surfhitcand = SurfaceIntersect()

treehitcand = TreeIntersect()

VISUALIZE

Handling the large amount of code SimulatorLiDAR_Classes.py class definitionsLiDAR_Functions_IO.py file I/O functionsLiDAR_Functions_Intersect.py ray-target intersection functionsLiDAR_Functions_Misc.py Other functionsLiDAR_Main.py import statements and the main()

The py-files need the import statements, e.g. if a class is used in a functionin a code module, import LiDAR_Classes must be added in tha code module.numpy package must be installed (for class PolygonSurface).

input files Output filesparams.txt lidar.csv (data for all ray's with a hit, header row)scene.csvtrees.csv

Visualizationgraph.py jetColorMAP.csv (defines 64 nice colors (HSV-colors))

Visualizationgraph.py

lidar simulator marv216 project 2012 ilkka korpela tree crown inclined ground house

Documents