new repo version

.gitignore

+venv
+LP model
+waypoints.json
+terrain_mapped.mp4
+terrain_2_gif.gif
+terrain_(-200.0, 200.0)(-200.0, 200.0)(0, 70)10.npy
+risk-aware-path.pdf
+rgp_bezier
+e_x_y_data.pkl
+.pytest_cache
+z__test__
\ No newline at end of file

.gitmodules

+[submodule "pyboostrtree"]
+	path = pyboostrtree
+	url = https://github.com/gavincangan/pyboostrtree.git

.idea/.gitignore

+# Default ignored files
+/workspace.xml

.idea/inspectionProfiles/profiles_settings.xml

+<component name="InspectionProjectProfileManager">
+  <settings>
+    <option name="PROJECT_PROFILE" value="Default" />
+    <option name="USE_PROJECT_PROFILE" value="false" />
+    <version value="1.0" />
+  </settings>
+</component>
\ No newline at end of file

.idea/misc.xml

+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="JavaScriptSettings">
+    <option name="languageLevel" value="ES6" />
+  </component>
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.6 (test_env)" project-jdk-type="Python SDK" />
+</project>
\ No newline at end of file

.idea/modules.xml

+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/planning_llh_bgc.iml" filepath="$PROJECT_DIR$/.idea/planning_llh_bgc.iml" />
+    </modules>
+  </component>
+</project>
\ No newline at end of file

.idea/other.xml

+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="PySciProjectComponent">
+    <option name="PY_SCI_VIEW" value="true" />
+    <option name="PY_SCI_VIEW_SUGGESTED" value="true" />
+  </component>
+</project>
\ No newline at end of file

.idea/planning_llh_bgc.iml

+<?xml version="1.0" encoding="UTF-8"?>
+<module type="PYTHON_MODULE" version="4">
+  <component name="NewModuleRootManager">
+    <content url="file://$MODULE_DIR$">
+      <excludeFolder url="file://$MODULE_DIR$/venv_laptop" />
+    </content>
+    <orderEntry type="jdk" jdkName="Python 3.6 (test_env)" jdkType="Python SDK" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+  <component name="PyDocumentationSettings">
+    <option name="renderExternalDocumentation" value="true" />
+  </component>
+</module>
\ No newline at end of file

.idea/vcs.xml

+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="$PROJECT_DIR$/pyboostrtree" vcs="Git" />
+  </component>
+</project>
\ No newline at end of file

README.md

+# planning_llh_bgc
+

bezier_interp/__init__.py

+__version__ = "0.0.1"
\ No newline at end of file

bezier_interp/__main__.py

+""" Tkinter demo gui for bezier fitting algorithm
+
+     (c) Volker Poplawski 2014
+"""
+import numpy as np
+from . import bezier
+from .fitCurves import fitCurve
+try:
+    import tkinter as tk
+except ImportError:
+    import Tkinter as tk
+
+
+# center of bounding box
+def cntr(x1, y1, x2, y2):
+    return x1+(x2-x1)/2, y1+(y2-y1)/2
+
+
+# tkinter Canvas plus some addons
+class MyCanvas(tk.Canvas):
+    def create_polyline(self, points, **kwargs):
+        for p1, p2 in zip(points, points[1:]):
+            self.create_line(p1, p2, kwargs)
+
+
+    def create_bezier(self, b, tag):
+        self.create_polyline([bezier.q(b, t/50.0).tolist() for t in range(0, 51)], tag=tag, fill='blue', width='2') # there are better ways to draw a bezier
+        self.create_line(b[0].tolist(), b[1].tolist(), tag=tag)
+        self.create_point(b[1][0], b[1][1], 2, fill='black', tag=tag)
+        self.create_line(b[3].tolist(), b[2].tolist(), tag=tag)
+        self.create_point(b[2][0], b[2][1], 2, fill='black', tag=tag)
+
+
+    def create_point(self, x, y, r, **kwargs):
+        return self.create_oval(x-r, y-r, x+r, y+r, kwargs)
+
+
+    def pos(self, idOrTag):
+        return cntr(*self.coords(idOrTag))
+
+
+    def itemsAtPos(self, x, y, tag):
+        return [item for item in self.find_overlapping(x, y, x, y) if tag in self.gettags(item)]
+
+
+class MainObject:
+    def run(self):
+        root = tk.Tk()
+
+        self.canvas = MyCanvas(root, bg='white', width=400, height=400)
+        self.canvas.pack(side=tk.LEFT)
+
+        frame = tk.Frame(root, relief=tk.SUNKEN, borderwidth=1)
+        frame.pack(side=tk.LEFT, fill=tk.Y)
+        label = tk.Label(frame, text='Max Error')
+        label.pack()
+        self.spinbox = tk.Spinbox(frame, width=8, from_=0.0, to=1000000.0, command=self.onSpinBoxValueChange)
+        self.spinbox.insert(0, 10.0)
+        self.spinbox.pack()
+
+        self.points = []
+        self.draggingPoint = None
+
+        self.canvas.bind('<ButtonPress-1>', self.onButton1Press)
+        self.canvas.bind('<ButtonPress-2>', self.onButton2Press)
+        self.canvas.bind('<B1-Motion>', self.onMouseMove)
+        self.canvas.bind('<ButtonRelease-1>', self.onButton1Release)
+
+        root.mainloop()
+
+
+    def onButton1Press(self, event):
+        items = self.canvas.itemsAtPos(event.x, event.y, 'point')
+        if items:
+            self.draggingPoint = items[0]
+        else:
+            self.points.append(self.canvas.create_point(event.x, event.y, 4, fill='red', tag='point'))
+            self.redraw()
+
+
+    def onButton2Press(self, event):
+        self.canvas.delete(self.points.pop())
+        self.redraw()
+
+
+    def onMouseMove(self, event):
+        if self.draggingPoint:
+            self.canvas.coords(self.draggingPoint, event.x-4, event.y-4, event.x+4, event.y+4)
+            self.redraw()
+
+
+    def onButton1Release(self, event):
+        self.draggingPoint = None
+
+
+    def onSpinBoxValueChange(self):
+        self.redraw()
+
+
+    def redraw(self):
+        # redraw polyline
+        self.canvas.delete('polyline')
+        self.canvas.create_polyline([self.canvas.pos(pId) for pId in self.points], fill='grey', tag='polyline')
+        self.canvas.tag_lower('polyline')
+
+        # redraw bezier
+        if len(self.points) < 2:
+            return
+
+        self.canvas.delete('bezier')
+        points = np.array([self.canvas.pos(p) for p in self.points])
+        beziers = fitCurve(points, float(self.spinbox.get())**2)
+        for bezier in beziers:
+            self.canvas.create_bezier(bezier, tag='bezier')
+
+
+if __name__ == '__main__':
+    o = MainObject()
+    o.run()
+
+

bezier_interp/bezier.py

+import numpy as np
+
+
+# evaluates cubic bezier at t, return point
+def q(ctrlPoly, t):
+    return (1.0-t)**3 * ctrlPoly[0] + 3*(1.0-t)**2 * t * ctrlPoly[1] + 3*(1.0-t)* t**2 * ctrlPoly[2] + t**3 * ctrlPoly[3]
+
+
+# evaluates cubic bezier first derivative at t, return point
+def qprime(ctrlPoly, t):
+    return 3*(1.0-t)**2 * (ctrlPoly[1]-ctrlPoly[0]) + 6*(1.0-t) * t * (ctrlPoly[2]-ctrlPoly[1]) + 3*t**2 * (ctrlPoly[3]-ctrlPoly[2])
+
+
+# evaluates cubic bezier second derivative at t, return point
+def qprimeprime(ctrlPoly, t):
+    return 6*(1.0-t) * (ctrlPoly[2]-2*ctrlPoly[1]+ctrlPoly[0]) + 6*(t) * (ctrlPoly[3]-2*ctrlPoly[2]+ctrlPoly[1])
+
+
+# approx qint(0,1) = 12 (a^2 + 3 (b^2 - b c + c^2) - 3 c d + d^2 + a (-3 b + d))
+# https://raphlinus.github.io/curves/2018/12/28/bezier-arclength.html
+def qint_approx(ctrlPoly):
+    return ( 12 *( ctrlPoly[0]**2 \
+                + 3 *(ctrlPoly[1]**2 - ctrlPoly[1]*ctrlPoly[2] + ctrlPoly[2]**2)
+                - 3 * ctrlPoly[2] * ctrlPoly[3] + ctrlPoly[3]**2 
+                + ctrlPoly[0] * (-3 *ctrlPoly[1] + ctrlPoly[3]) ) ).sum()
+
+def qint_multi(ctrlPolyMulti):
+    _num = len(ctrlPolyMulti)
+    return sum([ qint_approx(ctrlPolyMulti[_ix:_ix+4]) for  _ix in range(0, _num-1, 3) ])
+
+    # _values = []
+    # for _ix in range(0, _num, 4):
+    #     _ctrlPoly = ctrlPolyMulti[_ix:_ix+4]
+    #     _values.append( qint(_ctrlPoly) )
+    # return sum(_values)
\ No newline at end of file

bezier_interp/fitCurves.py

