add mesh registration and cache face_normals

Author: mikedh

examples/scan_register.py

@@ -0,0 +1,103 @@
+"""
+scan_register.py
+-------------
+
+Create some simulated 3D scan data and register 
+it to a "truth" mesh.
+"""
+
+import trimesh
+import numpy as np
+
+
+def simulated_brick(face_count, extents, noise, max_iter=10):
+    """
+    Produce a mesh that is a rectangular solid with noise
+    with a random transform.
+
+    Parameters
+    -------------
+    face_count : int
+      Approximate number of faces desired
+    extents : (n,3) float
+      Dimensions of brick
+    noise : float
+      Magnitude of vertex noise to apply
+    """
+
+    # create the mesh as a simple box
+    mesh = trimesh.creation.box(extents=extents)
+
+    # add some systematic error pre- tesselation
+    mesh.vertices[0] += mesh.vertex_normals[0] + (noise * 2)
+    
+    # subdivide until we have more faces than we want
+    for i in range(max_iter):
+        if len(mesh.vertices) > face_count:
+            break
+        mesh = mesh.subdivide()
+
+    # apply tesselation and random noise
+    mesh = mesh.permutate.noise(noise)
+
+    # randomly rotation with translation
+    transform = trimesh.transformations.random_rotation_matrix()
+    transform[:3, 3] = (np.random.random(3) - .5) * 1000
+    
+    mesh.apply_transform(transform)
+    
+    return mesh
+
+
+if __name__ == '__main__':
+    # print log messages to terminal
+    trimesh.util.attach_to_log()
+    
+    # the size of our boxes
+    extents = [6, 12, 2]
+    
+    # create a simulated brick with noise and random transform
+    scan = simulated_brick(face_count=5000,
+                           extents=extents,
+                           noise=.05)
+
+    # create a "true" mesh
+    truth = trimesh.creation.box(extents=extents)
+
+    # (4, 4) float homogenous transform from truth to scan
+    truth_to_scan, cost = truth.register(scan)
+
+    print("centroid distance pre-registration:",
+          np.linalg.norm(truth.centroid - scan.centroid))
+
+    # apply the registration transform
+    truth.apply_transform(truth_to_scan)
+    print("centroid distance post-registration:",
+          np.linalg.norm(truth.centroid - scan.centroid))
+    
+    # find the distance from the truth mesh to each scan vertex
+    distance = truth.nearest.on_surface(scan.vertices)[1]
+    # 0.0 - 1.0 distance fraction
+    fraction = distance / distance.max()
+
+    # color the mesh by distance from truth with
+    # linear interpolation between two colors
+    colors = ([255,0,0,255] * fraction.reshape((-1,1)) +
+              [0,255,0,255] * (1 - fraction).reshape((-1,1)))
+    # apply the vertex colors to the scan mesh
+    scan.visual.vertex_colors = colors.astype(np.uint8)
+
+    # print some quick statistics about the mesh
+    print('distance max:', distance.max())
+    print('distance mean:', distance.mean())
+    print('distance STD:', distance.std())
+    
+    # export result with vertex colors for meshlab
+    scan.export('scan_new.ply')
+
+    # show in a pyglet window
+    scan.show()
+
+    
+    
+    

tests/test_registration.py

@@ -59,6 +59,49 @@ def test_icp_points(self):
                              g.np.linalg.inv(matrixN))
         assert g.np.allclose(transformed, points_a[index])
 
