Merge pull request #70 from abel-research/regBaryAdjust

Reg bary adjust

.gitignore

@@ -50,4 +50,5 @@ fill_test.stl
 tests/obj.obj
 tests/stl_file_2_hc.stl
 tests/stl_file_2_lp.stl
-tests/stl_file_trim.stl
+tests/stl_file_trim.stl
+tests/reg_test*

ampscan/core.py

@@ -53,7 +53,8 @@ def __init__(self, data=None, stype='limb', unify=True, struc=True):
         elif isinstance(data, dict):
             for k, v in data.items():
                 setattr(self, k, v)
-            self.calcStruct()
+            if struc is True:
+                self.calcStruct()
         elif isinstance(data, bytes):
             self.read_bytes(data, unify, struc)
 

ampscan/registration.py

@@ -122,7 +122,9 @@ def point2plane(self, steps = 1, neigh = 10, inside = True, subset = None,
         self.reg = AmpObject(regData, stype='reg')
         self.disp = AmpObject({'vert': np.zeros(self.reg.vert.shape),
                                'faces': self.reg.faces,
-                               'values':self.reg.values})
+                               'values':self.reg.values}, struc=False)
+        self.disp.calcEdges()
+
         if scale is not None:
             tmin = self.t.vert.min(axis=0)[2]
             rmin = self.reg.vert.min(axis=0)[2]