+""" Python implementation of
+    Algorithm for Automatically Fitting Digitized Curves
+    by Philip J. Schneider
+    "Graphics Gems", Academic Press, 1990
+"""
+import numpy as np
+from . import bezier
+
+# Fit one (ore more) Bezier curves to a set of points
+def fitCurve(points, maxError):
+    leftTangent = normalize(points[1] - points[0])
+    rightTangent = normalize(points[-2] - points[-1])
+    return fitCubic(points, leftTangent, rightTangent, maxError, _dim=points[0].shape[-1])
+
+
+def fitCubic(points, leftTangent, rightTangent, error, _dim=2):
+    # Use heuristic if region only has two points in it
+    if (len(points) == 2):
+        dist = np.linalg.norm(points[0] - points[1]) / 3.0
+        bezCurve = [points[0], points[0] + leftTangent * dist, points[1] + rightTangent * dist, points[1]]
+        return [bezCurve]
+
+    # Parameterize points, and attempt to fit curve
+    u = chordLengthParameterize(points)
+    bezCurve = generateBezier(points, u, leftTangent, rightTangent, _dim=_dim)
+    # Find max deviation of points to fitted curve
+    maxError, splitPoint = computeMaxError(points, bezCurve, u)
+    if maxError < error:
+        return [bezCurve]
+
+    # If error not too large, try some reparameterization and iteration
+    if maxError < error**2:
+        for _ in range(20):
+            uPrime = reparameterize(bezCurve, points, u)
+            bezCurve = generateBezier(points, uPrime, leftTangent, rightTangent, _dim=_dim)
+            maxError, splitPoint = computeMaxError(points, bezCurve, uPrime)
+            if maxError < error:
+                return [bezCurve]
+            u = uPrime
+
+    # Fitting failed -- split at max error point and fit recursively
+    beziers = []
+    centerTangent = normalize(points[splitPoint-1] - points[splitPoint+1])
+    beziers += fitCubic(points[:splitPoint+1], leftTangent, centerTangent, error, _dim=_dim)
+    beziers += fitCubic(points[splitPoint:], -centerTangent, rightTangent, error, _dim=_dim)
+
+    return beziers
+
+
+def generateBezier(points, parameters, leftTangent, rightTangent, _dim=2):
+    bezCurve = [points[0], None, None, points[-1]]
+    # compute the A's
+    A = np.zeros((len(parameters), 2, _dim))
+    for i, u in enumerate(parameters):
+        A[i][0] = leftTangent  * 3*(1-u)**2 * u
+        A[i][1] = rightTangent * 3*(1-u)    * u**2
+
+    # Create the C and X matrices
+    C = np.zeros((2, 2))
+    X = np.zeros(2)
+
+    for i, (point, u) in enumerate(zip(points, parameters)):
+        C[0][0] += np.dot(A[i][0], A[i][0])
+        C[0][1] += np.dot(A[i][0], A[i][1])
+        C[1][0] += np.dot(A[i][0], A[i][1])
+        C[1][1] += np.dot(A[i][1], A[i][1])
+
+        tmp = point - bezier.q([points[0], points[0], points[-1], points[-1]], u)
+
+        X[0] += np.dot(A[i][0], tmp)
+        X[1] += np.dot(A[i][1], tmp)
+
+    # Compute the determinants of C and X
+    det_C0_C1 = C[0][0] * C[1][1] - C[1][0] * C[0][1]
+    det_C0_X  = C[0][0] * X[1] - C[1][0] * X[0]
+    det_X_C1  = X[0] * C[1][1] - X[1] * C[0][1]
+
+    # Finally, derive alpha values
+    alpha_l = 0.0 if det_C0_C1 == 0 else det_X_C1 / det_C0_C1
+    alpha_r = 0.0 if det_C0_C1 == 0 else det_C0_X / det_C0_C1
+
+    # If alpha negative, use the Wu/Barsky heuristic (see text) */
+    # (if alpha is 0, you get coincident control points that lead to
+    # divide by zero in any subsequent NewtonRaphsonRootFind() call. */
+    segLength = np.linalg.norm(points[0] - points[-1])
+    epsilon = 1.0e-6 * segLength
+    if alpha_l < epsilon or alpha_r < epsilon:
+        # fall back on standard (probably inaccurate) formula, and subdivide further if needed.
+        bezCurve[1] = bezCurve[0] + leftTangent * (segLength / 3.0)
+        bezCurve[2] = bezCurve[3] + rightTangent * (segLength / 3.0)
+
+    else:
+        # First and last control points of the Bezier curve are
+        # positioned exactly at the first and last data points
+        # Control points 1 and 2 are positioned an alpha distance out
+        # on the tangent vectors, left and right, respectively
+        bezCurve[1] = bezCurve[0] + leftTangent * alpha_l
+        bezCurve[2] = bezCurve[3] + rightTangent * alpha_r
+
+    return bezCurve
+
+
+def reparameterize(bezier, points, parameters):
+    return [newtonRaphsonRootFind(bezier, point, u) for point, u in zip(points, parameters)]
+
+
+def newtonRaphsonRootFind(bez, point, u):
+    """
+       Newton's root finding algorithm calculates f(x)=0 by reiterating
+       x_n+1 = x_n - f(x_n)/f'(x_n)
+
+       We are trying to find curve parameter u for some point p that minimizes
+       the distance from that point to the curve. Distance point to curve is d=q(u)-p.
+       At minimum distance the point is perpendicular to the curve.
+       We are solving
+       f = q(u)-p * q'(u) = 0
+       with
+       f' = q'(u) * q'(u) + q(u)-p * q''(u)
+
+       gives
+       u_n+1 = u_n - |q(u_n)-p * q'(u_n)| / |q'(u_n)**2 + q(u_n)-p * q''(u_n)|
+    """
+    d = bezier.q(bez, u)-point
+    numerator = (d * bezier.qprime(bez, u)).sum()
+    denominator = (bezier.qprime(bez, u)**2 + d * bezier.qprimeprime(bez, u)).sum()
+
+    if denominator == 0.0:
+        return u
+    else:
+        return u - numerator/denominator
+
+
+def chordLengthParameterize(points):
+    u = [0.0]
+    for i in range(1, len(points)):
+        u.append(u[i-1] + np.linalg.norm(points[i] - points[i-1]))
+
+    for i, _ in enumerate(u):
+        u[i] = u[i] / u[-1]
+
+    return u
+
+
+def computeMaxError(points, bez, parameters):
+    maxDist = 0.0
+    splitPoint = len(points)/2
+    for i, (point, u) in enumerate(zip(points, parameters)):
+        dist = np.linalg.norm(bezier.q(bez, u)-point)**2
+        if dist > maxDist:
+            maxDist = dist
+            splitPoint = i
+
+    return maxDist, splitPoint
+
+
+def normalize(v):
+    return v / np.linalg.norm(v)
+

cubic_interp/__init__.py

+__version__ = "0.0.1"
\ No newline at end of file

cubic_interp/cubic.py

+import numpy as np
+
+
+# evaluates cubic bezier at x, return point
+# tcPoly --> (xk, a, b, c, d)
+def q(tcPoly, x):
+    # q = a[i]+dx*(b[i]+dx*(c[i]+dx*d[i]))
+    dx = x-tcPoly[0] 
+    return tcPoly[1] + dx*(tcPoly[2] + dx*(tcPoly[3] + dx*tcPoly[4]))
+
+
+# evaluates cubic bezier first derivative at x, return point
+# tcPoly --> (xk, a, b, c, d)
+def qprime(tcPoly, x):
+    # dq = b[i]+dx*(2.*c[i]+dx*3.*d[i])
+    dx = x-tcPoly[0] 
+    return tcPoly[2] + dx*(2.*tcPoly[3] + dx*3.*tcPoly[4])
+
+
+# evaluates cubic bezier second derivative at x, return point
+# tcPoly --> (xk, a, b, c, d)
+def qprimeprime(tcPoly, x):
+    # ddq = 2.*c[i]+6.*d[i]*(x-xk[i])
+    dx = x-tcPoly[0] 
+    return 2.*tcPoly[3] + dx*6.*tcPoly[4]
+
+
+# cPolyAll --> [ (xk, a, b, c, d), [..], .. n rows ]
+# _tcPoly --> (xk, a, b, c, d)
+def qint(cPolyAll):
+    _values = [ _qintvalue(cPolyAll[i], cPolyAll[i+1, 0]-cPolyAll[i, 0]) for i in range(cPolyAll.shape[0]-1) ]
+    return sum(_values)
+
+# evaluate the integral of q at x (note there's no (x-xk) here!)
+# tcPoly --> (xk, a, b, c, d)
+def _qintvalue(tcPoly, x):
+    return x*(tcPoly[1] + x*(0.5*tcPoly[2] + x*(1./3.*tcPoly[3] + 0.25*tcPoly[4]*x)))
\ No newline at end of file

cubic_interp/fitSplines.py