+    def test_mesh(self):
+        noise = .05
+        extents = [6, 12, 3]
+
+        # create the mesh as a simple box
+        mesh = g.trimesh.creation.box(extents=extents)
+
+        # subdivide until we have more faces than we want
+        for i in range(3):
+            mesh = mesh.subdivide()
+        # apply tesselation and random noise
+        mesh = mesh.permutate.noise(noise)
+        # randomly rotation with translation
+        transform = g.trimesh.transformations.random_rotation_matrix()
+        transform[:3, 3] = (g.np.random.random(3) - .5) * 1000
+
+        mesh.apply_transform(transform)
+
+        scan = mesh
+        # create a "true" mesh
+        truth = g.trimesh.creation.box(extents=extents)
+
+        for a, b in [[truth, scan], [scan, truth]]:
+            a_to_b, cost = a.register(b)
+
+            a_check = a.copy()
+            a_check.apply_transform(a_to_b)
+
+            assert g.np.linalg.norm(a_check.centroid - b.centroid) < (noise * 2)
+
+            # find the distance from the truth mesh to each scan vertex
+            distance = a_check.nearest.on_surface(b.vertices)[1]
+            assert distance.max() < (noise * 2)
+
+        # try our registration with points
+        points = g.trimesh.transform_points(
+            scan.sample(100),
+            matrix=g.trimesh.transformations.random_rotation_matrix())
+        truth_to_points, cost = truth.register(points)
+        truth.apply_transform(truth_to_points)
+        distance = truth.nearest.on_surface(points)[1]
+        assert distance.mean() < noise
+
 
 if __name__ == '__main__':
     g.trimesh.util.attach_to_log()

trimesh/base.py

@@ -331,12 +331,18 @@ def face_normals(self):
 
         # if all triangles are valid shape is correct
         if valid.all():
+            # put calculated face normals into cache manually
+            self._cache['face_normals'] = normals
             return normals
 
         # make a padded list of normals for correct shape
         padded = np.zeros((len(self.triangles), 3),
                           dtype=np.float64)
         padded[valid] = normals
+
+        # put calculated face normals into cache manually
+        self._cache['face_normals'] = padded
+
         return padded
 
     @face_normals.setter
@@ -1674,6 +1680,39 @@ def fill_holes(self):
         """
         return repair.fill_holes(self)
 
+    def register(self, other, **kwargs):
+        """
+        Align a mesh with another mesh or a PointCloud using
+        the principal axes of inertia as a starting point which
+        is refined by iterative closest point.
+
+        Parameters
+        ------------
+        mesh : trimesh.Trimesh object
+          Mesh to align with other
+        other : trimesh.Trimesh or (n, 3) float
+          Mesh or points in space
+        samples : int
+          Number of samples from mesh surface to align
+        icp_first : int
+          How many ICP iterations for the 9 possible
+          combinations of
+        icp_final : int
+          How many ICP itertations for the closest
+          candidate from the wider search
+
+        Returns
+        -----------
+        mesh_to_other : (4, 4) float
+          Transform to align mesh to the other object
+        cost : float
+          Average square distance per point
+        """
+        mesh_to_other, cost = registration.mesh_other(mesh=self,
+                                                      other=other,
+                                                      **kwargs)
+        return mesh_to_other, cost
+
     def compute_stable_poses(self,
                              center_mass=None,
                              sigma=0.0,

trimesh/points.py

@@ -320,6 +320,19 @@ def merge_vertices(self):
                 len(self.colors) == len(inverse)):
             self.colors = self.colors[unique]
 
+    def apply_transform(self, transform):
+        """
+        Apply a homogenous transformation to the PointCloud
+        object in- place.
+
+        Parameters
+        --------------
+        transform : (4, 4) float
+          Homogenous transformation to apply to PointCloud
+        """
+        self.vertices = transformations.transform_points(self.vertices,
+                                                         matrix=transform)
+
     @property
     def bounds(self):
         """

trimesh/registration.py

@@ -10,9 +10,149 @@
 from scipy.spatial import cKDTree
 
 from . import util
+from . import bounds
+from . import transformations
+
 from .transformations import transform_points
 
 
