From 4fe916a23b756322392155a01187d5d533b2962a Mon Sep 17 00:00:00 2001
From: gabriel venberg <gabevenberg@gmail.com>
Date: Mon, 8 Mar 2021 20:38:31 -0600
Subject: [PATCH] old algorithm ran in O(a*b*c) time, where a is the number of
 points in the LIDAR file, b and c are the dimentions of the resulting
 heatmap.

new algorithm runs in O(a) time... before, it took ~5 days on my machine
to make a 1000*1000 heatmap. now it takes 5 seconds to make ANY size
image. (the graph making library takes a bit longer, but it is very
little compared to mapping the image array.)
---
 outputHightMap.py | 120 ++++++++++++++++++----------------------------
 1 file changed, 46 insertions(+), 74 deletions(-)

diff --git a/outputHightMap.py b/outputHightMap.py
index a085a0b..ab5837a 100755
--- a/outputHightMap.py
+++ b/outputHightMap.py
@@ -12,110 +12,82 @@ import seaborn as sns; sns.set_theme()
 import matplotlib.pyplot as plt
 from PIL import Image
 
-logging.basicConfig(format='%(asctime)s:%(levelname)s:%(message)s', level=logging.INFO)
+logging.basicConfig(format='%(asctime)s:%(message)s', level=logging.INFO)
+#logging.basicConfig(format='%(asctime)s:%(message)s', level=logging.DEBUG)
 
-def sort(array):
-    #sort by zDim column, first to last.
-    logging.debug(f'zDim sliced points is\n{array[:,zDim]}')
-    #the [::-1] reverses the resulting array, so that sortedPoints will be from biggest to smallest.
-    ind = np.argsort(array[:,zDim])[::-1]
-    sortedPoints = array[ind]
-    logging.debug(f'sortedPoints is\n{sortedPoints}')
-    return sortedPoints
+def scale(array, desiredmaxX, desiredmaxY):
+    logging.debug(f'xMax is {np.max(array[:,xDim])} and xMin is {np.min(array[:,xDim])}')
+    logging.debug(f'yMax is {np.max(array[:,yDim])} and yMin is {np.min(array[:,yDim])}')
+    ax=desiredmaxX/(np.max(array[:,xDim])-np.min(array[:,xDim]))
+    bx=-ax*np.min(array[:,xDim])
+    ay=desiredmaxY/(np.max(array[:,yDim])-np.min(array[:,yDim]))
+    by=-ay*np.min(array[:,yDim])
 
-def scale(array, xRange, yRange, maxX, maxY):
-    logging.debug(f'xRange is {xRange} and yRange is {yRange}')
-    xScale = maxX/xRange
-    yScale = maxY/yRange
 
-    scaledArray = sortedPoints[:, 0:3]
-    scaledArray[:,xDim]=scaledArray[:,xDim]-mins[xDim]
-    scaledArray[:,xDim]=scaledArray[:,xDim]*xScale
-    logging.debug(f'xmin in scaledArray is {scaledArray[:,xDim].min()}')
-    logging.debug(f'xmin in scaledArray is {scaledArray[:,xDim].max()}')
+    #slice indexes 0-2 from the second dimention
+    array[:,xDim]=ax*array[:,xDim]+bx
+    array[:,yDim]=ay*array[:,yDim]+by
 
-    scaledArray[:,yDim]=scaledArray[:,yDim]-mins[yDim]
-    scaledArray[:,yDim]=scaledArray[:,yDim]*yScale
-    logging.debug(f'ymin in scaledArray is {scaledArray[:,yDim].min()}')
-    logging.debug(f'ymin in scaledArray is {scaledArray[:,yDim].max()}')
-    logging.debug(f'scaledArray is\n{scaledArray}')
-    return scaledArray
+    logging.debug(f'array is\n{array}')
+    logging.debug(f'xMax is {np.max(array[:,xDim])} and xMin is {np.min(array[:,xDim])}')
+    logging.debug(f'yMax is {np.max(array[:,yDim])} and yMin is {np.min(array[:,yDim])}')
 
-def isInxyRange(xMin, xMax, yMin, yMax, xVal, yVal):
-    return (xMin<=xVal) and (xVal<xMax) and (yMin<=yVal) and (yVal<yMax)
+    return array
 