+""" Python implementation of
+    Algorithm for Automatically Fitting Digitized Curves
+    by Philip J. Schneider
+    "Graphics Gems", Academic Press, 1990
+"""
+import numpy as np
+from . import cubic
+
+import pdb
+
+# Fit one (ore more) Bezier curves to a set of points
+def fitCurve(points, maxError):
+    _y = points
+    _nints = _y.shape[0]-1
+    _u = chordLengthParameterize( _y )
+    _h = np.array( [ _u[i+1]-_u[i] for i in range(_nints) ] )
+
+    return fitCubic(_y, _u, _h, _nints, maxError, _dim=points[0].shape[-1])
+
+def _abcd(_y, _h, _d2y):
+    _a = _y[:len(_h)]
+    _b = np.array( [ (_y[i+1]-_y[i])/_h[i] - (2.*_d2y[i]+_d2y[i+1])*_h[i]/6. for i in range(len(_h)) ] )
+    _c = _d2y[:len(_h)]*0.5
+    _d = np.array( [ (_d2y[i+1]-_d2y[i])/_h[i]/6. for i in range(len(_h)) ] )
+    return (_a, _b, _c, _d)
+
+def natural(_y, _h, _d2y0=0.0, _d2yn=0.0, _dim=1):
+    _n = len(_h)
+    _A = np.zeros((_n-1, _n-1))
+    _b = np.zeros((_n-1,))
+    for ix in range(_n-1):
+        _A[ix,ix] = 2*(_h[ix]+_h[ix+1])
+        if ix>0:    _A[ix,ix-1] = _h[ix]
+        if ix<_n-2: _A[ix,ix+1] = _h[ix+1]
+        _b[ix] = 6*( (_y[ix+2]-_y[ix+1])/_h[ix+1] - (_y[ix+1]-_y[ix])/_h[ix] )
+    _b[0]  -= _h[0]  *_d2y0
+    _b[-1] -= _h[_n-1]*_d2yn
+    _z = np.linalg.solve(_A, _b)
+
+    _d2y = np.insert(_z,(0,len(_z)),(_d2y0,_d2yn))
+    return _abcd(_y, _h, _d2y)
+
+# clamped spline boundary conditions (guessed from data)
+def clamped(_y, _u, _h, _dim=1):
+    _n = len(_h)
+    _dy0 = (_y[1]-_y[0])/(_u[1]-_u[0])
+    _dyn = (_y[-1]-_y[-2])/(_u[-1]-_u[-2])
+
+    _A = np.zeros((_n-1, _n-1))
+    _b = np.zeros((_n-1,))
+
+    _A[0,:2]   = 1.5*_h[0]+2.*_h[1], _h[1]
+    _A[-1,-2:] = _h[_n-2],            2.*_h[_n-2]+1.5*_h[_n-1]
+    _b[0] = 6.*( (_y[2]-_y[1]) /_h[1] - (_y[1]-_y[0])  /_h[0] ) - 3.*( (_y[1]-_y[0])/_h[0] - _dy0)
+    _b[-1] = 6.*( (_y[_n]-_y[_n-1])/_h[_n-1] - (_y[_n-1]-_y[_n-2])/_h[_n-2] ) - 3.*( _dyn - (_y[_n]-_y[_n-1])/_h[_n-1] )
+
+    for ix in range(1,_n-2):
+        _A[ix,ix-1:ix+2] = _h[ix], 2.*(_h[ix]+_h[ix+1]), _h[ix+1]
+        _b[ix] = 6.*( (_y[ix+2]-_y[ix+1])/_h[ix+1] - (_y[ix+1]-_y[ix])/_h[ix] )
+    _z = np.linalg.solve(_A, _b)
+
+    _d2y0 = 3./_h[0]*  ( (_y[1]-_y[0])/_h[0] - _dy0 ) - 0.5*_z[0]
+    _d2yn = 3./_h[_n-1]*( _dyn - (_y[_n]-_y[_n-1])/_h[_n-1] )
+    _d2y = np.insert(_z,(0,len(_z)),(_d2y0,_d2yn))
+    return _abcd(_y, _h, _d2y)
+
+def notaknot(_y, _h, _dim=1):
+    _n = len(_h)
+    _A = np.zeros((_n-1, _n-1))
+    _b = np.zeros((_n-1,))
+
+    _A[0,:2]   = 3.*_h[0]+2.*_h[1]+_h[0]*_h[0]/_h[1], _h[1]-_h[0]*_h[0]/_h[1]
+    _A[-1,-2:] = _h[_n-2]-_h[_n-1]*_h[_n-1]/_h[_n-2],     2.*_h[_n-2]+3.*_h[_n-1]+_h[_n-1]*_h[_n-1]/_h[_n-2]
+    for i in range(_n-1):
+        if i>0 and i<_n-2:
+            _A[i,i-1:i+2] = _h[i], 2.*(_h[i]+_h[i+1]), _h[i+1]
+        _b[i] = 6.*( (_y[i+2]-_y[i+1])/_h[i+1] - (_y[i+1]-_y[i])/_h[i] )
+    _z = np.linalg.solve(_A,_b)
+
+    _d2y0 = _z[0]  - _h[0]/_h[1]*    ( _z[1]-_z[0] )
+    _d2yn = _z[-1] + _h[_n-1]/_h[_n-2]*( _z[-1]-_z[-2] )
+    _d2y = np.insert(_z,(0,len(_z)),(_d2y0,_d2yn))
+    return _abcd(_y, _h, _d2y)
+
+def fitCubic(_y, _u, _h, _nints, error, _dim=2):
+    points = _y
+
+    bezCurve = generateBezier(points, u, leftTangent, rightTangent, _dim=_dim)
+    # Find max deviation of points to fitted curve
+    maxError, splitPoint = computeMaxError(points, bezCurve, u)
+    if maxError < error:
+        return [bezCurve]
+
+    # If error not too large, try some reparameterization and iteration
+    if maxError < error**2:
+        for _ in range(20):
+            uPrime = reparameterize(bezCurve, points, u)
+            bezCurve = generateBezier(points, uPrime, leftTangent, rightTangent, _dim=_dim)
+            maxError, splitPoint = computeMaxError(points, bezCurve, uPrime)
+            if maxError < error:
+                return [bezCurve]
+            u = uPrime
+
+    # Fitting failed -- split at max error point and fit recursively
+    beziers = []
+    centerTangent = normalize(points[splitPoint-1] - points[splitPoint+1])
+    beziers += fitCubic(points[:splitPoint+1], leftTangent, centerTangent, error, _dim=_dim)
+    beziers += fitCubic(points[splitPoint:], -centerTangent, rightTangent, error, _dim=_dim)
+
+    return beziers
+
+
+def generateBezier(points, parameters, leftTangent, rightTangent, _dim=2):
+    bezCurve = [points[0], None, None, points[-1]]
+    # compute the A's
+    A = np.zeros((len(parameters), 2, _dim))
+    for i, u in enumerate(parameters):
+        A[i][0] = leftTangent  * 3*(1-u)**2 * u
+        A[i][1] = rightTangent * 3*(1-u)    * u**2
+
+    # Create the C and X matrices
+    C = np.zeros((2, 2))
+    X = np.zeros(2)
+
+    for i, (point, u) in enumerate(zip(points, parameters)):
+        C[0][0] += np.dot(A[i][0], A[i][0])
+        C[0][1] += np.dot(A[i][0], A[i][1])
+        C[1][0] += np.dot(A[i][0], A[i][1])
+        C[1][1] += np.dot(A[i][1], A[i][1])
+
+        tmp = point - bezier.q([points[0], points[0], points[-1], points[-1]], u)
+
+        X[0] += np.dot(A[i][0], tmp)
+        X[1] += np.dot(A[i][1], tmp)
+
+    # Compute the determinants of C and X
+    det_C0_C1 = C[0][0] * C[1][1] - C[1][0] * C[0][1]
+    det_C0_X  = C[0][0] * X[1] - C[1][0] * X[0]
+    det_X_C1  = X[0] * C[1][1] - X[1] * C[0][1]
+
+    # Finally, derive alpha values
+    alpha_l = 0.0 if det_C0_C1 == 0 else det_X_C1 / det_C0_C1
+    alpha_r = 0.0 if det_C0_C1 == 0 else det_C0_X / det_C0_C1
+
+    # If alpha negative, use the Wu/Barsky heuristic (see text) */
+    # (if alpha is 0, you get coincident control points that lead to
+    # divide by zero in any subsequent NewtonRaphsonRootFind() call. */
+    segLength = np.linalg.norm(points[0] - points[-1])
+    epsilon = 1.0e-6 * segLength
+    if alpha_l < epsilon or alpha_r < epsilon:
+        # fall back on standard (probably inaccurate) formula, and subdivide further if needed.
+        bezCurve[1] = bezCurve[0] + leftTangent * (segLength / 3.0)
+        bezCurve[2] = bezCurve[3] + rightTangent * (segLength / 3.0)
+
+    else:
+        # First and last control points of the Bezier curve are
+        # positioned exactly at the first and last data points
+        # Control points 1 and 2 are positioned an alpha distance out
+        # on the tangent vectors, left and right, respectively
+        bezCurve[1] = bezCurve[0] + leftTangent * alpha_l
+        bezCurve[2] = bezCurve[3] + rightTangent * alpha_r
+
+    return bezCurve
+
+
+def reparameterize(bezier, points, parameters):
+    return [newtonRaphsonRootFind(bezier, point, u) for point, u in zip(points, parameters)]
+
+
+def newtonRaphsonRootFind(bez, point, u):
+    """
+       Newton's root finding algorithm calculates f(x)=0 by reiterating
+       x_n+1 = x_n - f(x_n)/f'(x_n)
+
+       We are trying to find curve parameter u for some point p that minimizes
+       the distance from that point to the curve. Distance point to curve is d=q(u)-p.
+       At minimum distance the point is perpendicular to the curve.
+       We are solving
+       f = q(u)-p * q'(u) = 0
+       with
+       f' = q'(u) * q'(u) + q(u)-p * q''(u)
+
+       gives
+       u_n+1 = u_n - |q(u_n)-p * q'(u_n)| / |q'(u_n)**2 + q(u_n)-p * q''(u_n)|
+    """
+    d = cubic.q(bez, u)-point
+    numerator = (d * cubic.qprime(bez, u)).sum()
+    denominator = (cubic.qprime(bez, u)**2 + d * cubic.qprimeprime(bez, u)).sum()
+
+    if denominator == 0.0:
+        return u
+    else:
+        return u - numerator/denominator
+
+
+def chordLengthParameterize(points):
+    u = [0.0]
+    for i in range(1, len(points)):
+        u.append(u[i-1] + np.linalg.norm(points[i] - points[i-1]))
+
+    for i, _ in enumerate(u):
+        u[i] = u[i] / u[-1]
+
+    return u
+
+
+def computeMaxError(points, bez, parameters):
+    maxDist = 0.0
+    splitPoint = len(points)/2
+    for i, (point, u) in enumerate(zip(points, parameters)):
+        dist = np.linalg.norm(cubic.q(bez, u)-point)**2
+        if dist > maxDist:
+            maxDist = dist
+            splitPoint = i
+
+    return maxDist, splitPoint
+
+
+def normalize(v):
+    return v / np.linalg.norm(v)
+

