Convert GPS logging data to terrain mesh

nopaste

#!/usr/bin/env python3

import bpy
import csv
from math import *
import statistics as st
import os
import glob
from pprint import pprint

def dotprod(list1,list2):
    dotsum = 0
    for x,y in zip(list1,list2):
        dotsum += x * y
    return dotsum

def stripextra(vertlist):
    verts = []
    for x, y, z, *r in vertlist:
        verts.append((x, y, z))
    return verts

def midpoint(tuplist):
    if len(tuplist) == 1:
        return tuplist[0]
    
    sz = len(tuplist[0])
    itemlist = [[] for x in range(0,sz)]
    for t in tuplist:
        for i in range(0,sz):
            itemlist[i].append(t[i])
    means = []
    for i in range(0, sz):
        means.append(st.mean(itemlist[i]))
    return tuple(means)

def getmin(rawdata):
    minvert = [None, None, None]
    for fname in rawdata.keys():
        for vert in rawdata[fname]:
            for i in range(0,3):
                if minvert[i] == None or minvert[i] > vert[i]:
                    minvert[i] = vert[i]
    return minvert

def smoothout(verts):
    xwin, ywin, zwin = [], [], []
    weights = [1,2,4,5,4,2,1]
    wsum = fsum(weights)
    smoothverts = []            
    for x, y, z, *r in verts:
        xwin.append(x)
        ywin.append(y)
        zwin.append(z)
        if len(xwin) < 7:
            continue
        # Limit lists to 9 items
        xwin, ywin, zwin = xwin[-7:], ywin[-7:], zwin[-7:]
        rec = [dotprod(xwin, weights)/wsum, dotprod(ywin, weights)/wsum, dotprod(zwin, weights)/wsum]
        rec.extend(r)
        smoothverts.append(tuple(rec))
    return smoothverts

def fillempty(celllists, rx, ry, radius=8, btype="flat"):
    # Only fill empty cells
    if len(celllists[rx][ry]):
        return

    cols = len(celllists)
    rows = len(celllists[0])
    bounds = []
    lh = dh = rh = uh = 0.0
    ldx = ddy = rdx = udy = 0
    # Search left for populated cells
    if rx > 0:
        for ldx in range(rx-1,-1,-1):
            if len(celllists[ldx][ry]):
                lh = findheight(celllists, ldx, ry, radius, btype)
                bounds.append((rx - ldx, lh))
                break
    # Search right
    if rx + 1 < cols - 1:
        for rdx in range(rx+1, cols):
            if len(celllists[rdx][ry]):
                rh = findheight(celllists, rdx, ry, radius, btype)
                bounds.append((rdx - rx, rh))
                break
    # Search down
    if ry > 0:
        for ddy in range(ry-1,-1,-1):
            if len(celllists[rx][ddy]):
                dh = findheight(celllists, rx, ddy, radius, btype)
                bounds.append((ry - ddy, dh))
                break
    # Search up
    if ry + 1 < rows - 1:
        for udy in range(ry+1, rows):
            if len(celllists[rx][udy]):
                uh = findheight(celllists, rx, udy, radius, btype)
                bounds.append((udy - ry, uh))
                break
    # Calculate weighted mean
    w = tz = 0
    for dst, bz in bounds:
        w += 1.0/dst**2
        tz += bz/dst**2
        
    cz = tz/w if w else 0
    # Add single record
    celllists[rx][ry].append((rx+0.5, ry+0.5, cz, 1))

def makebrush(radius,btype="linear"):
    brush = None
    if btype == "flat":
        brush = [[ 0 if (n := ((x-radius+1)**2 + (y-radius+1)**2)**.5) > (radius - .58) else 1.0 for x in range(0,radius*2-1)] for y in range(0,radius*2-1)]
    elif btype == "linear":
        brush = [[ 0 if (n := ((x-radius+1)**2 + (y-radius+1)**2)**.5) > (radius - .58) else (radius - n)/radius for x in range(0,radius*2-1)] for y in range(0,radius*2-1)]
    elif btype == "circular":
        brush = [[ 0 if (n := ((x-radius+1)**2 + (y-radius+1)**2)**.5) > (radius - .58) else ((radius**2 - n**2)/radius**2)**.5 for x in range(0,radius*2-1)] for y in range(0,radius*2-1)]
    return brush

def findheight(celllists, rx, ry, radius = 5, btype="linear"):
    cols = len(celllists)
    rows = len(celllists[0])
    brush = makebrush(radius,btype)
    if brush == None:
        return 0.0
    wsum = 0
    tz = 0
    for xo in range(1-radius,radius):
        for yo in range(1-radius,radius):
            if rx + xo < 0 or rx + xo > cols - 1 or ry + yo < 0 or ry + yo > rows - 1 or not brush[xo+radius-1][yo+radius-1]:
                continue
            if not len(celllists[rx+xo][ry+yo]):
                continue
            pz = 0
            pw = 0
            for cl in celllists[rx+xo][ry+yo]:
                iw = 1.0/cl[3]**2
                pz += cl[2] * iw
                pw += iw
            pz = pz / pw
            w = brush[xo+radius-1][yo+radius-1]
            tz += pz * w 
            wsum += w
    if not wsum:
        # print("No data points found for {},{}".format(rx,ry))
        return 0.0
    return tz/wsum

        