-imgX=1000
-imgY=1000
+imgX=500
+imgY=500
 
 #TODO: make it iterate over multiple files.
 inFile = os.path.realpath(sys.argv[1])
-lasFile = laspy.file.File(inFile, mode = "r")
+lasFile = laspy.file.File(inFile, mode = 'r')
 
 outFile = f'{os.path.dirname(inFile)}/{imgX}*{imgY}{os.path.basename(inFile)}.png'
 
 print(f'outputing to {outFile}')
 
 #import each dimention scaled.
+z = lasFile.z
 x = lasFile.x
 y = lasFile.y
-z = lasFile.z
-maxes = np.array(lasFile.header.max)*np.array(lasFile.header.scale)
-mins = np.array(lasFile.header.min)*np.array(lasFile.header.scale)
-logging.debug(f'max values is {maxes}')
-logging.debug(f'min values is {mins}')
 intensity = lasFile.intensity
 
-#dimention that will be z(top down) dimention in final heatmap. TODO: auto detect this based on dimention with least variance.
-zDim=0
-xDim=1
-yDim=2
+points = np.stack((z,x,y), axis=-1)
+
+#dimention that will be z(top down) dimention in final heatmap. TODO: auto detect this based on dimention with least variance.
+zDim=1
+xDim=2
+yDim=0
 
-points = np.stack((x,y,z,intensity), axis=-1)
 #points should now look like
-#[[x,y,z,intensity]
-# [x,y,z,intensity]
+#[[z,x,y]
+# [z,x,y]
 # ...
-# [x,y,z,intensity]
-# [x,y,z,intensity]]
+# [z,x,y]
+# [z,x,y]]
 
 logging.debug(f'points is\n{points}')
-print(f'{points.shape[0]} points in LIDAR file.')
-#found experimentally.
-print(f'estimated worst-case time is {points.shape[0]*7.77e-07*imgX*imgY} sec')
-
-xRange = maxes[xDim]-mins[xDim]
-yRange = maxes[yDim]-mins[yDim]
-zRange = maxes[zDim]-mins[zDim]
-
-sortedPoints = sort(points)
+length=points.shape[0]
+print(f'{length} points in LIDAR file.')
 
 imageArray = np.zeros((imgX, imgY))
 
-scaledArray = scale(points, xRange, yRange, imgX, imgY)
+points = scale(points, imgX, imgY)
 
-for x in range(imgX):
-    for y in range(imgY):
-        if x==imgX:
-            xMax=x+2
-        else:
-            xMax=x+1
-
-        if y==imgY:
-            yMax=y+2
-        else:
-            yMax=y+1
-
-        zVal=0
-        logging.debug(f'yMax is {yMax} and xMax is {xMax}')
-        for i in range(scaledArray.shape[0]):
-            if isInxyRange(x, xMax, y, yMax, scaledArray[i,xDim], scaledArray[i,yDim]):
-                zVal = scaledArray[i,zDim]
-                break;
-        imageArray[x,y]=zVal
-        print(f'zVal at {x},{y} is {zVal}')
+#sys.exit()
+#for each entry in points, figure out what pixel it will go into, and assign that pixel the zval, unless the zval already in that pixel is higher.
+for i in range(len(points)):
+    print(f'{i} points processed of {length} total points')
+    #the if statements are reqired for edge cases relateing to the bottom row and the far right column, to make sure points dont get left out.
+    xPixel=np.floor(points[i,xDim]).astype(int)
+    if xPixel==imgX:
+        xPixel-=1
+    yPixel=np.floor(points[i,yDim]).astype(int)
+    if yPixel==imgY:
+        yPixel-=1
+    imageArray[xPixel,yPixel]=np.maximum(imageArray[xPixel,yPixel], points[i,zDim])
 
 logging.debug(f'imageArray is {imageArray}')
 
-heatMap = sns.heatmap(imageArray, center=((maxes[zDim]+mins[zDim])/2), square=True)
+heatMap = sns.heatmap(imageArray, center=(np.max(imageArray)+np.min(imageArray))/2, robust=True, square=True)
 heatMapFig = heatMap.get_figure()
 heatMapFig.savefig(outFile)