db_upload.py

+#!/usr/bin/python3
+from functools import partial
+import mysql.connector
+import datetime
+import json
+
+def load_waypoints(filename):
+    with open(filename, 'rb') as f:
+        all_waypoints = json.load(f)
+
+    # setup table stuff
+    mydb = mysql.connector.connect(
+      host="127.0.0.1",
+      user="larkin_h",
+      password="Meepp973",
+      database="llh_saruser",
+      auth_plugin="caching_sha2_password"
+    )
+    mycursor = mydb.cursor()
+#    if mydb.is_connected():
+#        value = input("connected to database, upload waypoints? y/n: ")
+#        if value == 'y': # WAS TESTING HERE! SEEING IF IS_CONNECTED WILL HELP WITH HISTORICAL GPS DATA 
+
+
+    waypoint_dict = all_waypoints['robot_list']['robot_0'] # only use one robot for now
+
+    waypoints_json = []
+    for i,pt in enumerate(waypoint_dict.keys()):
+        waypoints_json.append({'stamp' : waypoint_dict[pt][0], 'timestamp' : -1, 'lat' :    waypoint_dict[pt][2], 'long' : waypoint_dict[pt][3]}) # leaving out altitude for now!
+    waypoints_json = sorted(waypoints_json, key = lambda i: i['stamp'])
+    wp_table = 'app3_waypointsdata'
+    wp_data = {'deviceid' : 'drone_0',
+               'taskid' : 'search_0',
+               'waypointsdata' : json.dumps(waypoints_json),
+               'created_at':datetime.datetime.now(),
+               'updated_at':datetime.datetime.now()}
+
+    sql_waypoint_update = 'UPDATE ' + wp_table + ' SET {}'.format(', '.join('{}=%s'.format(k) for k in wp_data)) + ' WHERE deviceid = %s'
+    mycursor.execute(sql_waypoint_update, list(wp_data.values()) + ['drone_0'])
+    mydb.commit()
+    print("waypoints updated")
+
+    mydb.close()
+    mycursor.close()
+    #rospy.loginfo("shuttin' down, bye")
+    print("shuttin' down")
+
+
+if __name__ == '__main__':
+    load_waypoints('waypoints.json')

docs/mindmap.mm