def createterrain(allverts, xmax, ymax, bsize=5):
    cols, rows = floor(xmax), floor(ymax)
    celllists = [[[] for gy in range(0, rows+1)] for gx in range(0, cols+1)]
    # Fill cells lists with GPS records
    for fname in allverts.keys():
        for v in allverts[fname]:
            # Stop any bleedover
            xdx, ydx = floor(v[0]) if floor(v[0])>0 else 0, floor(v[1]) if floor(v[1])>0 else 0
            celllists[xdx][ydx].append(v)
    # Fill empty cells with at least one record
    # Start somewhere close to center
    cx, cy = ceil(cols/2), ceil(rows/2)
    fillempty(celllists,cx,cy)
    # Work you way to the edges, alternating from side to side,  up and down
    for lp in range(1, max(cx,cy) + 1):
        for cp in range(0, 2*lp):
            offset = ceil(cp/2)*(1 if cp%2 else -1)
            if cx >= lp and cy >= offset and cy - offset < rows:
                fillempty(celllists, cx-lp, cy-offset)
            if cx + lp < cols and cy + offset < rows and cy + offset >= 0:
                fillempty(celllists, cx+lp, cy+offset)
            if cx >= offset and cx - offset < cols and cy >= lp:
                fillempty(celllists, cx-offset, cy-lp)
            if cx + offset < cols and cx + offset >= 0 and cy + lp < rows:
                fillempty(celllists, cx+offset, cy+lp)

    verts = []
    edges = []
    faces = []
    for rx in range(0, cols):
        for ry in range(0, rows):
            ndx = rx*rows + ry
            verts.append((rx+0.5, ry+0.5, findheight(celllists,rx,ry, bsize)))
            if rx > 0:
                edges.append((ndx-rows,ndx))
            if ry > 0:
                edges.append((ndx-1,ndx))
            if rx > 0 and ry > 0:
                edges.append((ndx-rows-1,ndx))
                faces.append((ndx-rows-1,ndx-rows,ndx))
                faces.append((ndx-rows-1,ndx,ndx-1))
    return verts, edges, faces

def addterrainmesh(verts, edges, faces, mname="Terrain"):
    col = bpy.data.collections.get(mname+" Collection")
    if not col:
        col = bpy.data.collections.new(mname+" Collection")
        bpy.context.scene.collection.children.link(col)

    mesh = bpy.data.meshes.new("Mesh-"+mname)
    mesh.from_pydata(verts,edges,faces)
    mesh.update()
    obj = bpy.data.objects.new("Obj-"+mname, mesh)
    col.objects.link(obj)
    
def addpathmesh(col, verts, mname):
    verts = stripextra(verts)
    edges = [(x,x+1) for x in range(0, len(verts)-1)]
    faces = []
    mesh  = bpy.data.meshes.new("Mesh-" + mname)
    mesh.from_pydata(verts, edges, faces)
    mesh.update()
    obj   = bpy.data.objects.new("Obj-" + mname, mesh)
    col.objects.link(obj)
    bpy.context.view_layer.objects.active = obj
   
def createpaths(allverts):
    col = bpy.data.collections.get("Path Collection")
    if not col:
        col = bpy.data.collections.new("Path Collection")
        bpy.context.scene.collection.children.link(col)

    smoothverts = {}
    for mname in allverts.keys():
        if not len(allverts[mname]):
            print("Empty vertex list")
            continue
        smoothverts[mname] = smoothout(allverts[mname])
        addpathmesh(col,smoothverts[mname],mname)
    return smoothverts
   
def convgps(rawdata):
    # Calculate lengths relative to minimum bounds
    latmin, lonmin, altmin = getmin(rawdata)
    allverts = {}
    xmax, ymax = None, None
    for fname in rawdata.keys():
        allverts[fname] = []
        restpoint = None
        for lat, lon, alt, acc, speed in rawdata[fname]:
            # Convert location to meters
            x, y, z = 111139.0 * cos(radians(lat)) * (lon - lonmin), 111139.0 * (lat - latmin), alt - altmin
            if xmax == None or xmax < x:
                xmax = x
            if ymax == None or ymax < y:
                ymax = y

            # Phone knows when it's not moving. Use this time to calc a better point
            if speed < 0.001:
                # Bag down, good place to calc variance if there was any use for that
                if restpoint == None:
                    restpoint = []
                restpoint.append((x, y, z, acc))
                continue
            
            if restpoint != None:
                rp = midpoint(restpoint)
                restpoint = None
                allverts[fname].append(rp)
            
            allverts[fname].append((x, y, z, acc))
    return allverts, xmax, ymax

def readgps(gpsfile):
    gpsdata = []
    with open(gpsfile) as csvfile:
        content = csv.reader(csvfile, dialect="excel", delimiter=",")
        for i, row in enumerate(content):
            if i == 0: # skip header
                continue
            lat, lon, alt = float(row[2]), float(row[3]), float(row[5]) + float(row[6])
            acc, speed = float(row[4]), float(row[7])
            gpsdata.append((lat, lon, alt, acc, speed))
    return gpsdata

if __name__ == '__main__':
    path = "/devel/blender/gps"
    gpsinput = {}
    # Read all data before setting bounds
    for tfile in glob.glob(path+"/*.txt"):
        pname = os.path.basename(tfile)[:-4]
        gpsinput[pname] = readgps(tfile)

    # Calc boundries, filter out overkill
    allverts, xmax, ymax = convgps(gpsinput)
    # Create each path walked    
    smoothverts = createpaths(allverts)
    addterrainmesh(*createterrain(allverts,xmax,ymax,8))
    # addterrainmesh(*createterrain(smoothverts,xmax,ymax),"Smooth")
    # createterrain(smoothverts,xmax,ymax,name="Smooth")