def find_rigid_transform(a, b, visualize=False):
"""
Args:
a: a 3xN array of vertex locations
b: a 3xN array of vertex locations
Returns: (R,T) such that R.dot(a)+T ~= b
Based on Arun et al, "Least-squares fitting of two 3-D point sets," 1987.
See also Eggert et al, "Estimating 3-D rigid body transformations: a
comparison of four major algorithms," 1997.
"""
import numpy as np
import scipy.linalg
if a.shape[0] != 3:
if a.shape[1] == 3:
a = a.T
if b.shape[0] != 3:
if b.shape[1] == 3:
b = b.T
assert a.shape[0] == 3
assert b.shape[0] == 3
a_mean = np.mean(a, axis=1)
b_mean = np.mean(b, axis=1)
a_centered = a - a_mean.reshape(-1, 1)
b_centered = b - b_mean.reshape(-1, 1)
c = a_centered.dot(b_centered.T)
u, s, v = np.linalg.svd(c, full_matrices=False)
v = v.T
R = v.dot(u.T)
if scipy.linalg.det(R) < 0:
if np.any(s == 0
): # This is only valid in the noiseless case; see the paper
v[:, 2] = -v[:, 2]
R = v.dot(u.T)
else:
raise ValueError(
"find_rigid_transform found a reflection that it cannot recover from. Try RANSAC or something..."
)
T = (b_mean - R.dot(a_mean)).reshape(-1, 1)
if visualize != False:
from lace.mesh import Mesh
from lace.meshviewer import MeshViewer
mv = MeshViewer() if visualize is True else visualize
a_T = R.dot(a) + T
mv.set_dynamic_meshes([
Mesh(v=a.T, f=[]).set_vertex_colors("red"),
Mesh(v=b.T, f=[]).set_vertex_colors("green"),
Mesh(v=a_T.T, f=[]).set_vertex_colors("orange"),
])
return R, T
def test_mesh_from_mesh(self):
m = Mesh(
v=np.random.randn(10, 3),
f=np.random.randn(10, 3),
vc=np.random.randn(10, 3),
vn=np.random.randn(10, 3),
)
m2 = Mesh(m)
np.testing.assert_array_equal(m.v, m2.v)
np.testing.assert_array_equal(m.f, m2.f)
np.testing.assert_array_equal(m.vc, m2.vc)
np.testing.assert_array_equal(m.vn, m2.vn)
self.assertNotEqual(id(m.v), id(m2.v))
def test_flip_faces(self):
raw_box = Mesh(vc('/unittest/serialization/obj/test_box_simple.obj'))
box = Mesh(v=raw_box.v, f=raw_box.f)
box.reset_normals()
original_vn = box.vn.copy()
original_f = box.f.copy()
box.flip_faces()
box.reset_normals()
self.assertEqual(box.f.shape, original_f.shape)
for face, orig_face in zip(box.f, original_f):
self.assertNotEqual(list(face), list(orig_face))
self.assertEqual(set(face), set(orig_face))
np.testing.assert_array_almost_equal(box.vn, np.negative(original_vn))
def load_texture(self, texture_version):
'''
Expect a texture version number as an integer, load the texture version from /is/ps/shared/data/body/template/texture_coordinates/.
Currently there are versions [0, 1, 2, 3] availiable.
'''
import numpy as np
lowres_tex_template = 's3://bodylabs-korper-assets/is/ps/shared/data/body/template/texture_coordinates/textured_template_low_v%d.obj' % texture_version
highres_tex_template = 's3://bodylabs-korper-assets/is/ps/shared/data/body/template/texture_coordinates/textured_template_high_v%d.obj' % texture_version
from lace.mesh import Mesh
from lace.cache import sc
mesh_with_texture = Mesh(filename=sc(lowres_tex_template))
if not np.all(mesh_with_texture.f.shape == self.f.shape):
mesh_with_texture = Mesh(filename=sc(highres_tex_template))
self.transfer_texture(mesh_with_texture)
def test_concatenate_vertices_only(self):
v1 = np.array([
[0., 0., 0.],
[5., 5., 5.],
[5., 0., 5.],
])
m1 = Mesh(v=v1)
v2 = np.array([
[0., 0., 0.],
[5., -5., 5.],
[5., 0., 5.],
])
m2 = Mesh(v=v2)
self.assertEqual(Mesh.concatenate(m1, m2).v.shape[0], 6)
def test_explicit_landmarks_as_dict_of_indexes(self):
m = Mesh(filename=self.template_fname, landmarks={'foo': 0, 'bar': 7})
self.assertEqual(m.landm, {'foo': 0, 'bar': 7})
self.assertDictOfArraysAlmostEqual(m.landm_xyz, {
'foo': self.template.v[0],
'bar': self.template.v[7]
})
def test_writing_binary_ply_locally_using_serializer(self):
local_file = os.path.join(
self.tmp_dir,
"test_writing_binary_ply_locally_using_serializer.ply")
m = Mesh(filename=self.test_ply_path)
ply.dump(m, local_file)
self.assertFilesEqual(local_file, self.test_bin_ply_path)
def test_writing_ascii_ply_locally_using_mesh_write_ply(self):
local_file = os.path.join(
self.tmp_dir,
"test_writing_ascii_ply_locally_using_mesh_write_ply.ply")
m = Mesh(filename=self.test_ply_path)
m.write_ply(local_file, ascii=True)
self.assertFilesEqual(local_file, self.test_ply_path)
def test_scale_vertex_colors(self):
m = Mesh(v=np.ones((4, 3)), vc='red')
m.scale_vertex_colors(np.array([2.0, 1.0, 0.5, 0.0]))
expected_c = np.array([[1.0, 0.0, 0.0], [0.5, 0.0, 0.0],
[0.25, 0.0, 0.0], [0.0, 0.0, 0.0]])
np.testing.assert_array_equal(expected_c, m.vc)
self.assertEqual(m.vc.dtype, np.float64)
def test_explicit_landmarks_as_array_of_indexes(self):
m = Mesh(filename=self.template_fname, landmarks=np.array([0, 7]))
self.assertEqual(m.landm, {'0': 0, '1': 7})
self.assertDictOfArraysAlmostEqual(m.landm_xyz, {
'0': self.template.v[0],
'1': self.template.v[7]
})
def test_yml_points(self):
from baiji.serialization import yaml
path = self.make_point_data(yaml, '.yml')
m = Mesh(filename=self.template_fname, landmarks=path)
self.assertEqual(m.landm, self.template.landm)
self.assertDictOfArraysAlmostEqual(m.landm_xyz,
self.template.landm_xyz)
def test_json_index(self):
from baiji.serialization import json
path = self.make_index_data(json, '.json')
m = Mesh(filename=self.template_fname, landmarks=path)
self.assertEqual(m.landm, self.template.landm)
self.assertDictOfArraysAlmostEqual(
m.landm_xyz, self.template_without_regressors.landm_xyz)
def create_rectangular_prism(origin, size):
'''
Return a Mesh which is an axis-aligned rectangular prism. One vertex is
`origin`; the diametrically opposite vertex is `origin + size`.
size: 3x1 array.
'''
from lace.topology import quads_to_tris
lower_base_plane = np.array([
# Lower base plane
origin,
origin + np.array([size[0], 0, 0]),
origin + np.array([size[0], 0, size[2]]),
origin + np.array([0, 0, size[2]]),
])
upper_base_plane = lower_base_plane + np.array([0, size[1], 0])
vertices = np.vstack([lower_base_plane, upper_base_plane])
faces = quads_to_tris(np.array([
[0, 1, 2, 3], # lower base (-y)
[7, 6, 5, 4], # upper base (+y)
[4, 5, 1, 0], # +z face
[5, 6, 2, 1], # +x face
[6, 7, 3, 2], # -z face
[3, 7, 4, 0], # -x face
]))
return Mesh(v=vertices, f=faces)
def test_writing_empty_mesh(self):
m = Mesh()
local_file = os.path.join(self.tmp_dir, 'test_writing_empty_mesh.obj')
obj.dump(m, local_file)
self.assertEqual(os.stat(local_file).st_size, 0)
def test_writing_obj_locally_using_serializer(self):
local_file = os.path.join(
self.tmp_dir,
"test_writing_ascii_obj_locally_using_serializer.obj")
m = Mesh(filename=self.test_obj_path)
obj.dump(m, local_file)
self.assertFilesEqual(local_file, self.test_obj_path)
def test_writing_obj_locally_using_mesh_write_obj(self):
local_file = os.path.join(
self.tmp_dir,
"test_writing_ascii_obj_locally_using_mesh_write_ply.obj")
m = Mesh(filename=self.test_obj_path)
m.write_obj(local_file)
self.assertFilesEqual(local_file, self.test_obj_path)
def main():
import argparse
from lace.mesh import Mesh
parser = argparse.ArgumentParser()
parser.add_argument('-p', '--path', help='filepath to mesh', required=True)
parser.add_argument('-c', '--cloud', help='display point cloud', required=False, default=False, action='store_true')
parser.add_argument('-d', '--direction', help='direction of connected component',
choices=['N', 'S', 'E', 'W'], default=None, required=False)
args = parser.parse_args()
path_to_mesh = args.path
mesh = Mesh(filename=path_to_mesh, vc='SteelBlue')
point_on_plane = np.array([0., 1., 0.])
n1 = np.array([0., 1., 0.])
p1 = Plane(point_on_plane, n1)
n2 = np.array([1., 0., 0.])
p2 = Plane(point_on_plane, n2)
n3 = np.array([1., 1., 0.])
n3 /= np.linalg.norm(n3)
p3 = Plane(point_on_plane, n3)
n4 = np.array([-1., 1., 0.])
n4 /= np.linalg.norm(n4)
p4 = Plane(point_on_plane, n4)
dirmap = {
'N': [0., +100., 0.],
'S': [0., -100., 0.],
'E': [+100., 0., 0.],
'W': [-100., 0., 0.],
None: None,
}
neighborhood = dirmap[args.direction]
if neighborhood != None:
neighborhood = np.array([neighborhood])
xs1 = p1.mesh_xsection(mesh, neighborhood=neighborhood)
xs2 = p2.mesh_xsection(mesh, neighborhood=neighborhood)
xs3 = p3.mesh_xsection(mesh, neighborhood=neighborhood)
xs4 = p4.mesh_xsection(mesh, neighborhood=neighborhood)
lines = [
polyline.as_lines()
for polyline in [xs1, xs2, xs3, xs4]
]
if args.cloud:
mesh.f = []
from lace.meshviewer import MeshViewer
mv = MeshViewer(keepalive=True)
mv.set_dynamic_meshes([mesh], blocking=True)
mv.set_dynamic_lines(lines)
def test_explicit_landmarks_as_array_of_points(self):
m = Mesh(filename=self.template_fname,
landmarks=[self.template.v[0], self.template.v[7]])
self.assertEqual(m.landm, {'0': 0, '1': 7})
self.assertDictOfArraysAlmostEqual(m.landm_xyz, {
'0': self.template.v[0],
'1': self.template.v[7]
})
def test_recenter_over_floor_raises_expected_on_empty_mesh(self):
mesh = Mesh()
with self.assertRaises(ValueError) as ctx:
mesh.recenter_over_floor()
self.assertEqual(str(ctx.exception),
'Mesh has no vertices; centroid is not defined')
def test_bounding_box_is_undefined_on_empty_mesh(self):
mesh = Mesh()
with self.assertRaises(ValueError) as ctx:
mesh.bounding_box # pylint: disable=pointless-statement
self.assertEqual(str(ctx.exception),
'Mesh has no vertices; bounding box is not defined')
def load_front_torso_mesh():
from lace.mesh import Mesh
mesh = Mesh(filename="/Users/pnm/code/tape/examples/anonymized_female.obj")
mesh.cut_across_axis(1, minval=100.0, maxval=120.0)
mesh.cut_across_axis(0, minval=-19.0, maxval=19.0)
mesh.cut_across_axis(2, minval=0)
return mesh
def test_loads_from_local_path_using_constructor_with_landmarks(self):
skip_on_import_error('lace-search')
m = Mesh(filename=self.test_obj_with_landmarks_path,
ppfilename=self.test_pp_path)
self.assertTrue((m.v == self.truth['box_v']).all())
self.assertTrue((m.f == self.truth['box_f']).all())
self.assertEqual(m.landm, self.truth['landm'])
self.assertDictOfArraysEqual(m.landm_xyz, self.truth['landm_xyz'])
self.assertDictOfArraysEqual(m.segm, self.truth['box_segm'])
def _load(f, existing_mesh=None):
import numpy as np
from lace.mesh import Mesh
v = np.loadtxt(f.name)
if existing_mesh is None:
return Mesh(v=v)
else:
existing_mesh.v = v
return existing_mesh
def _load(f, existing_mesh=None):
from lace.mesh import Mesh
parser = BSFParser(f)
if existing_mesh is None:
return Mesh(v=parser.v, f=parser.f, vc=parser.vc)
else:
existing_mesh.v = parser.v
existing_mesh.f = parser.f
existing_mesh.vc = parser.vc
return existing_mesh
def create_horizontal_plane():
'''
Creates a horizontal plane.
'''
v = np.array([
[1., 0., 0.],
[-1., 0., 0.],
[0., 0., 1.],
[0., 0., -1.]
])
f = [[0, 1, 2], [3, 1, 0]]
return Mesh(v=v, f=f)
def test_estimate_vertex_normals(self):
# normals of a sphere should be scaled versions of the vertices
test_sphere_path = sc(
's3://bodylabs-korper-assets/is/ps/shared/data/body/'
'korper_testdata/sphere.ply'
)
m = Mesh(filename=test_sphere_path)
m.v -= np.mean(m.v, axis=0)
rad = np.linalg.norm(m.v[0])
m.estimate_vertex_normals()
mse = np.mean(np.sqrt(np.sum((m.vn - m.v/rad)**2, axis=1)))
self.assertTrue(mse < 0.05)
def _load(f, mesh=None):
import numpy as np
from lace.mesh import Mesh
if not mesh:
mesh = Mesh()
faces = []
facenormals = []
verts = []
head = f.readline().strip()
if head.startswith("solid"): # ascii STL format
#name = head[6:]
current_face = []
for line in f:
line = line.split()
if line[0] == "endsolid":
break
elif line[0] == "facet":
current_face = []
if line[1] == "normal":
try:
facenormals.append([float(x) for x in line[2:]])
except: # pylint: disable=bare-except
facenormals.append([np.nan, np.nan, np.nan])
else:
facenormals.append([np.nan, np.nan, np.nan])
elif line[0] == "endfacet":
faces.append(current_face)
current_face = []
elif line[0:2] == ["outer", "loop"]:
pass
elif line[0] == "endloop":
pass
elif line[0] == "vertex":
current_face.append(len(verts))
try:
verts.append([float(x) for x in line[1:]])
except: # pylint: disable=bare-except
verts.append([np.nan, np.nan, np.nan])
else:
raise ValueError(
"Badly formatted STL file. I don't understand the line %s"
% line)
else:
raise Exception(
"Looks like this is a binary STL file; you're going to have to implement that"
)
# format docs are here: http://en.wikipedia.org/wiki/STL_(file_format)
mesh.v = np.array(verts, dtype=np.float64).copy()
mesh.f = np.array(faces, dtype=np.uint32).copy()
mesh.fn = np.array(facenormals, dtype=np.float64).copy()
return mesh
def setUp(self):
skip_on_import_error('lace-search')
self.scan_fname = sc(
's3://bodylabs-korper-assets/is/ps/shared/data/body/caesar/RawScans/csr0001a.ply'
)
self.scan_lmrk = sc(
's3://bodylabs-korper-assets/is/ps/shared/data/body/caesar/Landmarks/csr0001a.lmrk'
)
self.template_fname = sc(
's3://bodylabs-korper-assets/is/ps/shared/data/body/template/textured_mean_scape_female.obj'
)
self.template_pp = sc(
's3://bodylabs-korper-assets/is/ps/shared/data/body/template/template_caesar_picked_points.pp'
)
self.scan = Mesh(filename=self.scan_fname, lmrkfilename=self.scan_lmrk)
self.template = Mesh(filename=self.template_fname,
ppfilename=self.template_pp)
self.template_without_regressors = Mesh(filename=self.template_fname,
ppfilename=self.template_pp)
self.template_without_regressors.landm_regressors = {}
super(TestLandmarks, self).setUp()
def test_concatenate_partial_missing_vc(self):
v1 = np.array([
[0., 0., 0.],
[5., 5., 5.],
[5., 0., 5.],
])
vc1 = np.array([
[1., 0., 0.],
[0., 1., 0.],
[0., 0., 1.],
])
m1 = Mesh(v=v1, vc=vc1)
v2 = np.array([
[0., 0., 0.],
[5., -5., 5.],
[5., 0., 5.],
])
m2 = Mesh(v=v2)
with self.assertRaisesRegexp(ValueError, 'all args or none.'):
Mesh.concatenate(m1, m2)
def test_explicit_landmark_indexes_from_another_mesh(self):
m = Mesh(filename=self.template_fname, landmarks=self.template.landm)
np.testing.assert_array_equal(self.template.v, m.v)
self.assertEqual(m.landm, self.template.landm)
# The following are _not_ expected to pass, as the initial load of template is
# loading points that are slightly off from the actual vertex locations,
# so the regressors are going to be subtly different.
# self.assertDictOfArraysAlmostEqual(m.landm_xyz, self.template.landm_xyz)
# self.assertDictOfArraysAlmostEqual(m.landm_regressors, self.template.landm_regressors)
# But if we disable the regressors and just use the landmark verts:
m.landm_regressors = {}
self.assertDictOfArraysAlmostEqual(
m.landm_xyz, self.template_without_regressors.landm_xyz)