+<map version="freeplane 1.6.0">
+<!--To view this file, download free mind mapping software Freeplane from http://freeplane.sourceforge.net -->
+<node TEXT="Planning for MRMH-SAR" FOLDED="false" ID="ID_1962365023" CREATED="1548337355793" MODIFIED="1548342107695" STYLE="oval">
+<font SIZE="18"/>
+<hook NAME="MapStyle" zoom="0.827">
+    <properties edgeColorConfiguration="#808080ff,#ff0000ff,#0000ffff,#00ff00ff,#ff00ffff,#00ffffff,#7c0000ff,#00007cff,#007c00ff,#7c007cff,#007c7cff,#7c7c00ff" fit_to_viewport="false"/>
+
+<map_styles>
+<stylenode LOCALIZED_TEXT="styles.root_node" STYLE="oval" UNIFORM_SHAPE="true" VGAP_QUANTITY="24.0 pt">
+<font SIZE="24"/>
+<stylenode LOCALIZED_TEXT="styles.predefined" POSITION="right" STYLE="bubble">
+<stylenode LOCALIZED_TEXT="default" ICON_SIZE="12.0 pt" COLOR="#000000" STYLE="fork">
+<font NAME="SansSerif" SIZE="10" BOLD="false" ITALIC="false"/>
+</stylenode>
+<stylenode LOCALIZED_TEXT="defaultstyle.details"/>
+<stylenode LOCALIZED_TEXT="defaultstyle.attributes">
+<font SIZE="9"/>
+</stylenode>
+<stylenode LOCALIZED_TEXT="defaultstyle.note" COLOR="#000000" BACKGROUND_COLOR="#ffffff" TEXT_ALIGN="LEFT"/>
+<stylenode LOCALIZED_TEXT="defaultstyle.floating">
+<edge STYLE="hide_edge"/>
+<cloud COLOR="#f0f0f0" SHAPE="ROUND_RECT"/>
+</stylenode>
+</stylenode>
+<stylenode LOCALIZED_TEXT="styles.user-defined" POSITION="right" STYLE="bubble">
+<stylenode LOCALIZED_TEXT="styles.topic" COLOR="#18898b" STYLE="fork">
+<font NAME="Liberation Sans" SIZE="10" BOLD="true"/>
+</stylenode>
+<stylenode LOCALIZED_TEXT="styles.subtopic" COLOR="#cc3300" STYLE="fork">
+<font NAME="Liberation Sans" SIZE="10" BOLD="true"/>
+</stylenode>
+<stylenode LOCALIZED_TEXT="styles.subsubtopic" COLOR="#669900">
+<font NAME="Liberation Sans" SIZE="10" BOLD="true"/>
+</stylenode>
+<stylenode LOCALIZED_TEXT="styles.important">
+<icon BUILTIN="yes"/>
+</stylenode>
+</stylenode>
+<stylenode LOCALIZED_TEXT="styles.AutomaticLayout" POSITION="right" STYLE="bubble">
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level.root" COLOR="#000000" STYLE="oval" SHAPE_HORIZONTAL_MARGIN="10.0 pt" SHAPE_VERTICAL_MARGIN="10.0 pt">
+<font SIZE="18"/>
+</stylenode>
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level,1" COLOR="#0033ff">
+<font SIZE="16"/>
+</stylenode>
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level,2" COLOR="#00b439">
+<font SIZE="14"/>
+</stylenode>
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level,3" COLOR="#990000">
+<font SIZE="12"/>
+</stylenode>
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level,4" COLOR="#111111">
+<font SIZE="10"/>
+</stylenode>
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level,5"/>
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level,6"/>
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level,7"/>
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level,8"/>
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level,9"/>
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level,10"/>
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level,11"/>
+</stylenode>
+</stylenode>
+</map_styles>
+</hook>
+<hook NAME="AutomaticEdgeColor" COUNTER="19" RULE="ON_BRANCH_CREATION"/>
+<node TEXT="Human models" POSITION="left" ID="ID_1684848335" CREATED="1548337412756" MODIFIED="1548343523978">
+<arrowlink SHAPE="CUBIC_CURVE" COLOR="#000000" WIDTH="2" TRANSPARENCY="200" FONT_SIZE="9" FONT_FAMILY="SansSerif" DESTINATION="ID_1876475106" STARTINCLINATION="-15;-95;" ENDINCLINATION="-330;-21;" STARTARROW="NONE" ENDARROW="DEFAULT"/>
+<edge COLOR="#ff0000"/>
+<node TEXT="Lost person model" ID="ID_260661044" CREATED="1548338990360" MODIFIED="1548338995115"/>
+<node TEXT="Searcher model" ID="ID_1727449039" CREATED="1548338996496" MODIFIED="1548339002770"/>
+</node>
+<node TEXT="Terrain map" POSITION="left" ID="ID_1441035129" CREATED="1548337506683" MODIFIED="1548339052815">
+<arrowlink SHAPE="CUBIC_CURVE" COLOR="#000000" WIDTH="2" TRANSPARENCY="200" FONT_SIZE="9" FONT_FAMILY="SansSerif" DESTINATION="ID_1876475106" STARTINCLINATION="28;-27;" ENDINCLINATION="-681;-114;" STARTARROW="NONE" ENDARROW="DEFAULT"/>
+<edge COLOR="#ff00ff"/>
+</node>
+<node TEXT="Generate heatmap showing likelihood of the lost person having been at a location at a given time" POSITION="right" ID="ID_1876475106" CREATED="1548337624259" MODIFIED="1548337914559" HGAP_QUANTITY="25.249999664723884 pt" VSHIFT_QUANTITY="-6.74999979883433 pt">
+<edge COLOR="#7c007c"/>
+</node>
+<node FOLDED="true" POSITION="right" ID="ID_1921489374" CREATED="1548337929203" MODIFIED="1548339290118" HGAP_QUANTITY="24.499999687075622 pt"><richcontent TYPE="NODE">
+
+<html>
+  <head>
+    
+  </head>
+  <body>
+    <p>
+      Generate trajectories to optimize for <b>cost</b>&#160;while meeting <b>constraints</b>
+    </p>
+  </body>
+</html>
+
+</richcontent>
+<edge COLOR="#7c7c00"/>
+<node ID="ID_1843182780" CREATED="1548339153496" MODIFIED="1548339285583"><richcontent TYPE="NODE">
+
+<html>
+  <head>
+    
+  </head>
+  <body>
+    <p>
+      <b>Cost</b>: Time / distance / energy expenditure
+    </p>
+    <p>
+      <b>Constraints</b>: upper-bound the risk of misclassifying a &quot;cell&quot; given current data
+    </p>
+  </body>
+</html>
+
+</richcontent>
+</node>
+<node ID="ID_1244403626" CREATED="1548339153496" MODIFIED="1548339352243"><richcontent TYPE="NODE">
+
+<html>
+  <head>
+    
+  </head>
+  <body>
+    <p>
+      <b>Constraint</b>: Maximum Time / distance / energy expenditure
+    </p>
+    <p>
+      <b>Cost</b>: minimize the risk of misclassifying a &quot;cell&quot; given current data
+    </p>
+  </body>
+</html>
+
+</richcontent>
+</node>
+</node>
+<node TEXT="Human searcher(s) and UAVs begin search" POSITION="right" ID="ID_20122805" CREATED="1548338010530" MODIFIED="1548343552960" HGAP_QUANTITY="27.499999597668662 pt" VSHIFT_QUANTITY="9.749999709427366 pt">
+<edge COLOR="#ff0000"/>
+</node>
+<node TEXT="Information is collected during the search that points to the target having been at a location" POSITION="right" ID="ID_143645614" CREATED="1548338069210" MODIFIED="1548338965055" HGAP_QUANTITY="26.7499996200204 pt" VSHIFT_QUANTITY="5.999999821186071 pt">
+<edge COLOR="#00ffff"/>
+<node TEXT="Quantify new information and update heatmap accordingly" ID="ID_12597618" CREATED="1548338093873" MODIFIED="1548339623444" HGAP_QUANTITY="20.74999979883433 pt" VSHIFT_QUANTITY="14.249999575316918 pt">
+<arrowlink SHAPE="CUBIC_CURVE" COLOR="#000000" WIDTH="2" TRANSPARENCY="200" FONT_SIZE="9" FONT_FAMILY="SansSerif" DESTINATION="ID_1288342923" STARTINCLINATION="86;0;" ENDINCLINATION="58;-33;" STARTARROW="NONE" ENDARROW="DEFAULT"/>
+</node>
+<node TEXT="Update trajectories" ID="ID_1488239750" CREATED="1548338134417" MODIFIED="1548339687502" HGAP_QUANTITY="19.999999821186073 pt" VSHIFT_QUANTITY="5.249999843537812 pt">
+<arrowlink SHAPE="CUBIC_CURVE" COLOR="#000000" WIDTH="2" TRANSPARENCY="200" FONT_SIZE="9" FONT_FAMILY="SansSerif" DESTINATION="ID_847074053" STARTINCLINATION="208;0;" ENDINCLINATION="-140;-50;" STARTARROW="NONE" ENDARROW="DEFAULT"/>
+</node>
+</node>
+<node TEXT="New information changes the prior generated by model (parametric updates)" POSITION="right" ID="ID_146741951" CREATED="1548338425856" MODIFIED="1548338972465" HGAP_QUANTITY="28.24999957531692 pt" VSHIFT_QUANTITY="10.499999687075624 pt">
+<edge COLOR="#007c00"/>
+<node TEXT="Reconstruct map with new prior and measurements so far" ID="ID_63790389" CREATED="1548338497942" MODIFIED="1548339614672">
+<arrowlink SHAPE="CUBIC_CURVE" COLOR="#000000" WIDTH="2" TRANSPARENCY="200" FONT_SIZE="9" FONT_FAMILY="SansSerif" DESTINATION="ID_1288342923" STARTINCLINATION="53;0;" ENDINCLINATION="33;-32;" STARTARROW="NONE" ENDARROW="DEFAULT"/>
+</node>
+<node TEXT="Update trajectories" ID="ID_816464801" CREATED="1548338553520" MODIFIED="1548339691140" HGAP_QUANTITY="16.249999932944775 pt" VSHIFT_QUANTITY="5.999999821186071 pt">
+<arrowlink SHAPE="CUBIC_CURVE" COLOR="#000000" WIDTH="2" TRANSPARENCY="200" FONT_SIZE="9" FONT_FAMILY="SansSerif" DESTINATION="ID_847074053" STARTINCLINATION="83;0;" ENDINCLINATION="-179;-29;" STARTARROW="NONE" ENDARROW="DEFAULT"/>
+</node>
+</node>
+<node TEXT="Smoothing and Mapping" LOCALIZED_STYLE_REF="defaultstyle.floating" POSITION="right" ID="ID_1288342923" CREATED="1548339426987" MODIFIED="1548340239089" HGAP_QUANTITY="874.4999739378699 pt" VSHIFT_QUANTITY="223.49999333918112 pt">
+<hook NAME="FreeNode"/>
+<node TEXT="GraphSLAM, SAM, square-root SAM" ID="ID_813195303" CREATED="1548339528063" MODIFIED="1548342085909"/>
+<node TEXT="iSAM, iSAM2, GTSAM" ID="ID_403995845" CREATED="1548342038634" MODIFIED="1548342099131">
+<cloud COLOR="#ffff66" SHAPE="ARC"/>
+</node>
+</node>
+<node TEXT="Continuous-time Gaussian motion planning" LOCALIZED_STYLE_REF="defaultstyle.floating" POSITION="right" ID="ID_847074053" CREATED="1548339648400" MODIFIED="1548342107695" HGAP_QUANTITY="874.4999739378698 pt" VSHIFT_QUANTITY="305.2499909028414 pt">
+<hook NAME="FreeNode"/>
+<node TEXT="GPMP" ID="ID_1922303414" CREATED="1548339717903" MODIFIED="1548341637759"/>
+<node TEXT="GPMP2" ID="ID_957653490" CREATED="1548341620523" MODIFIED="1548341624999"/>
+<node TEXT="iGPMP" FOLDED="true" ID="ID_1701369459" CREATED="1548341625883" MODIFIED="1548342346412">
+<cloud COLOR="#ffff66" SHAPE="ARC"/>
+<node TEXT="No global optimality guarantees" ID="ID_1791607412" CREATED="1548342162841" MODIFIED="1548342201907">
+<node TEXT="Rapid random restarts?" ID="ID_211301315" CREATED="1548342317249" MODIFIED="1548342328980"/>
+</node>
+<node ID="ID_1815845281" CREATED="1548342204113" MODIFIED="1548342272429"><richcontent TYPE="NODE">
+
+<html>
+  <head>
+    
+  </head>
+  <body>
+    <p>
+      Limited ability to handle nonlinear inequality constraints <i>(motion constraints)</i>
+    </p>
+  </body>
+</html>
+
+</richcontent>
+<node ID="ID_738755480" CREATED="1548342278793" MODIFIED="1548342311822"><richcontent TYPE="NODE">
+
+<html>
+  <head>
+    
+  </head>
+  <body>
+    <p>
+      Sequential Quadratic Programming <i>(SQP)</i>
+    </p>
+  </body>
+</html>
+
+</richcontent>
+</node>
+</node>
+</node>
+</node>
+</node>
+</map>

docs/mindmap_overall.mm

