def sphere_into_ellipsoid(img_path):
"""See if we can turn a sphere into an ellipse by changing the radius of
vertices
The point cloud (ellipse) should have more points than the initial mesh.
When the intial mesh is coarse the resulting mesh will also be heavily faceted, but this will avoid the big holes, and large changes in depth
"""
# define the sphere
vs, fs = wavefront.ellipsoid_mesh(1, 1, 1, density=20, subdivisions=1)
ve, fe = wavefront.ellipsoid_mesh(2, 3, 4, density=20, subdivisions=1)
mfig = graphics.mayavi_figure(offscreen=True)
mesh = graphics.mayavi_addMesh(mfig, vs, fs)
ms = mesh.mlab_source
index = 0
# in a loop add each vertex of the ellipse into the sphere mesh
for jj in range(2):
for ii, pt in enumerate(ve):
index += 1
filename = os.path.join(
img_path, 'sphere_ellipsoid_' + str(index).zfill(6) + '.jpg')
graphics.mlab.savefig(filename, magnification=4)
mesh_param = wavefront.polyhedron_parameters(vs, fs)
vs, fs = wavefront.radius_mesh_incremental_update(
pt, vs, fs, mesh_param, max_angle=np.deg2rad(10))
ms.reset(x=vs[:, 0], y=vs[:, 1], z=vs[:, 2], triangles=fs)
graphics.mayavi_addPoint(mfig, pt)
vs, fs = wavefront.mesh_subdivide(vs, fs, 1)
ms.reset(x=vs[:, 0], y=vs[:, 1], z=vs[:, 2], triangles=fs)
return 0
def cube_into_sphere(img_path):
"""Transform a cube into a sphere
"""
vc, fc = wavefront.read_obj('./integration/cube.obj')
vs, fs = wavefront.ellipsoid_mesh(2, 2, 2, density=10, subdivisions=0)
mfig = graphics.mayavi_figure(offscreen=True)
mesh = graphics.mayavi_addMesh(mfig, vc, fc)
ms = mesh.mlab_source
index = 0
for ii in range(5):
for jj, pt in enumerate(vs):
index += 1
filename = os.path.join(
img_path, 'cube_sphere_' + str(index).zfill(6) + '.jpg')
graphics.mlab.savefig(filename, magnification=4)
mesh_param = wavefront.polyhedron_parameters(vc, fc)
vc, fc = wavefront.radius_mesh_incremental_update(
pt, vc, fc, mesh_param, max_angle=np.deg2rad(5))
ms.reset(x=vc[:, 0], y=vc[:, 1], z=vc[:, 2], triangles=fc)
graphics.mayavi_addPoint(mfig, pt)
vc, fc = wavefront.mesh_subdivide(vc, fc, 1)
ms.reset(x=vc[:, 0], y=vc[:, 1], z=vc[:, 2], triangles=fc)
return 0
def castalia_raycasting_plot(img_path):
"""Generate an image of the raycaster in work
"""
if not os.path.exists(img_path):
os.makedirs(img_path)
output_filename = os.path.join(img_path, 'castalia_raycasting_plot.jpg')
input_filename = './data/shape_model/CASTALIA/castalia.obj'
polydata = wavefront.read_obj_to_polydata(input_filename)
# initialize the raycaster
caster_obb = raycaster.RayCaster(polydata, flag='obb')
caster_bsp = raycaster.RayCaster(polydata, flag='bsp')
sensor = raycaster.Lidar(view_axis=np.array([1, 0, 0]), num_step=3)
# need to translate the sensor and give it a pointing direction
pos = np.array([2, 0, 0])
dist = 2 # distance for each raycast
R = attitude.rot3(np.pi)
# find the inersections
# targets = pos + sensor.rotate_fov(R) * dist
targets = sensor.define_targets(pos, R, dist)
intersections_obb = caster_obb.castarray(pos, targets)
intersections_bsp = caster_bsp.castarray(pos, targets)
# plot
fig = graphics.mayavi_figure()
graphics.mayavi_addPoly(fig, polydata)
graphics.mayavi_addPoint(fig, pos, radius=0.05)
# draw lines from pos to all targets
for pt in targets:
graphics.mayavi_addLine(fig, pos, pt, color=(1, 0, 0))
for ints in intersections_bsp:
graphics.mayavi_addPoint(fig, ints, radius=0.05, color=(1, 1, 0))
graphics.mayavi_axes(fig=fig, extent=[-1, 1, -1, 1, -1, 1])
graphics.mayavi_view(fig=fig)
# savefig
graphics.mayavi_savefig(fig=fig, filename=output_filename, magnification=4)
def castalia_reconstruction(img_path):
"""Incrementally modify an ellipse into a low resolution verision of castalia
by adding vertices and modifying the mesh
"""
surf_area = 0.01
a = 0.22
delta = 0.01
# load a low resolution ellipse to start
ast = asteroid.Asteroid('castalia', 0, 'obj')
ellipsoid = surface_mesh.SurfMesh(ast.axes[0], ast.axes[1], ast.axes[2],
10, 0.025, 0.5)
ve, fe = ellipsoid.verts(), ellipsoid.faces()
vc, fc = ast.V, ast.F
# sort the vertices in in order (x component)
vc = vc[vc[:, 0].argsort()]
# uncertainty for each vertex in meters (1/variance)
vert_weight = np.full(ve.shape[0], (np.pi * np.max(ast.axes))**2)
# calculate maximum angle as function of surface area
max_angle = wavefront.spherical_surface_area(np.max(ast.axes), surf_area)
# loop and create many figures
mfig = graphics.mayavi_figure(offscreen=False)
mesh = graphics.mayavi_addMesh(mfig, ve, fe)
ms = mesh.mlab_source
index = 0
for ii, pt in enumerate(vc):
index += 1
filename = os.path.join(
img_path, 'castalia_reconstruct_' + str(index).zfill(7) + '.jpg')
# graphics.mlab.savefig(filename, magnification=4)
ve, vert_weight = wavefront.spherical_incremental_mesh_update(
pt, ve, fe, vertex_weight=vert_weight, max_angle=max_angle)
ms.reset(x=ve[:, 0], y=ve[:, 1], z=ve[:, 2], triangles=fe)
graphics.mayavi_addPoint(mfig, pt, radius=0.01)
graphics.mayavi_points3d(mfig, ve, scale_factor=0.01, color=(1, 0, 0))
return 0
def test_normal_face_plot():
"""Plot the normals to the faces on a mesh and view in Mayavi
"""
v, f = wavefront.read_obj('./integration/cube.obj')
normal_face = wavefront.normal_face(v, f)
cof = wavefront.center_of_face(v, f)
mfig = graphics.mayavi_figure()
_ = graphics.mayavi_addMesh(mfig, v, f)
for p1, u in zip(cof, normal_face):
graphics.mayavi_addPoint(mfig, p1, radius=0.1, color=(1, 0, 0))
graphics.mayavi_addLine(mfig, p1, u + p1)
graphics.mayavi_addTitle(mfig,
'Normals to each face',
color=(0, 0, 0),
size=0.5)
def test_radius_mesh_update_cube(pt=np.array([1, 0, 0])):
"""Update the mesh by modifying the radius of the closest point
"""
# load the cube
v, f = wavefront.read_obj('./integration/cube.obj')
mesh_parameters = wavefront.polyhedron_parameters(v, f)
# pick a point
nv, nf = wavefront.radius_mesh_incremental_update(pt,
v,
f,
mesh_parameters,
max_angle=np.deg2rad(10))
# plot the new mesh
mfig = graphics.mayavi_figure()
graphics.mayavi_addMesh(mfig, nv, nf)
graphics.mayavi_addPoint(mfig, pt)
graphics.mayavi_points3d(mfig, v, color=(0, 1, 0))
return nv, nf
def test_radius_cube_into_sphere():
"""Transform a cube into a sphere
"""
vc, fc = wavefront.read_obj('./integration/cube.obj')
vs, fs = wavefront.ellipsoid_mesh(2, 2, 2, density=10, subdivisions=0)
mfig = graphics.mayavi_figure()
mesh = graphics.mayavi_addMesh(mfig, vc, fc)
graphics.mayavi_points3d(mfig, vc, color=(0, 1, 0))
ms = mesh.mlab_source
for ii in range(5):
for pt in vs:
mesh_param = wavefront.polyhedron_parameters(vc, fc)
vc, fc = wavefront.radius_mesh_incremental_update(
pt, vc, fc, mesh_param, max_angle=np.deg2rad(45))
ms.reset(x=vc[:, 0], y=vc[:, 1], z=vc[:, 2], triangles=fc)
graphics.mayavi_addPoint(mfig, pt)
input('Mesh subdivison')
vc, fc = wavefront.mesh_subdivide(vc, fc, 1)
ms.reset(x=vc[:, 0], y=vc[:, 1], z=vc[:, 2], triangles=fc)
def plot_data(pt, v, f, D, P, V, E, F, string='Closest Primitive', radius=0.1):
# draw the mayavi figure
mfig = graphics.mayavi_figure()
graphics.mayavi_addMesh(mfig, v, f)
graphics.mayavi_addPoint(mfig, pt, radius=radius, color=(0, 1, 0))
if P.any():
graphics.mayavi_points3d(mfig, P, scale_factor=radius, color=(1, 0, 0))
# different color for each face
if F.size:
try:
_ = iter(F[0])
for f_list in F:
for f_ind in f_list:
face_verts = v[f[f_ind, :], :]
graphics.mayavi_addMesh(mfig,
face_verts, [(0, 1, 2)],
color=tuple(np.random.rand(3)))
except IndexError as err:
face_verts = v[f[F, :], :]
graphics.mayavi_addMesh(mfig,
face_verts, [(0, 1, 2)],
color=tuple(np.random.rand(3)))
except (TypeError, ) as err:
for f_ind in F:
face_verts = v[f[f_ind, :], :]
graphics.mayavi_addMesh(mfig,
face_verts, [(0, 1, 2)],
color=tuple(np.random.rand(3)))
# draw the points which make up the edges and draw a line for the edge
if V.size:
try:
_ = iter(V)
for v_ind in V:
graphics.mayavi_addPoint(mfig,
v[v_ind, :],
radius=radius,
color=(0, 0, 1))
except TypeError as err:
graphics.mayavi_addPoint(mfig,
v[V, :],
radius=radius,
color=(0, 0, 1))
# draw edges
if E.size:
try:
_ = iter(E[0][0])
for e_list in E:
for e_ind in e_list:
graphics.mayavi_addLine(mfig,
v[e_ind[0], :],
v[e_ind[1], :],
color=(0, 0, 0))
except IndexError as err:
graphics.mayavi_addLine(mfig,
v[E[0], :],
v[E[1], :],
color=(0, 0, 0))
except (TypeError, ) as err:
for e_ind in E:
graphics.mayavi_addLine(mfig,
v[e_ind[0], :],
v[e_ind[1], :],
color=(0, 0, 0))
graphics.mayavi_addTitle(mfig, string, color=(0, 0, 0), size=0.5)
# need to translate the sensor and give it a pointing direction
pos = np.array([2, 0, 0])
dist = 2 # distance for each raycast
R = attitude.rot3(np.pi)
# find the inersections
# targets = pos + sensor.rotate_fov(R) * dist
targets = sensor.define_targets(pos, R, dist)
intersections_obb = caster_obb.castarray(pos, targets)
intersections_bsp = caster_bsp.castarray(pos, targets)
# plot
fig = graphics.mayavi_figure(bg=(0, 0, 0))
graphics.mayavi_addPoly(fig, polydata)
graphics.mayavi_addPoint(fig, pos, radius=0.05)
# draw lines from pos to all targets
for pt in targets:
graphics.mayavi_addLine(fig, pos, pt, color=(1, 0, 0))
# draw a point at all intersections
# for ints in intersections_obb:
# graphics.mayavi_addPoint(fig, ints, radius=0.01, color=(0, 1, 0))
for ints in intersections_bsp:
graphics.mayavi_addPoint(fig, ints, radius=0.02, color=(1, 1, 0))
graphics.mlab.orientation_axes(figure=fig)
# graphics.mlab.text3d(0, 0, 0.1, 'Green: OBBTree', figure=fig, scale=0.1)
# graphics.mlab.text3d(0, 0, -0.1, 'Yellow: BSPTree', figure=fig, scale=0.1)
def sphere_into_ellipsoid_spherical_coordinates(img_path):
"""See if we can turn a sphere into an ellipse by changing the radius of
vertices in spherical coordinates
The point cloud (ellipse) should have the same number of points than the initial mesh.
"""
surf_area = 0.06
a = 0.25 # at a*100 % of maximum angle the scale will be 50% of measurement
delta = 0.01
# define the sphere
# vs, fs = wavefront.ellipsoid_mesh(0.5, 0.5, 0.5, density=10, subdivisions=1)
# ve, fe = wavefront.ellipsoid_mesh(1,2, 3, density=10, subdivisions=1)
# import the sphere and ellipsoid from matlab files
sphere_data = scipy.io.loadmat('./data/sphere_distmesh.mat')
ellipsoid_data = scipy.io.loadmat('./data/ellipsoid_distmesh.mat')
vs, fs = sphere_data['v'], sphere_data['f']
ve, fe = ellipsoid_data['v'], ellipsoid_data['f']
# sphere = surface_mesh.SurfMesh(0.5, 0.5, 0.5, 10, 0.05, 0.5)
# ellipsoid = surface_mesh.SurfMesh(1, 2, 3, 10, 0.2, 0.5)
# vs, fs = sphere.verts(), sphere.faces()
# ve, fe = ellipsoid.verts(), sphere.faces()
print("Sphere V: {} F: {}".format(vs.shape[0], fs.shape[0]))
print("Ellipsoid V: {} F: {}".format(ve.shape[0], fe.shape[0]))
# convert to spherical coordinates
vs_spherical = wavefront.cartesian2spherical(vs)
ve_spherical = wavefront.cartesian2spherical(ve)
mfig = graphics.mayavi_figure(offscreen=False)
mesh = graphics.mayavi_addMesh(mfig, vs, fs)
ms = mesh.mlab_source
index = 0
# graphics.mayavi_points3d(mfig, vs, color=(1, 0, 0))
# graphics.mayavi_points3d(mfig, ve, color=(0, 1, 0))
# in a loop add each vertex of the ellipse into the sphere mesh
for ii, pt in enumerate(ve_spherical):
index += 1
filename = os.path.join(
img_path, 'sphere_ellipsoid_' + str(index).zfill(6) + '.jpg')
# graphics.mlab.savefig(filename, magnification=4)
vs_spherical, fs = wavefront.spherical_incremental_mesh_update(
mfig, pt, vs_spherical, fs, surf_area=surf_area, a=a, delta=delta)
# convert back to cartesian for plotting
vs_cartesian = wavefront.spherical2cartesian(vs_spherical)
ms.reset(x=vs_cartesian[:, 0],
y=vs_cartesian[:, 1],
z=vs_cartesian[:, 2],
triangles=fs)
graphics.mayavi_addPoint(mfig,
wavefront.spherical2cartesian(pt),
radius=0.02)
graphics.mayavi_points3d(mfig,
vs_cartesian,
scale_factor=0.02,
color=(1, 0, 0))
return 0
vert_weight = np.full(vs.shape[0],
(np.pi * np.max(np.linalg.norm(vs, axis=1)))**2)
vs_new, vw_new = wavefront.spherical_incremental_mesh_update(
pt, vs, fs, vertex_weight=vert_weight, max_angle=max_angle)
# view the mesh
graphics.mayavi_addMesh(mfig,
vs_new,
fs,
representation='surface',
scalars=vw_new,
colormap='viridis',
color=None)
graphics.mayavi_addPoint(mfig, pt)
# duplicate the same thing with c++
mfig_cpp = graphics.mayavi_figure()
vw = np.full(vs.shape[0], (np.pi * np.max(np.linalg.norm(vs, axis=1)))**2)
# mesh = mesh_data.MeshData(vs, fs)
rmesh = reconstruct.ReconstructMesh(vs, fs, vw)
rmesh.update(pt, max_angle)
graphics.mayavi_addMesh(mfig_cpp,
rmesh.get_verts(),
rmesh.get_faces(),
representation='surface',
scalars=np.squeeze(rmesh.get_weights()),
"""Test out creating and plotting a geodesic waypoint (great circle) on the
sphere
"""
from point_cloud import wavefront
from kinematics import sphere
from visualization import graphics
from lib import geodesic, surface_mesh
import numpy as np
# generate a sphere for plotting
sphere_mesh = surface_mesh.SurfMesh(1, 1, 1, 10, 0.15, 0.5)
vs, fs = sphere_mesh.verts(), sphere_mesh.faces()
# create two random points on the sphere
initial_point_cartesian = sphere.rand(2)
final_point_cartesian = sphere.rand(2)
# compute waypoints inbetween
waypoints_cartesian = geodesic.sphere_waypoint(initial_point_cartesian,
final_point_cartesian, 5)
# plot everythign on a mayavi figure
mfig = graphics.mayavi_figure()
graphics.mayavi_axes(mfig, [-1, 1, -1, 1, -1, 1])
graphics.mayavi_addMesh(mfig, vs, fs, color=(0, 0, 1), opacity=0.2)
graphics.mayavi_points3d(mfig, waypoints_cartesian, color=(0, 0, 1))
graphics.mayavi_addPoint(mfig, initial_point_cartesian, color=(0, 1, 0))
graphics.mayavi_addPoint(mfig, final_point_cartesian, color=(1, 0, 0))