@@ -139,6 +141,8 @@ def point2plane(self, steps = 1, neigh = 10, inside = True, subset = None,
         for step in np.arange(steps, 0, -1, dtype=float):
             # Index of 10 centroids nearest to each baseline vertex
             ind = tTree.query(self.reg.vert, neigh)[1]
+            if ind.ndim == 1:
+                ind = ind[:, None]
             # Define normals for faces of nearest faces
             norms = normals[ind]
             # Get a point on each face
@@ -149,26 +153,29 @@ def point2plane(self, steps = 1, neigh = 10, inside = True, subset = None,
             # Calculate the vector from old point to new point
             G = self.reg.vert[:, None, :] + np.einsum('ijk, ij->ijk', norms, t)
             # Ensure new points lie inside points otherwise set to 99999
+            # print(G.shape, ind.shape)
+            if inside is True:
+                # Adjust so inside face 
+                G = self.__adjustBarycentric(self.reg.vert, G, ind)
+                # G = self.__calcBarycentric(self.reg.vert, G, ind)
             # Find smallest distance from old to new point 
-            if inside is False:
-                G = G - self.reg.vert[:, None, :]
-                GMag = np.sqrt(np.einsum('ijk, ijk->ij', G, G))
-                GInd = GMag.argmin(axis=1)
-            else:
-                G, GInd = self.__calcBarycentric(self.reg.vert, G, ind)
+            G = G - self.reg.vert[:, None, :]
+            GMag = np.sqrt(np.einsum('ijk, ijk->ij', G, G))
+            GInd = GMag.argmin(axis=1)
             # Define vector from baseline point to intersect point
             D = G[np.arange(len(G)), GInd, :]
 #            rVert += D/step
             self.disp.vert += D/step
             if smooth > 0 and step > 1:
-                self.disp.hc_smooth(smooth, beta=0.6,  brim = fixBrim)
+                self.disp.hc_smooth(smooth, beta=0.6,  brim = fixBrim, norms=False)
                 self.reg.vert = self.b.vert + self.disp.vert
             else:
                 self.reg.vert = self.b.vert + self.disp.vert
-                self.reg.calcNorm()
+                self.reg.adjustCoincident(beta=0.6)
         self.reg.calcStruct(vNorm=True)
         self.values = self.calcError(error)
         self.reg.values[:] = self.values
+
 
     def calcError(self, method='norm'):
         r"""
@@ -193,8 +200,7 @@ def calcError(self, method='norm'):
             values = getattr(self, method)()
             return values
         except: ValueError('"%s" is not a method, try "abs", "cent" or "prod"' % method)
-
-
+
 
     def __absDist(self):
         r"""
@@ -266,9 +272,7 @@ def __calcBarycentric(self, vert, G, ind):
         G: array_like 
             The new array of candidates for registered vertices, from here, the one with 
             smallest magnitude is selected. All these points will lie within the target face
-        GInd: array_like
-            The index of the shortest distance between each baseline vertex and the registered vertex
-            
+
         """
         P0 = self.t.vert[self.t.faces[ind, 0]]
         P1 = self.t.vert[self.t.faces[ind, 1]]
@@ -297,10 +301,76 @@ def __calcBarycentric(self, vert, G, ind):
         i, j = np.meshgrid(np.arange(P.shape[0]), np.arange(P.shape[1]))
         nearP = P[i.T, j.T, :, pdx]
         G[~logic, :] = nearP[~logic, :]
-        G = G - vert[:, None, :]
-        GMag = np.sqrt(np.einsum('ijk, ijk->ij', G, G))
-        GInd = GMag.argmin(axis=1)
-        return G, GInd
+        return G
+
+
+    def __adjustBarycentric(self, vert, G, ind):
+        r"""
+        Calculate the barycentric co-ordinates of each target face and the registered vertex, 
+        this ensures that the registered vertex is within the bounds of the target face. If not 
+        the registered vertex is moved to the nearest vertex or edge on the target face 
+
+        Parameters
+        ----------
+        vert: array_like
+            The array of baseline vertices
+        G: array_like
+            The array of candidates for registered vertices. If neigh>1 then axis 2 will correspond 
+            to the number of nearest neighbours selected
+        ind: array_like
+            The index of the nearest faces to the baseline vertices
+        
+        Returns
+        -------
+        G: array_like 
+            The new array of candidates for registered vertices, from here, the one with 
+            smallest magnitude is selected. All these points will lie within the target face
+ 
+        """
+        P0 = self.t.vert[self.t.faces[ind, 0]]
+        P1 = self.t.vert[self.t.faces[ind, 1]]
+        P2 = self.t.vert[self.t.faces[ind, 2]]
+
+
+        v0 = P2 - P0
+        v1 = P1 - P0
+        v2 = G - P0
+
+        d00 = np.einsum('ijk, ijk->ij', v0, v0)
+        d01 = np.einsum('ijk, ijk->ij', v0, v1)
+        d02 = np.einsum('ijk, ijk->ij', v0, v2)
+        d11 = np.einsum('ijk, ijk->ij', v1, v1)
+        d12 = np.einsum('ijk, ijk->ij', v1, v2)
+
+        # Compute barycentric co-ordinates
+        denom = d00*d11 - d01*d01
+        u = (d11 * d02 - d01 * d12)/denom
+        v = (d00 * d12 - d01 * d02)/denom
+        w = 1 - u - v
+
+        # Logic for adjustment 
+        P0_log = (w > 0) * (v < 0) * (u < 0)
+        P1_log = (w < 0) * (v > 0) * (u < 0)
+        P2_log = (w < 0) * (v < 0) * (u > 0)
+        P0P1_log = (w > 0) * (v > 0) * (u < 0)
+        P0P2_log = (w > 0) * (v < 0) * (u > 0)
+        P1P2_log = (w < 0) * (v > 0) * (u > 0)
+
+        G[P0_log, :] = P0[P0_log, :]
+        G[P1_log, :] = P1[P1_log, :]
+        G[P2_log, :] = P2[P2_log, :]
+
+        # Compute line intersection 
+        for pa, pb, log in [[P0, P1, P0P1_log], [P0, P2, P0P2_log], [P1, P2, P1P2_log]]:
+            s = pb - pa
+            t = G - pa
+            ps = np.einsum('ijk, ijk->ij', t, s)
+            l2 = np.einsum('ijk, ijk->ij', s, s)
+            newG = pa + ps[:, :, None] / l2[:, :, None] * s
+            G[log, :] = newG[log, :]
+        return G
+
+
 
     def plotResults(self, name=None, xrange=None, color=None, alpha=None):
         r"""

ampscan/smooth.py

@@ -4,6 +4,7 @@
 Copyright: Joshua Steer 2020, Joshua.Steer@soton.ac.uk
 """
 
+from itertools import count
 import numpy as np
 import copy
 
@@ -49,7 +50,7 @@ def lp_smooth(self, n=1, brim = True):
         self.calcNorm()
         self.calcVNorm()
 
-    def hc_smooth(self, n=1 ,beta=0.6, brim=True):
+    def hc_smooth(self, n=1 ,beta=0.6, brim=True, norms = True):
         r"""
         Function to apply a Humphrey’s Classes smooth to the mesh. Note, this assumes
         that alpha=0 (ie the original point through the iteration has no effect). 
@@ -102,9 +103,50 @@ def hc_smooth(self, n=1 ,beta=0.6, brim=True):
                 d = (adj - q).mean(axis=0)
                 # Based upon beta, get the updated location 
                 self.vert[j, :] = q + beta*b - (1-beta)*d
+        if norms is True:
+            self.calcNorm()
+            self.calcVNorm()
+
+    def adjustCoincident(self, maxiter = 10, beta = 1):
+        r"""
+        Adjust any coincident vertices via a hc smooth
+        """
+        e = self.edges.flatten()
+        # Get the indicies to sort edges 
+        o_idx = e.argsort()
+        # Get indicies of sorted array where last of each vertex index 
+        # occurs 
+        ndx = np.searchsorted(e[o_idx], np.arange(len(self.vert)), 
+                              side='right')
+        ndx = np.r_[0, ndx]
+        # Map indicies between flatted edges array and standard
+        row, col = np.unravel_index(o_idx, self.edges.shape)
+        for i in range(maxiter):
+            # List all vertices 
+            vert = copy.deepcopy(self.getVert())
+            neighVerts = vert[self.edges[row, 1-col], :]
+            unq_vert, indC, unq_count = np.unique(vert, return_inverse=True, return_counts=True, axis=0)
+            vert_count = unq_count[indC]
+            vRange = np.arange(vert.shape[0])
+            vRange = vRange[vert_count > 1]
+            if vRange.size == 0:
+                break
+            for j in vRange:
+                # Get the adjacent vertices
+                adj = neighVerts[ndx[j]:ndx[j+1]]
+                # Get the original vertex
+                q = self.vert[j, :]
+                # calculate new Laplacian location 
+                p = adj.mean(axis=0)
+                # Distance between Laplacian and original 
+                b = p - q
+                # Mean distance adjacent between original 
+                d = (adj - q).mean(axis=0)
+                # Based upon beta, get the updated location 
+                self.vert[j, :] = q + beta*b - (1-beta)*d
         self.calcNorm()
         self.calcVNorm()
-    
+
     def smoothValues(self, n=1):
         """
         Function to apply a simple Laplacian smooth to the values array. 

tests/test_smoothing.py

@@ -6,6 +6,7 @@
 from util import get_path
 from ampscan import analyse
 import math
+import numpy as np
 
 
 class TestSmoothing(unittest.TestCase):
@@ -21,6 +22,7 @@ def setUp(self):
         self.amp = AmpObject(stl_path)
         self.amp2 = AmpObject(stl_path)
         self.amp3 = AmpObject(stl_path)
+        self.amp4 = AmpObject(stl_path)
 
     def test_smoothing_nans(self):
         """Tests that NaNs are properly dealt with by smooth method"""
@@ -47,4 +49,16 @@ def test_smoothing_volume(self):
         print(vol3)
         # self.assertAlmostEqual(analyse.est_volume(poly1), analyse.est_volume(poly3), delta=TestSmoothing.DELTA)
         self.assertLess(vol1-vol3, vol1-vol2)
+
+    def test_coincident(self):
+        idx_max = np.argmax(self.amp4.vert[:, 1])
+        idx_min = np.argmin(self.amp4.vert[:, 1])
+        delta = self.amp4.vert[idx_max, 1] - self.amp4.vert[idx_min, 1]
+        self.amp4.vert[idx_max, :] = self.amp4.vert[idx_min, :]
+        self.amp4.vert[idx_max, :] = self.amp4.vert[idx_min, :]
+        self.amp4.adjustCoincident(beta=1)
+        delta2 = self.amp4.vert[idx_max, 1] - self.amp4.vert[idx_min, 1]
+        self.assertGreater(delta2, delta*0.99)
+
+