+<map version="freeplane 1.6.0">
+<!--To view this file, download free mind mapping software Freeplane from http://freeplane.sourceforge.net -->
+<node TEXT="Planning for MRMH-SAR" FOLDED="false" ID="ID_1962365023" CREATED="1548337355793" MODIFIED="1548342107695" STYLE="oval">
+<font SIZE="18"/>
+<hook NAME="MapStyle" zoom="1.101">
+    <properties edgeColorConfiguration="#808080ff,#ff0000ff,#0000ffff,#00ff00ff,#ff00ffff,#00ffffff,#7c0000ff,#00007cff,#007c00ff,#7c007cff,#007c7cff,#7c7c00ff" fit_to_viewport="false"/>
+
+<map_styles>
+<stylenode LOCALIZED_TEXT="styles.root_node" STYLE="oval" UNIFORM_SHAPE="true" VGAP_QUANTITY="24.0 pt">
+<font SIZE="24"/>
+<stylenode LOCALIZED_TEXT="styles.predefined" POSITION="right" STYLE="bubble">
+<stylenode LOCALIZED_TEXT="default" ICON_SIZE="12.0 pt" COLOR="#000000" STYLE="fork">
+<font NAME="SansSerif" SIZE="10" BOLD="false" ITALIC="false"/>
+</stylenode>
+<stylenode LOCALIZED_TEXT="defaultstyle.details"/>
+<stylenode LOCALIZED_TEXT="defaultstyle.attributes">
+<font SIZE="9"/>
+</stylenode>
+<stylenode LOCALIZED_TEXT="defaultstyle.note" COLOR="#000000" BACKGROUND_COLOR="#ffffff" TEXT_ALIGN="LEFT"/>
+<stylenode LOCALIZED_TEXT="defaultstyle.floating">
+<edge STYLE="hide_edge"/>
+<cloud COLOR="#f0f0f0" SHAPE="ROUND_RECT"/>
+</stylenode>
+</stylenode>
+<stylenode LOCALIZED_TEXT="styles.user-defined" POSITION="right" STYLE="bubble">
+<stylenode LOCALIZED_TEXT="styles.topic" COLOR="#18898b" STYLE="fork">
+<font NAME="Liberation Sans" SIZE="10" BOLD="true"/>
+</stylenode>
+<stylenode LOCALIZED_TEXT="styles.subtopic" COLOR="#cc3300" STYLE="fork">
+<font NAME="Liberation Sans" SIZE="10" BOLD="true"/>
+</stylenode>
+<stylenode LOCALIZED_TEXT="styles.subsubtopic" COLOR="#669900">
+<font NAME="Liberation Sans" SIZE="10" BOLD="true"/>
+</stylenode>
+<stylenode LOCALIZED_TEXT="styles.important">
+<icon BUILTIN="yes"/>
+</stylenode>
+</stylenode>
+<stylenode LOCALIZED_TEXT="styles.AutomaticLayout" POSITION="right" STYLE="bubble">
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level.root" COLOR="#000000" STYLE="oval" SHAPE_HORIZONTAL_MARGIN="10.0 pt" SHAPE_VERTICAL_MARGIN="10.0 pt">
+<font SIZE="18"/>
+</stylenode>
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level,1" COLOR="#0033ff">
+<font SIZE="16"/>
+</stylenode>
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level,2" COLOR="#00b439">
+<font SIZE="14"/>
+</stylenode>
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level,3" COLOR="#990000">
+<font SIZE="12"/>
+</stylenode>
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level,4" COLOR="#111111">
+<font SIZE="10"/>
+</stylenode>
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level,5"/>
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level,6"/>
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level,7"/>
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level,8"/>
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level,9"/>
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level,10"/>
+<stylenode LOCALIZED_TEXT="AutomaticLayout.level,11"/>
+</stylenode>
+</stylenode>
+</map_styles>
+</hook>
+<hook NAME="AutomaticEdgeColor" COUNTER="19" RULE="ON_BRANCH_CREATION"/>
+<node TEXT="Human models" POSITION="left" ID="ID_1684848335" CREATED="1548337412756" MODIFIED="1548343523978">
+<arrowlink SHAPE="CUBIC_CURVE" COLOR="#000000" WIDTH="2" TRANSPARENCY="200" FONT_SIZE="9" FONT_FAMILY="SansSerif" DESTINATION="ID_1876475106" STARTINCLINATION="-15;-95;" ENDINCLINATION="-330;-21;" STARTARROW="NONE" ENDARROW="DEFAULT"/>
+<edge COLOR="#ff0000"/>
+<node TEXT="Lost person model" ID="ID_260661044" CREATED="1548338990360" MODIFIED="1548338995115"/>
+<node TEXT="Searcher model" ID="ID_1727449039" CREATED="1548338996496" MODIFIED="1548339002770"/>
+</node>
+<node TEXT="Terrain map" POSITION="left" ID="ID_1441035129" CREATED="1548337506683" MODIFIED="1548339052815">
+<arrowlink SHAPE="CUBIC_CURVE" COLOR="#000000" WIDTH="2" TRANSPARENCY="200" FONT_SIZE="9" FONT_FAMILY="SansSerif" DESTINATION="ID_1876475106" STARTINCLINATION="28;-27;" ENDINCLINATION="-681;-114;" STARTARROW="NONE" ENDARROW="DEFAULT"/>
+<edge COLOR="#ff00ff"/>
+</node>
+<node TEXT="Generate heatmap showing likelihood of the lost person having been at a location at a given time" POSITION="right" ID="ID_1876475106" CREATED="1548337624259" MODIFIED="1548337914559" HGAP_QUANTITY="25.249999664723884 pt" VSHIFT_QUANTITY="-6.74999979883433 pt">
+<edge COLOR="#7c007c"/>
+</node>
+<node FOLDED="true" POSITION="right" ID="ID_1921489374" CREATED="1548337929203" MODIFIED="1548339290118" HGAP_QUANTITY="24.499999687075622 pt"><richcontent TYPE="NODE">
+
+<html>
+  <head>
+    
+  </head>
+  <body>
+    <p>
+      Generate trajectories to optimize for <b>cost</b>&#160;while meeting <b>constraints</b>
+    </p>
+  </body>
+</html>
+</richcontent>
+<edge COLOR="#7c7c00"/>
+<node ID="ID_1843182780" CREATED="1548339153496" MODIFIED="1548339285583"><richcontent TYPE="NODE">
+
+<html>
+  <head>
+    
+  </head>
+  <body>
+    <p>
+      <b>Cost</b>: Time / distance / energy expenditure
+    </p>
+    <p>
+      <b>Constraints</b>: upper-bound the risk of misclassifying a &quot;cell&quot; given current data
+    </p>
+  </body>
+</html>
+</richcontent>
+</node>
+<node ID="ID_1244403626" CREATED="1548339153496" MODIFIED="1548339352243"><richcontent TYPE="NODE">
+
+<html>
+  <head>
+    
+  </head>
+  <body>
+    <p>
+      <b>Constraint</b>: Maximum Time / distance / energy expenditure
+    </p>
+    <p>
+      <b>Cost</b>: minimize the risk of misclassifying a &quot;cell&quot; given current data
+    </p>
+  </body>
+</html>
+</richcontent>
+</node>
+</node>
+<node TEXT="Human searcher(s) and UAVs begin search" POSITION="right" ID="ID_20122805" CREATED="1548338010530" MODIFIED="1548343552960" HGAP_QUANTITY="27.499999597668662 pt" VSHIFT_QUANTITY="9.749999709427366 pt">
+<edge COLOR="#ff0000"/>
+</node>
+<node TEXT="Information is collected during the search that points to the target having been at a location" POSITION="right" ID="ID_143645614" CREATED="1548338069210" MODIFIED="1548338965055" HGAP_QUANTITY="26.7499996200204 pt" VSHIFT_QUANTITY="5.999999821186071 pt">
+<edge COLOR="#00ffff"/>
+<node TEXT="Quantify new information and update heatmap accordingly" ID="ID_12597618" CREATED="1548338093873" MODIFIED="1548339623444" HGAP_QUANTITY="20.74999979883433 pt" VSHIFT_QUANTITY="14.249999575316918 pt">
+<arrowlink SHAPE="CUBIC_CURVE" COLOR="#000000" WIDTH="2" TRANSPARENCY="200" FONT_SIZE="9" FONT_FAMILY="SansSerif" DESTINATION="ID_1288342923" STARTINCLINATION="86;0;" ENDINCLINATION="58;-33;" STARTARROW="NONE" ENDARROW="DEFAULT"/>
+</node>
+<node TEXT="Update trajectories" ID="ID_1488239750" CREATED="1548338134417" MODIFIED="1548339687502" HGAP_QUANTITY="19.999999821186073 pt" VSHIFT_QUANTITY="5.249999843537812 pt">
+<arrowlink SHAPE="CUBIC_CURVE" COLOR="#000000" WIDTH="2" TRANSPARENCY="200" FONT_SIZE="9" FONT_FAMILY="SansSerif" DESTINATION="ID_847074053" STARTINCLINATION="208;0;" ENDINCLINATION="-140;-50;" STARTARROW="NONE" ENDARROW="DEFAULT"/>
+</node>
+</node>
+<node TEXT="New information changes the prior generated by model (parametric updates)" POSITION="right" ID="ID_146741951" CREATED="1548338425856" MODIFIED="1548338972465" HGAP_QUANTITY="28.24999957531692 pt" VSHIFT_QUANTITY="10.499999687075624 pt">
+<edge COLOR="#007c00"/>
+<node TEXT="Reconstruct map with new prior and measurements so far" ID="ID_63790389" CREATED="1548338497942" MODIFIED="1548339614672">
+<arrowlink SHAPE="CUBIC_CURVE" COLOR="#000000" WIDTH="2" TRANSPARENCY="200" FONT_SIZE="9" FONT_FAMILY="SansSerif" DESTINATION="ID_1288342923" STARTINCLINATION="53;0;" ENDINCLINATION="33;-32;" STARTARROW="NONE" ENDARROW="DEFAULT"/>
+</node>
+<node TEXT="Update trajectories" ID="ID_816464801" CREATED="1548338553520" MODIFIED="1548339691140" HGAP_QUANTITY="16.249999932944775 pt" VSHIFT_QUANTITY="5.999999821186071 pt">
+<arrowlink SHAPE="CUBIC_CURVE" COLOR="#000000" WIDTH="2" TRANSPARENCY="200" FONT_SIZE="9" FONT_FAMILY="SansSerif" DESTINATION="ID_847074053" STARTINCLINATION="83;0;" ENDINCLINATION="-179;-29;" STARTARROW="NONE" ENDARROW="DEFAULT"/>
+</node>
+</node>
+<node TEXT="Smoothing and Mapping" LOCALIZED_STYLE_REF="defaultstyle.floating" POSITION="right" ID="ID_1288342923" CREATED="1548339426987" MODIFIED="1548340239089" HGAP_QUANTITY="874.4999739378699 pt" VSHIFT_QUANTITY="223.49999333918112 pt">
+<hook NAME="FreeNode"/>
+<node TEXT="GraphSLAM, SAM, square-root SAM" ID="ID_813195303" CREATED="1548339528063" MODIFIED="1548342085909"/>
+<node TEXT="iSAM, iSAM2, GTSAM" ID="ID_403995845" CREATED="1548342038634" MODIFIED="1548342099131">
+<cloud COLOR="#ffff66" SHAPE="ARC"/>
+</node>
+</node>
+<node TEXT="Continuous-time Gaussian motion planning" LOCALIZED_STYLE_REF="defaultstyle.floating" POSITION="right" ID="ID_847074053" CREATED="1548339648400" MODIFIED="1548342107695" HGAP_QUANTITY="874.4999739378698 pt" VSHIFT_QUANTITY="305.2499909028414 pt">
+<hook NAME="FreeNode"/>
+<node TEXT="GPMP" ID="ID_1922303414" CREATED="1548339717903" MODIFIED="1548341637759"/>
+<node TEXT="GPMP2" ID="ID_957653490" CREATED="1548341620523" MODIFIED="1548341624999"/>
+<node TEXT="iGPMP" FOLDED="true" ID="ID_1701369459" CREATED="1548341625883" MODIFIED="1548351915574">
+<cloud COLOR="#ffff66" SHAPE="ARC"/>
+<node TEXT="No global optimality guarantees" ID="ID_1791607412" CREATED="1548342162841" MODIFIED="1548342201907">
+<node TEXT="Rapid random restarts?" ID="ID_211301315" CREATED="1548342317249" MODIFIED="1548342328980"/>
+</node>
+<node FOLDED="true" ID="ID_1815845281" CREATED="1548342204113" MODIFIED="1548451448596"><richcontent TYPE="NODE">
+
+<html>
+  <head>
+    
+  </head>
+  <body>
+    <p>
+      Limited ability to handle nonlinear inequality constraints <i>(motion constraints)</i>
+    </p>
+  </body>
+</html>
+</richcontent>
+<node ID="ID_738755480" CREATED="1548342278793" MODIFIED="1548451448594"><richcontent TYPE="NODE">
+
+<html>
+  <head>
+    
+  </head>
+  <body>
+    <p>
+      Sequential Quadratic Programming <i>(SQP)</i>
+    </p>
+  </body>
+</html>
+</richcontent>
+</node>
+</node>
+</node>
+</node>
+</node>
+</map>

