text: """
  * affine triangulation
  * display point clouds
  * DEM projection
  
  Copyright (C) 2018, Enric Meinhardt-Llopis <enric.meinhardt@cmla.ens-cachan.fr>
  Copyright (C) 2018, Carlo de Franchis <carlo.de-franchis@ens-cachan.fr>
  Copyright (C) 2018, Gabriele Facciolo <facciolo@cmla.ens-cachan.fr>
  """
  
  import numpy as np
  
  
  def triangulation_affine(PA, PB, x1, y1, x2, y2):
      """
      Triangulate a (list of) match(es) between two images of affine cameras.
  
      Arguments:
          PA, PB : affine (projection) camera matrices of the two images
          x1, y1 : pixel coordinates in the domain of the first image
          x2, y2 : pixel coordinates in the domain of the second image
  
      Return value: a 4-tuple (lon, lat, h, e)
          lon, lat, h, e : coordinates of the 3D point(s), reprojection error
      """
  
      # build projection and localization matrices as 4x4 homogeneous maps
      # (x,y,h,1) <-> (lon,lat,h,1)
      PA = np.vstack([ PA[0:2], [0,0,1,0], [0,0,0,1]])  # pick only first two rows
      PB = np.vstack([ PB[0:2], [0,0,1,0], [0,0,0,1]])  # pick only first two rows
      LA = np.linalg.inv(PA)  # inverse of a 4x4 matrix
  
      # affine epipolar correspondence
      E = PB @ LA
  
      # Now, the linear system E * (x1, y1, h, 1) = (x2, y2, h, 1)
      # has two different equations and one unknown h.  We solve it by
      # least squares (a simple projection, in this case).
  
      # give names to the 8 non-trivial coefficients of E
      a, b, p, r = E[0]
      c, d, q, s = E[1]
  
      # coefficients of the affine triangulation
      f = [-p*a - q*c, -p*b - q*d, p, q, -p*r - q*s ] / (p*p + q*q)
  
      # apply the triangulation (first use of the input points)
      h = f[0]*x1 + f[1]*y1 + f[2]*x2 + f[3]*y2 + f[4]
  
      # finish the computation and return the 4 required numbers (or vectors)
      lon = LA[0,0] * x1 + LA[0,1] * y1 + LA[0,2] * h + LA[0,3]
      lat = LA[1,0] * x1 + LA[1,1] * y1 + LA[1,2] * h + LA[1,3]
      ex = E[0,0] * x1 + E[0,1] * y1 + E[0,2] * h + E[0,3] - x2
      ey = E[1,0] * x1 + E[1,1] * y1 + E[1,2] * h + E[1,3] - y2
      e = ex * ex + ey * ey
      return lon, lat, h, e
  
  
  def triangulate_disparities(dmap, rpc1, rpc2, S1, S2, PA, PB,):
      """
      Triangulate a disparity map
  
      Arguments:
          dmap : a disparity map between two rectified images
          rpc1, rpc2 : calibration data of each image
          S1, S2 : rectifying affine maps (from the domain of the fullsize images)
          PA, PB : the affine approximations of rpc1 and rpc2 (not always used)
  
      Return:
          xyz : a matrix of size Nx3 (where N is the number of finite disparites
                in dmap) this matrix contains the coordinates of the 3d points in
                "lon,lat,h" or "e,n,h"
      """
      from utils import utm_from_lonlat
  
      # 1. unroll all the valid (finite) disparities of dmap into a vector
      m = np.isfinite(dmap.flatten())
      x = np.argwhere(np.isfinite(dmap))[:,1]  # attention to order of the indices
      y = np.argwhere(np.isfinite(dmap))[:,0]
      d = dmap.flatten()[m]
  
      # 2. for each disparity
  
      # 2.1. produce a pair in the original domain by composing with S1 and S2
      p = np.linalg.inv(S1) @ np.vstack( (x+0, y, np.ones(len(d))) )
      q = np.linalg.inv(S2) @ np.vstack( (x+d, y, np.ones(len(d))) )
  
      # 2.2. triangulate the pair of image points to find a 3D point (in UTM)
      lon, lat, h, e = triangulation_affine(PA,PB, p[0,:],p[1,:], q[0,:],q[1,:])
      #east, north, _, _ = np.vectorize(utm.from_latlon)(lat, lon)
      east, north = utm_from_lonlat(lon, lat)
  
      # 2.3. add this point to the output list
      xyz = np.vstack((east, north, h)).T
  
      return xyz
  
  
  # code for projection into UTM coordinates
  from numba import jit
  
  # utility function for computing the average at each cell
  @jit
  def reduceavg(w, h,  ix, iy, z):
      D_sum = np.zeros((h,w))  #-np.ones((h,w))*np.inf
      D_cnt = np.zeros((h,w))
      for t in range(len(ix)):
          if iy[t]<0 or ix[t]<0 or iy[t]>=h or ix[t]>=w:
              continue
          ty = iy[t]
          tx = ix[t]
          D_sum[ty,tx] += z[t]
          D_cnt[ty,tx] += 1
  
      D_sum /= D_cnt*(D_cnt>0) + 1*(D_cnt==0) # needed for computing average
  
      return D_sum
  
  
  # projects them into a grid defined by the bunding box (emin, emax, nmin, nmax)
  def project_cloud_into_utm_grid(xyz, emin, emax, nmin, nmax, resolution=1):
      """
      Project a point cloud into an utm grid to produce a DEM
      The algorithm averages all the points that fall into each square of the grid
  
      Arguments:
          xyz : a Nx3 matrix representing a point cloud in (lon,lat,h) coordinates
          emin,emax,nmin,nmax : a bounding box in UTM coordinates
          resolution : the target resolution in meters (by default, 1 meter)
  
      Return:
          dem : a 2D array of heights in meters
      """
  
      # width and height of the image domain
      w = int(np.ceil((emax - emin)/resolution))
      h = int(np.ceil((nmax - nmin)/resolution))
  
      # extract and quantize columns
      x = xyz[:,0]
      y = xyz[:,1]
      z = xyz[:,2]
  
      ix = np.asarray((x - emin)/resolution, dtype="int")
      iy = np.asarray((nmax - y)/resolution, dtype="int")
  
      # per-cell average
      dem = reduceavg (w,h,  ix, iy, z )
  
      return dem
  
  