+def key_points(mesh, count):
+    """
+    Return a combination of mesh vertices and surface samples
+    with vertices chosen by likelyhood to be important to registation
+    """
+    stack = []
+    if len(mesh.vertices) < (count / 2):
+        return np.vstack((
+            mesh.vertices,
+            mesh.sample(count - len(mesh.vertices))))
+    else:
+        return mesh.sample(count)
+
+
+def mesh_other(mesh, other, samples=500, icp_first=10, icp_final=50):
+    """
+    Align a mesh with another mesh or a PointCloud using
+    the principal axes of inertia as a starting point which
+    is refined by iterative closest point.
+
+    Parameters
+    ------------
+    mesh : trimesh.Trimesh object
+      Mesh to align with other
+    other : trimesh.Trimesh or (n, 3) float
+      Mesh or points in space
+    samples : int
+      Number of samples from mesh surface to align
+    icp_first : int
+      How many ICP iterations for the 9 possible
+      combinations of
+    icp_final : int
+      How many ICP itertations for the closest
+      candidate from the wider search
+
+    Returns
+    -----------
+    mesh_to_other : (4, 4) float
+      Transform to align mesh to the other object
+    cost : float
+      Average squared distance per point
+    """
+
+    if not util.is_instance_named(mesh, 'Trimesh'):
+        raise ValueError('mesh must be Trimesh object!')
+
+    inverse = True
+    search = mesh
+    # if both are meshes use the smaller one for searching
+    if util.is_instance_named(other, 'Trimesh'):
+        if len(mesh.vertices) > len(other.vertices):
+            search = other
+            inverse = False
+            points = key_points(mesh=mesh,
+                                count=samples)
+            points_mesh = mesh
+        else:
+            points_mesh = other
+            points = key_points(mesh=other,
+                                count=samples)
+
+        if points_mesh.is_volume:
+            points_PIT = points_mesh.principal_inertia_transform
+        else:
+            points_PIT = points_mesh.bounding_box_oriented.principal_inertia_transform
+
+    elif util.is_shape(other, (-1, 3)):
+        # case where other is just points
+        points = other
+        points_PIT = bounds.oriented_bounds(points)[0]
+    else:
+        raise ValueError('other must be mesh or (n, 3) points!')
+
+    if search.is_volume:
+        search_PIT = search.principal_inertia_transform
+    else:
+        search_PIT = search.bounding_box_oriented.principal_inertia_transform
+
+        # move from mesh a to mesh b
+    search_to_points = np.dot(np.linalg.inv(points_PIT), search_PIT)
+
+    # permutations of cube rotations
+    # the principal inertia transform has arbitrary sign
+    # along the 3 major axis so try all combinations of
+    # 180 degree rotations with a quick first ICP pass
+    cubes = np.array([np.eye(4) * np.append(diag, 1)
+                      for diag in [[1, 1, 1],
+                                   [1, 1, -1],
+                                   [1, -1, 1],
+                                   [-1, 1, 1],
+                                   [-1, -1, 1],
+                                   [-1, 1, -1],
+                                   [1, -1, -1],
+                                   [-1, -1, -1]]])
+
+    #from IPython import embed
+    # embed()
+
+    # loop through permutations and run iterative closest point on each
+    costs, transforms = [], []
+    centroid = search.centroid
+    for flip in cubes:
+        a_to_b = np.dot(transformations.transform_around(flip, centroid),
+                        np.linalg.inv(search_to_points))
+
+        # import trimesh
+        # vpt = trimesh.PointCloud(points)
+        # vpt.apply_transform(a_to_b)
+        # trimesh.Scene([search, vpt]).show()
+
+        # run first pass ICP
+        matrix, junk, cost = icp(a=points,
+                                 b=search,
+                                 initial=a_to_b,
+                                 max_iterations=int(icp_first),
+                                 scale=False)
+        transforms.append(matrix)
+        costs.append(cost)
+
+    # run a final ICP refinement step
+    matrix, junk, cost = icp(a=points,
+                             b=search,
+                             initial=transforms[np.argmin(costs)],
+                             max_iterations=int(icp_final),
+                             scale=False)
+
+    # convert square sum distance to squared average distance
+    cost /= len(points)
+
+    if inverse:
+        mesh_to_other = np.linalg.inv(matrix)
+    else:
+        mesh_to_other = matrix
+
+    return mesh_to_other, cost
+
+
 def procrustes(a,
                b,
                reflection=True,

trimesh/version.py

@@ -1 +1 @@
-__version__ = '2.33.37'
+__version__ = '2.33.38'