gp/notebooks/.ipynb_checkpoints/GPyTorch_sample-checkpoint.ipynb

+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": 1,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import math\n",
+    "import torch\n",
+    "import gpytorch\n",
+    "from matplotlib import pyplot as plt\n",
+    "\n",
+    "%matplotlib inline"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 6,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "train_x = torch.linspace(0, 1, 100).view(1, -1, 1)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": []
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": []
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Python 3",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.6.8"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}

gp/notebooks/.ipynb_checkpoints/Untitled-checkpoint.ipynb

+{
+ "cells": [],
+ "metadata": {},
+ "nbformat": 4,
+ "nbformat_minor": 2
+}

gp/notebooks/Untitled.ipynb

+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": 6,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import sys, os, time\n",
+    "import json\n",
+    "\n",
+    "import numpy as np\n",
+    "import pymc3 as pm"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 5,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "(400, 400)\n"
+     ]
+    }
+   ],
+   "source": [
+    "p = np.load('p1903221634.npy')\n",
+    "print(p.shape)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 7,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "with open('params1903221634.json', 'r') as jfp:\n",
+    "    params = json.load(jfp)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 13,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "IS_HEATMAP_DYN\tT\t_x_shape\t_y_shape\tdt\tdynheatmap_log_file\texp_description\texp_id\texp_name\texp_results_dir\texp_str\theatmap_log_file\thistory_log_file\thmax\thmin\titer_steps\tmax_search_steps\tnum_humans\tnum_iter\tnum_searchers\tparam_log_file\trandom_motion_init\trandom_pos\tres\tsave_terrain\tsweep_bounds\tsx\tterrainType\ttime_str\ttqdm\txlims\txmax\txmin\tylims\tymax\tymin\tzlims\n"
+     ]
+    }
+   ],
+   "source": [
+    "print( '\\t'.join( list(params.keys()) ) )"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": []
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Python 3",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.6.8"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}

gp/notebooks/params1903221634.json

+{
+    "IS_HEATMAP_DYN": false,
+    "T": 150,
+    "_x_shape": 400,
+    "_y_shape": 400,
+    "dt": 0.25,
+    "dynheatmap_log_file": "results/000_190322-163414_test/dynheatmap_190322-163414.mp4",
+    "exp_description": "Generate a heatmap",
+    "exp_id": 0,
+    "exp_name": "test",
+    "exp_results_dir": "results/000_190322-163414_test",
+    "exp_str": "000_190322-163414_test",
+    "heatmap_log_file": "results/000_190322-163414_test/heatmap_190322-163414.png",
+    "history_log_file": "results/000_190322-163414_test/p_history_190322-163414.npy",
+    "hmax": 50.0,
+    "hmin": -50.0,
+    "iter_steps": 600,
+    "max_search_steps": 100000,
+    "num_humans": 1,
+    "num_iter": 10000,
+    "num_searchers": 3,
+    "param_log_file": "results/000_190322-163414_test/params_190322-163414.log",
+    "random_motion_init": true,
+    "random_pos": true,
+    "res": 0.25,
+    "save_terrain": false,
+    "sweep_bounds": [
+        [
+            16.66666666666667,
+            50.0
+        ],
+        [
+            -50.0,
+            50.0
+        ]
+    ],
+    "sx": [
+        33.33333333333333,
+        -50.0
+    ],
+    "terrainType": "random",
+    "time_str": "190322-163414",
+    "tqdm": null,
+    "xlims": [
+        -50.0,
+        50.0
+    ],
+    "xmax": 50.0,
+    "xmin": -50.0,
+    "ylims": [
+        -50.0,
+        50.0
+    ],
+    "ymax": 50.0,
+    "ymin": -50.0,
+    "zlims": [
+        -50.0,
+        50.0
+    ]
+}
\ No newline at end of file

gp/searchergp.py

+import rtree
+from scipy.spatial import KDTree, cKDTree
+
+import torch
+import pyro
+import pyro.contrib.gp as gp
+
+import numpy as np
+from scipy import ndimage, misc, signal, interpolate
+
+import matplotlib.pyplot as plt
+from matplotlib import cm
+from mpl_toolkits.mplot3d import Axes3D
+
+from mrmh_model import params
+
+from tqdm import tqdm, trange
+
+import time
+import pdb
+
+class SearcherGP:
+    def __init__(self, mc_handle):
+        self.mc_handle = mc_handle
+        self.searcherClass = mc_handle.searcherClass
+
+        props = rtree.index.Property()
+        props.dimension = 2
+        self.idx = rtree.index.Index(properties=props)
+        self.p_interp = mc_handle.p_interp
+        
+        self.sgpr = None
+        self.init_GP()
+
+    def p_at_posxy(self, pos_xy):
+        return self.p_interp.ev(*pos_xy)
+
+    def init_GP(self):
+        kernel = gp.kernels.RBF(input_dim=2, variance=torch.tensor(5.),
+                                lengthscale=torch.tensor(5.))
+
+        X = [[], []] # search points
+        Xu = [[], []] # inducing points
+        y = [] # prior at search points
+
+        for tsr in self.searcherClass.searchers_list:
+            all_x, all_y, _, _ = tsr.smd_history
+            X[0] += [ float(tx) for tx in all_x ]
+            X[1] += [ float(ty) for ty in all_y ]
+            y += [ float(self.p_at_posxy( (tx, ty) )) for tx, ty in zip(all_x, all_y) ]
+            # y += [ 1 for tx in all_x ]
+
+            gx, gy = tsr.sweep_guides.transpose().tolist()
+            Xu[0] += gx
+            Xu[1] += gy
+
+        X = [*zip(*X)]
+        Xu = [*zip(*Xu)]
+
+        self.Xtrain = torch.FloatTensor(X)
+        self.ytrain = torch.FloatTensor(y)
+        self.ntrain = len(X)
+        self.Xu = torch.FloatTensor(Xu)
+        self.sgpr = gp.models.SparseGPRegression(self.Xtrain, self.ytrain, kernel, Xu=self.Xu, jitter=1.0e-5)
+
+        optimizer = torch.optim.Adam(self.sgpr.parameters(), lr=0.005)
+        loss_fn = pyro.infer.Trace_ELBO().differentiable_loss
+
+        num_steps = 5
+        for i in trange(num_steps):
+            optimizer.zero_grad()
+            loss = loss_fn(self.sgpr.model, self.sgpr.guide)
+            loss.backward()
+            optimizer.step()
+
+    def gp_calc(self):
+        pass
+
+    def inference_grid(self):
+        # _y = torch.linspace( self.mc_handle.ymin, self.mc_handle.ymax, 4*(self.mc_handle.ymax-self.mc_handle.ymin) )
+        x = torch.as_tensor(self.mc_handle.terrain.x, dtype=torch.float).view(-1, 1)
+        y = torch.as_tensor(self.mc_handle.terrain.y, dtype=torch.float).view(-1, 1)
+        self.Xtest = torch.cat([x, y], 1)
+        with torch.no_grad():
+            self.ytest, self.test_cov = self.sgpr(self.Xtest)
+
+    def visualize_gp(self):
+        # if(self.searcherClass.terrain is not None and self.searcherClass.terrain.paths_plot is not None):
+        #     fig = self.searcherClass.terrain.paths_plot
+        #     plt.figure(fig.number)
+        #     plt.title('Heatmap - Lost person | Searchers\' paths')
+        # else:
+        fig = plt.figure()
+        self.ytest_np_plot = self.ytest.view(-1, self.mc_handle._y_shape).numpy()
+        # ax = fig.gca(projection='3d')
+        # # self.searcherClass.terrain.paths_plot = fig
+        # ax.plot( xs=self.Xtest[:, 0].numpy(), ys=self.Xtest[:, 0].numpy(), zs=self.mc_handle.terrain.h.flatten() )
+        extent=[self.mc_handle.xmin, self.mc_handle.xmax, self.mc_handle.ymin, self.mc_handle.ymax]
+        plt.imshow( self.ytest_np_plot.transpose(), cmap='hot', interpolation='nearest', extent=extent, origin='lower')
+
+    def visualize_training_data(self):
+        self.search_traj = np.zeros_like(self.ytest_np_plot)
+        for tsr in self.searcherClass.searchers_list:
+            all_x, all_y, _, _ = tsr.smd_history
+            for tx, ty in zip(all_x, all_y):
+                tx_idx, ty_idx = self.mc_handle.terrain.idx_from_coords(tx, ty)
+                self.search_traj[tx_idx, ty_idx] += 25
+
+        fig = plt.figure()
+        extent=[self.mc_handle.xmin, self.mc_handle.xmax, self.mc_handle.ymin, self.mc_handle.ymax]
+        plt.imshow( self.search_traj.transpose(), cmap='hot', interpolation='nearest', extent=extent, origin='lower')
+
+    def add_searcher_paths(self):
+        """
+        Adds searchers' paths from self.searcherClass into self.index
+        for quick nearest neighbors lookup
+        """
+        for tsr in self.searcherClass.searchers_list:
+            x, y, z, t = tsr.smd_history
+            for tx, ty, tz, tt in zip(x, y, z, t):
+                pos_xy = tuple([ tx, ty ])
+                pos_xyzt = pos_xy + tuple([tz, tt])
+                pdb.set_trace()
+                self.idx.insert(0, pos_xy, pos_xyzt)
+
+    def time_inference(self):
+        x = torch.as_tensor(self.mc_handle.terrain.x, dtype=torch.float).view(-1, 1)
+        y = torch.as_tensor(self.mc_handle.terrain.y, dtype=torch.float).view(-1, 1)
+        self.Xtest = torch.cat([x, y], 1)
+
+        stime = time.time()
+        with torch.no_grad():
+            self.ytest, self.test_cov = self.sgpr(self.Xtest)
+        print( '{} points in one go took {} secs.'.format( self.Xtest.shape[0], time.time()-stime ) )
+
+        stime = time.time()
+        num_points = self.Xtest.size()[0]
+        for i in trange(num_points):
+            with torch.no_grad():
+                ytest, test_cov = self.sgpr(self.Xtest[i:i+1, :])
+        print( '{} points in a loop took {} secs.'.format( num_points, time.time()-stime ) )
+
+    def nbors_in_square(self, pos, sq_side):
+        """
+        return neighbors within a square of side `sq_side`
+        with `pos` at its center
+        :param pos: tuple(2), position in 2D
+        :param sq_side: float, side of square
+        """
+        cx, cy = pos
+        half_s = float(sq_side) / 2
+
+        # boundaries
+        left = cx - half_s
+        right = cx + half_s
+        bottom = cy - half_s
+        top = cy + half_s
+
+        return list( self.idx.intersection((left, bottom, right, top), objects="raw") )
+
+    def nearby(self, x, n):
+        """
+        Return nearby vertices
+        :param x: tuple, vertex around which searching
+        :param n: int, max number of neighbors to return
+        :return: list of nearby vertices
+        """
+        return list( self.idx.nearest(x, num_results=n, objects="raw") )
+
+    def nearest(self, x):
+        """
+        Return vertex nearest to x
+        :param x: tuple, vertex around which searching
+        :return: tuple, nearest vertex to x
+        """
+        return self.nearby(x, 1)[0]
+
+
+class SearchNode(object):
+    nextNodeIdx = 0
+    def __init__(self, pos, p=0.5, t=0.0, cost=float('inf') ):
+        """
+        Init a node
+        :param pos: np.array(3), xyz coordinates
+        :param t: float, time since t0
+        """
+        self.index = SearchNode.nextNodeIdx
+        self.pos = pos
+        self.x, self.y, self.z = tuple(self.pos)
+        self.p = p
+        self.t = t
+        self.hash_tuple = tuple(pos) + tuple([self.t])
+
+        self.cost = cost
+
+        SearchNode.nextNodeIdx += 1
+
+    def __hash__(self):
+        return hash(self.hash_tuple)
+
+    def __eq__(self, other):
+        return self.hash_tuple == other.hash_tuple
+
+    def __ne__(self, other):
+        return not(self==other)
+
+def main():
+    pass
+
+if __name__ == "__main__":
+    main()
\ No newline at end of file

