add local quadratic coherency functions (tests missing)

tools/DIC/__init__.py

-__all__ = ["correlate", "correlateGM", "deform", "grid"]
+__all__ = ["correlate", "correlateGM", "deform", "grid", "kinematics"]
 
 from .correlate import *
 from .grid import *
 from .correlateGM import *
 from .deform import *
+from .kinematics import *

tools/DIC/kinematics.py

+"""
+Library of SPAM functions for defining a regular grid in a reproducible way.
+Copyright (C) 2020 SPAM Contributors
+
+This program is free software: you can redistribute it and/or modify it
+under the terms of the GNU General Public License as published by the Free
+Software Foundation, either version 3 of the License, or (at your option)
+any later version.
+
+This program is distributed in the hope that it will be useful, but WITHOUT
+ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+more details.
+
+You should have received a copy of the GNU General Public License along with
+this program.  If not, see <http://www.gnu.org/licenses/>.
+"""
+
+import numpy
+import tifffile
+import scipy.spatial
+import multiprocessing
+import progressbar
+
+
+nProcessesDefault = multiprocessing.cpu_count()
+
+def estimateLocalQuadraticCoherency(points, displacements, neighbourRadius=150, epsilon = 0.1, nProcesses=nProcessesDefault, verbose=True):
+    '''
+    This function computes the local quadratic coherency (LQC) of a set of displacement vectors as per Masullo and Theunissen 2016.
+    LQC is the average residual between the point's displacement and a second-order (parabolic) surface Phi.
+    The quadratic surface Phi is fitted to the point's closest N neighbours and evaluated at the point's position.
+    A point with a LQC value smaller than a threshold (0.1 in Masullo and Theunissen 2016) is classified as coherent
+
+    Parameters
+    ----------
+        points : n x 3 numpy array of floats
+            Coordinates of the points Z, Y, X
+
+        displacements : n x 3 numpy array of floats
+            Displacements of the points
+
+        neighbourRadius: float, optional
+            Distance in pixels around the point to extract neighbours
+            Default = 150
+
+        epsilon: float, optional
+            Background error as per (Westerweel and Scarano 2005)
+            Default = 0.1
+
+        nProcesses : integer, optional
+            Number of processes for multiprocessing
+            Default = number of CPUs in the system
+
+        verbose : boolean, optional
+            Print progress?
+            Default = True
+
+    Returns
+    -------
+        LQC: n x 1 array of floats
+            The local quadratic coherency for each point
+
+    Note
+    -----
+        Based on: https://gricad-gitlab.univ-grenoble-alpes.fr/DVC/pt4d
+    '''
+
+    # initialise the coherency matrix
+    LQC = numpy.ones((points.shape[0]), dtype=float)
+
+    # build KD-tree for quick neighbour identification
+    treeCoord = scipy.spatial.KDTree(points)
+
+    # calculate coherency for each point
+    global coherencyOnePoint
+    def coherencyOnePoint(point):
+        radius  = neighbourRadius
+        indices = numpy.array(treeCoord.query_ball_point(points[point], radius))
+        while len(indices) <= 27: #TODO this could vary (even a treeCoord.query could be used instead)
+            radius *= 2
+            indices = numpy.array(treeCoord.query_ball_point(points[point], radius))
+
+        N = len(indices)
+        # fill in point+neighbours positions for the parabolic surface coefficients
+        X     = numpy.zeros((N, 10), dtype=float)
+        for i, neighbour in enumerate(indices):
+            pos     = points[neighbour]
+            X[i, 0] = 1
+            X[i, 1] = pos[0]
+            X[i, 2] = pos[1]
+            X[i, 3] = pos[2]
+            X[i, 4] = pos[0] * pos[1]
+            X[i, 5] = pos[0] * pos[2]
+            X[i, 6] = pos[1] * pos[2]
+            X[i, 7] = pos[0] * pos[0]
+            X[i, 8] = pos[1] * pos[1]
+            X[i, 9] = pos[2] * pos[2]
+
+        # keep point's index
+        i0 = numpy.where(indices == point)[0][0]
+
+        # fill in disp
+        u = displacements[indices, 0]
+        v = displacements[indices, 1]
+        w = displacements[indices, 2]
+        UnormMedian = numpy.median(numpy.linalg.norm(displacements[indices], axis=1))
+
+        # deviation of each disp vector from local median
+        sigma2 = (u-numpy.median(u))**2 + (v-numpy.median(v))**2 + (w-numpy.median(w))**2
+
+        # coefficient for gaussian weighting
+        K = (numpy.sqrt(sigma2).sum())/N
+        K += epsilon
+
+        # fill in gaussian weighting diag components
+        Wg = numpy.exp(-0.5* sigma2 * K**(-0.5))
+        # create the diag matrix
+        Wdiag = numpy.diag(Wg)
+
+        # create matrices to solve with least-squares
+        XtWX     = numpy.dot(X.T, numpy.dot(Wdiag, X))
+        XtWXInv  = numpy.linalg.inv(XtWX) # TODO: check for singular matrix
+        XtWUu    = numpy.dot(X.T, numpy.dot(Wdiag, u))
+        XtWUv    = numpy.dot(X.T, numpy.dot(Wdiag, v))
+        XtWUw    = numpy.dot(X.T, numpy.dot(Wdiag, w))
+
+        # solve least-squares to compute the coefficients of the parabolic surface
+        au = numpy.dot(XtWXInv, XtWUu)
+        av = numpy.dot(XtWXInv, XtWUv)
+        aw = numpy.dot(XtWXInv, XtWUw)
+
+        # evaluate parabolic surface at point's position
+        phiu = numpy.dot(au, X[i0, :])
+        phiv = numpy.dot(av, X[i0, :])
+        phiw = numpy.dot(aw, X[i0, :])
+
+        # compute normalised residuals
+        Cu = (phiu - u[i0])**2 / (UnormMedian + epsilon)**2
+        Cv = (phiv - v[i0])**2 / (UnormMedian + epsilon)**2
+        Cw = (phiw - w[i0])**2 / (UnormMedian + epsilon)**2
+
+        # return coherency as the average normalised residual
+        return point, (Cu + Cv + Cw)/3
+
+    if verbose:
+        pbar = progressbar.ProgressBar(maxval=points.shape[0]).start()
+        finishedPoints = 0
+
+    with multiprocessing.Pool(processes=nProcesses) as pool:
+        for returns in pool.imap_unordered(coherencyOnePoint, range(points.shape[0])):
+            if verbose:
+                finishedPoints += 1
+                pbar.update(finishedPoints)
+            LQC[returns[0]] = returns[1]
+
+    if verbose: pbar.finish()
+
+    return LQC
+
+def estimateDisplacementFromQuadraticFit(points, displacements, coherency, coherencyThreshold=0.1, neighbourRadius=150, epsilon=0.1, nProcesses=nProcessesDefault, verbose=True):
+    '''
+    This function estimates the displacement of an incoherent point based on a local quadratic fit
+    of the displacements of N coherent neighbours, as per Masullo and Theunissen 2016.
+    A quadratic surface Phi is fitted to the point's closest coherent neighbours
+
+    Parameters
+    ----------
+        points : n x 3 numpy array of floats
+            Coordinates of the points Z, Y, X
+
+        displacements : n x 3 numpy array of floats
+            Displacements of the points
+
+        coherency: n x 1 numpy array of floats
+            The local quadratic coherency for each point
+            The result of `estimateLocalQuadraticCoherency()`
+
+        coherencyThreshold: float, optional
+            Value below which the point is considered as coherent
+            Default = 0.1 (as per Masullo and Theunissen 2016)
+
+        neighbourRadius: float, optional
+            Distance around the point to extract neighbours
+            Default = 150
+
+        epsilon: float, optional
+            Background error as per (Westerweel and Scarano 2005)
+            Default = 0.1
+
+        nProcesses : integer, optional
+            Number of processes for multiprocessing
+            Default = number of CPUs in the system
+
+        verbose : boolean, optional
+            Print progress?
+            Default = True
+
+    Returns
+    -------
+        displacements: n x 3 array of floats
+            The filtered displacement array
+            For a coherent point is the input displacement
+            For an incoherent point is the displacement estimated based on the quadratic fit
+
+    Note
+    -----
+        Based on https://gricad-gitlab.univ-grenoble-alpes.fr/DVC/pt4d
+    '''
+
+    # define coherent and incoherent points based on coherency threshold
+    goodPoints = numpy.where(coherency < coherencyThreshold)
+    badPoints  = numpy.where(coherency >= coherencyThreshold)
+
+    # initialise disp of incoherent points
+    displacementsBad = numpy.zeros_like(displacements[badPoints])
+
+    # build KD-tree of coherent points for quick neighbour identification
+    treeCoord = scipy.spatial.KDTree(points[goodPoints])
+
+    # estimate disp for each incoherent point
+    global dispOnePoint
+    def dispOnePoint(badPoint):
+        radius = neighbourRadius
+        indices = numpy.array(treeCoord.query_ball_point(points[badPoints][badPoint], radius))
+        while len(indices) <= 27: #TODO this could vary (even a treeCoord.query could be used instead)
+            radius *= 2
+            indices = numpy.array(treeCoord.query_ball_point(points[badPoints][badPoint], radius))
+
+        N = len(indices)
+        # fill in neighbours positions for the parabolic surface coefficients
+        X     = numpy.zeros((N, 10), dtype=float)
+        for i, neighbour in enumerate(indices):
+            pos     = points[goodPoints][neighbour]
+            X[i, 0] = 1
+            X[i, 1] = pos[0]
+            X[i, 2] = pos[1]
+            X[i, 3] = pos[2]
+            X[i, 4] = pos[0] * pos[1]
+            X[i, 5] = pos[0] * pos[2]
+            X[i, 6] = pos[1] * pos[2]
+            X[i, 7] = pos[0] * pos[0]
+            X[i, 8] = pos[1] * pos[1]
+            X[i, 9] = pos[2] * pos[2]
+
+        # fill in point's position for the evaluation of the parabolic surface
+        pos0   = points[badPoints][badPoint]
+        X0     = numpy.zeros((10), dtype=float)
+        X0[0]  = 1
+        X0[1]  = pos0[0]
+        X0[2]  = pos0[1]
+        X0[3]  = pos0[2]
+        X0[4]  = pos0[0] * pos0[1]
+        X0[5]  = pos0[0] * pos0[2]
+        X0[6]  = pos0[1] * pos0[2]
+        X0[7]  = pos0[0] * pos0[0]
+        X0[8]  = pos0[1] * pos0[1]
+        X0[9]  = pos0[2] * pos0[2]
+
+        # fill in disp of neighbours
+        u = displacements[goodPoints][indices, 0]
+        v = displacements[goodPoints][indices, 1]
+        w = displacements[goodPoints][indices, 2]
+        UnormMedian = numpy.median(numpy.linalg.norm(displacements[goodPoints][indices], axis=1))
+
+        # deviation of each disp vector from local median
+        sigma2 = (u-numpy.median(u))**2 + (v-numpy.median(v))**2 + (w-numpy.median(w))**2
+
+        # coefficient for gaussian weighting
+        K = (numpy.sqrt(sigma2).sum())/N
+        K += epsilon
+
+        # fill in gaussian weighting diag components
+        Wg = numpy.exp(-0.5* sigma2 * K**(-0.5)) # careful I think the first 0.5 was missing
+        # create the diag matrix
+        Wdiag = numpy.diag(Wg)
+
+        # create matrices to solve with least-squares
+        XtWX    = numpy.dot(X.T, numpy.dot(Wdiag, X))
+        XtWXInv  = numpy.linalg.inv(XtWX) # TODO: check for singular matrix
+        XtWUu   = numpy.dot(X.T, numpy.dot(Wdiag, u))
+        XtWUv   = numpy.dot(X.T, numpy.dot(Wdiag, v))
+        XtWUw   = numpy.dot(X.T, numpy.dot(Wdiag, w))
+
+        # solve least-squares to compute the coefficients of the parabolic surface
+        au = numpy.dot(XtWXInv, XtWUu)
+        av = numpy.dot(XtWXInv, XtWUv)
+        aw = numpy.dot(XtWXInv, XtWUw)
+
+        # evaluate parabolic surface at incoherent point's position
+        phiu = numpy.dot(au, X0)
+        phiv = numpy.dot(av, X0)
+        phiw = numpy.dot(aw, X0)
+
+        return badPoint, [phiu, phiv, phiw]
+
+    # Iterate through flat field of Fs
+    if verbose:
+        pbar = progressbar.ProgressBar(maxval=len(badPoints[0])).start()
+        finishedPoints = 0
+
+    # Run multiprocessing filling in FfieldFlatGood, which will then update FfieldFlat
+    with multiprocessing.Pool(processes=nProcesses) as pool:
+        for returns in pool.imap_unordered(dispOnePoint, range(len(badPoints[0]))):
+            if verbose:
+                finishedPoints += 1
+                pbar.update(finishedPoints)
+            displacementsBad[returns[0]] = returns[1]
+
+    if verbose: pbar.finish()
+
+    # overwrite bad points displacements
+    displacements[badPoints] = displacementsBad
+    return displacements
+
+