gp/static.py

+import pymc3 as pm
+
+def main():
+    pass
+
+if __name__ == "__main__":
+    main()
\ No newline at end of file

gp/test_pymc.py

+import sys, os, time
+import json
+
+import numpy as np
+import pymc3 as pm
+
+import theano
+import theano.tensor as tt
+
+import pdb
+
+zero_func = pm.gp.mean.Zero()
+
+ls_z = np.array([3.0]).astype('float64')
+
+t = np.linspace(-5, 5, 20)
+tpos = np.linspace(0, 5, 10)
+x, y, z = np.meshgrid(t, t, tpos)
+print(x.shape, y.shape, z.shape, t.shape)
+
+X = np.vstack( (x.flatten(), y.flatten(), z.flatten()) ).transpose()
+# X = np.asarray(X, dtype=float)
+print(X.shape)
+
+ls1 = 0.05
+ls2 = 0.6
+w = 0.3
+def tanh_func(x, ls1, ls2, w):
+    return tt.tanh(x)
+
+def ls_func(x, ls_z):
+    return theano.shared( np.asmatrix( np.array([ x[2], x[2], ls_z ]) ) ) #.astype('float64'))
+
+# cov = pm.gp.cov.Gibbs(1, ls_func, ls_z)
+cov = pm.gp.cov.Gibbs(3, tanh_func, (ls1, ls2, w))
+K = cov(X).eval()
+
+    # return np.array( [2]* np.shape(x)[-1] )
+
+    # ls_vec = tt.ones((3))
+    # ls_vec[0] = x[2]
+    # ls_vec[1] = ls_vec[0]
+    # ls_vec[2] = ls_z
\ No newline at end of file

gp/test_pyro.py

+import torch
+
+import matplotlib.pyplot as plt
+from matplotlib import cm
+from mpl_toolkits.mplot3d import Axes3D
+
+import numpy as np
+
+import pyro
+import pyro.contrib.gp as gp
+import pyro.distributions as dist
+
+import pdb
+
+from tqdm import tqdm, trange
+
+ls_z = torch.tensor([3.0])
+
+t = torch.linspace(-5, 5, 20)
+tpos = torch.linspace(0, 5, 10)
+x, y = torch.meshgrid(t, t)
+print(x.shape, y.shape, t.shape)
+
+X = torch.cat( ( x.flatten().view(1, -1), y.flatten().view(1, -1) ) ).transpose(1,0)
+
+Y = torch.linspace(0, 100, 400) + torch.rand((400)) * 50 - 25
+
+# X = np.asarray(X, dtype=float)
+print(X.shape, type(X))
+
+def ls_func3(x, ls_z):
+    ls0 = ls1 = x[:, 2:]
+    ls2 = ls_z * torch.ones_like(ls0)
+    return torch.cat( (ls0, ls1, ls2), 1)
+
+def ls_func(x):
+    ls0 = ls1 = x[:, 1:]
+    return torch.cat( (ls0, ls1), 1)
+
+kernel = gp.kernels.gibbs.Gibbs( input_dim=2, lengthscale_fn=ls_func )
+
+gpr = gp.models.GPRegression(X, Y, kernel)
+
+optimizer = torch.optim.Adam(gpr.parameters(), lr=0.005)
+loss_fn = pyro.infer.Trace_ELBO().differentiable_loss
+
+losses = []
+num_steps = 2
+for i in trange(num_steps):
+    optimizer.zero_grad()
+    loss = loss_fn(gpr.model, gpr.guide)
+    loss.backward()
+    optimizer.step()
+    losses.append(loss.item())
+
+x_t = torch.rand((10, 2)) * 10 - 5
+
+with  torch.no_grad():
+    m, cov = gpr(x_t, full_cov=True)
+
+print(m.shape)
+print(cov.shape)
+
+x_t_np = x_t.numpy()
+m_np = m.numpy()
+
+X_np = X.numpy()
+Y_np = Y.numpy()
+
+fig = plt.figure()
+ax = fig.gca(projection='3d')
+ax.plot( xs=x_t_np[:,0], ys=x_t_np[:,1], zs=m_np, color='red' )
+ax.scatter( xs=X_np[:,0], ys=X_np[:,1], zs=Y_np, color='blue' )
+
+plt.show()
+
+print(np.linalg.det(cov))
+print(np.trace(cov))
+
+print('Done!')
+
+# (Pdb) X.shape
+# torch.Size([400, 2])
+# (Pdb) Z.shape
+# torch.Size([10, 2])
+# (Pdb) rX.shape
+# torch.Size([400, 2])
+# (Pdb) rZ.shape
+# torch.Size([10, 2])
+# (Pdb) rX2.shape
+# torch.Size([400, 1, 2])
+# (Pdb) rZ2.shape
+# torch.Size([1, 10, 2])
+# (Pdb) rX2_plus_rZ2.shape
+# torch.Size([400, 10])
+# (Pdb) KK = ( _torch_sqrt( torch.pow( (2.0 * rX.matmul(rZ.t())) / rX2_plus_rZ2, dim ) )* torch.exp( -1.0 * r2 / rX2_plus_rZ2 ) )
+# (Pdb) KK.shape
+# torch.Size([400, 10])
\ No newline at end of file

larrt/README.md

+# larrt

larrt/notebooks/.ipynb_checkpoints/Untitled-checkpoint.ipynb

+{
+ "cells": [],
+ "metadata": {},
+ "nbformat": 4,
+ "nbformat_minor": 2
+}