class TestInertialTransform():
inertial_pos = np.array([1, 1, 1])
inertial_vel = np.random.rand(3)
R_sc2int = att.rot2(np.pi / 2)
body_ang_vel = np.random.rand(3)
time = np.array([0])
ast = asteroid.Asteroid(name='castalia', num_faces=64)
dum = dumbbell.Dumbbell()
input_state = np.hstack(
(inertial_pos, inertial_vel, R_sc2int.reshape(9), body_ang_vel))
inertial_state = transform.eoms_inertial_to_inertial(
time, input_state, ast, dum)
def test_eoms_inertial_to_inertial_scalar_pos(self):
np.testing.assert_almost_equal(self.inertial_pos,
self.inertial_state[0:3])
def test_eoms_inertial_to_inertial_scalar_vel(self):
np.testing.assert_almost_equal(self.inertial_vel,
self.inertial_state[3:6])
def test_eoms_inertial_to_inertial_scalar_att(self):
np.testing.assert_almost_equal(self.R_sc2int.reshape(9),
self.inertial_state[6:15])
def test_eoms_inertial_to_inertial_scalar_ang_vel(self):
np.testing.assert_almost_equal(self.R_sc2int.dot(self.body_ang_vel),
self.inertial_state[15:18])
def load_data(inertial_filename, relative_filename, mode):
"""Load saved data and extract out the states
"""
with np.load(inertial_filename, allow_pickle=True) as data:
inertial_state = data['state']
inertial_time = data['time']
inertial_KE = data['KE']
inertial_PE = data['PE']
ast_name = data['ast_name'][()]
num_faces = data['num_faces'][()]
tf = data['tf'][()]
num_steps = data['num_steps'][()]
with np.load(relative_filename, allow_pickle=True) as data:
relative_state = data['state']
relative_time = data['time']
relative_KE = data['KE']
relative_PE = data['PE']
relative_ast_name = data['ast_name'][()]
relative_num_faces = data['num_faces'][()]
relative_tf = data['tf'][()]
relative_num_steps = data['num_steps'][()]
# make sure we're dealing with the same simulation results or else the comparison is meaningless
np.testing.assert_string_equal(relative_ast_name, ast_name)
np.testing.assert_allclose(relative_num_faces, num_faces)
np.testing.assert_allclose(relative_tf, tf)
np.testing.assert_allclose(relative_num_steps, num_steps)
np.testing.assert_allclose(relative_state.shape, inertial_state.shape)
ast = asteroid.Asteroid(ast_name, num_faces)
dum = dumbbell.Dumbbell()
return relative_time, inertial_time, relative_state, inertial_state, ast, dum
class TestDumbbellRelativeODE():
dum = dumbbell.Dumbbell()
angle = (2 * np.pi - 0) * np.random.rand(1) + 0
# generate a random initial state that is outside of the asteroid
pos = np.random.rand(3) + np.array([2, 2, 2])
vel = np.random.rand(3)
R = attitude.rot1(angle).reshape(9)
ang_vel = np.random.rand(3)
t = np.random.rand() * 100
state = np.hstack((pos, vel, R, ang_vel))
statedot = dum.eoms_relative_ode(t, state, ast)
def test_relative_moment_of_inertia(self):
R = self.R.reshape((3, 3))
Jr = R.dot(self.dum.J).dot(R.T)
Jdr = R.dot(self.dum.Jd).dot(R.T)
np.testing.assert_array_almost_equal(
attitude.hat_map(Jr.dot(self.ang_vel)),
attitude.hat_map(self.ang_vel).dot(Jdr) +
Jdr.dot(attitude.hat_map(self.ang_vel)))
def test_eoms_relative_size(self):
np.testing.assert_allclose(self.statedot.shape, (18, ))
def test_eoms_relative_R_dot(self):
np.testing.assert_allclose(self.statedot[6:15].shape, (9, ))
def inertial_frame_comparison(ast_name='castalia',
num_faces=64,
tf=1e5,
num_steps=1e6):
"""Compare EOMs in the inertial frame
"""
initial_w = np.array([0.01, 0.0, 0.0])
print("Running inertial EOMS")
i_time, i_state = idriver.inertial_eoms_driver(ast_name, num_faces, tf,
num_steps, initial_w)
print("Running asteroid EOMs")
r_time, r_state = rdriver.relative_eoms_driver(ast_name, num_faces, tf,
num_steps, initial_w)
ast = asteroid.Asteroid(ast_name, num_faces)
dum = dumbbell.Dumbbell()
# also compute and compare the energy behavior
print("Computing inertial energy")
i_KE, i_PE = dum.inertial_energy(i_time, i_state, ast)
print("Computing asteroid energy")
r_KE, r_PE = dum.relative_energy(r_time, r_state, ast)
plotting.plot_energy(i_time, i_KE, i_PE)
# also look at the animation of both and the converted form as well
print("Plot comparison in the inertial frame")
plotting.plot_inertial_comparison(r_time, i_time, r_state, i_state, ast,
dum)
return 0
class TestDumbbellInertialTranslationalController():
dum = dumbbell.Dumbbell()
u_f = dum.translation_controller(t, state, np.zeros(3))
def test_control_force_size(self):
np.testing.assert_equal(self.u_f.shape, (3, ))
class TestDumbbellInertialAttitudeController():
"""Test the attitude controller for the inertial eoms
"""
dum = dumbbell.Dumbbell()
u_m = dum.attitude_controller(t, state, np.zeros(3))
def test_control_moment_size(self):
np.testing.assert_equal(self.u_m.shape, (3, ))
def initialize():
"""Initialize all the things for the simulation
"""
logger = logging.getLogger(__name__)
logger.info('Initialize asteroid and dumbbell objects')
ast = asteroid.Asteroid('castalia', 4092, 'obj')
dum = dumbbell.Dumbbell(m1=500, m2=500, l=0.003)
des_att_func = controller.random_sweep_attitude
des_tran_func = controller.inertial_fixed_state
AbsTol = 1e-9
RelTol = 1e-9
return ast, dum, des_att_func, des_tran_func, AbsTol, RelTol
def create_plots(plot_flags):
# load the h5py file with all the imagery and simulation data
with h5py.File('./data/itokawa_landing/cycles_high_7200.hdf5', 'r') as sim_data:
sim_data.visit(printname)
K = sim_data['K']
i_state = sim_data['i_state']
time = sim_data['time']
images = sim_data['landing']
RT_vector = sim_data['RT']
R_bcam2i_vector = sim_data['R_i2bcam'] # the name is incorrect - actually it's bcamera to inertial frame
R_ast2int = sim_data['Rast2inertial']
# define the asteroid and dumbbell objects like the simulation driver
ast_name = 'itokawa'
num_faces = 64
ast = asteroid.Asteroid(ast_name,num_faces)
dum = dumbbell.Dumbbell(m1=500, m2=500, l=0.003)
# draw some of the features from an example image
if plot_flags.feature_matching:
sift_flann_matching_image(images[:, :, :, 3000],
images[:, :, :, 3200], ratio=0.3,
plot=True,
filename='/tmp/itokawa_feature_matching.png',
save_fig=plot_flags.save_plots)
# draw the true and estimated trajectory
if plot_flags.simulation_plots:
plotting.plot_controlled_blender_inertial(time,
i_state,
ast,
dum,
plot_flags.save_plots,
1,
controller.traverse_then_land_vertically,
controller.body_fixed_pointing_attitude)
# create animation
if plot_flags.animation:
plotting.animate_inertial_trajectory(time, i_state, ast, dum, 3600, plot_flags.save_plots)
if plot_flags.keyframe:
# plot_keyframe_trajectory(time, i_state, R_ast2int, R_bcam2i_vector,
# plot_flags.save_plots, fwidth=1,
# filename='/tmp/keyframe_estimate.eps')
plot_keyframe_original(time, i_state, R_ast2int, R_bcam2i_vector,
plot_flags.save_plots, fwidth=1,
filename='/tmp/keyframe_estimate.eps')
class TestDumbbellInertialDesiredAttitude():
dum = dumbbell.Dumbbell()
alpha = np.random.rand()
axis = np.array([1, 0, 0])
Rd, Rd_dot, ang_vel_d, ang_vel_d_dot = dum.desired_attitude(1, alpha, axis)
def test_desired_rotation_matrix_determinant(self):
np.testing.assert_almost_equal(np.linalg.det(self.Rd), 1)
def test_desired_rotation_matrix_orthogonal(self):
np.testing.assert_array_almost_equal(self.Rd.T.dot(self.Rd),
np.eye(3, 3))
def test_desired_attitude_satifies_kinematics(self):
np.testing.assert_array_almost_equal(
self.Rd_dot, self.Rd.dot(attitude.hat_map(self.ang_vel_d)))
class TestDumbbellInertial():
dum = dumbbell.Dumbbell()
statedot = dum.eoms_inertial(state, t, ast)
def test_inertia_symmetric(self):
np.testing.assert_array_almost_equal(self.dum.J.T, self.dum.J)
def test_eoms_inertial_size(self):
np.testing.assert_allclose(self.statedot.shape, (18, ))
def test_eoms_inertial_pos_dot(self):
np.testing.assert_allclose(self.statedot[0:3], vel)
def test_eoms_inertial_R_dot(self):
np.testing.assert_allclose(
self.statedot[6:15],
R.reshape(3, 3).dot(attitude.hat_map(ang_vel)).reshape(9))
def test_moment_of_inertia(self):
np.testing.assert_allclose(
self.dum.J,
np.trace(self.dum.Jd) * np.eye(3, 3) - self.dum.Jd)
def asteroid_frame_comparison():
"""Compare EOMs in the asteroid frame
"""
ast_name = 'castalia'
num_faces = 64
tf = 1e4
num_steps = 1e5
initial_w = np.array([0.0, 0.01, 0.0])
i_time, i_state = id.inertial_eoms_driver(ast_name, num_faces, tf,
num_steps, initial_w)
r_time, r_state = rd.relative_eoms_driver(ast_name, num_faces, tf,
num_steps, initial_w)
ast = asteroid.Asteroid(ast_name, num_faces)
dum = dumbbell.Dumbbell()
# also compute and compare the energy behavior
i_KE, i_PE = dum.inertial_energy(i_time, i_state, ast)
r_KE, r_PE = dum.relative_energy(r_time, r_state, ast)
class TestHamiltonRelativeTransform():
time = np.array([0])
ast = asteroid.Asteroid(name='castalia', num_faces=64)
dum = dumbbell.Dumbbell()
inertial_pos = np.array([1, 1, 1])
inertial_vel = np.random.rand(3) + att.hat_map(
ast.omega * np.array([0, 0, 1])).dot(inertial_pos)
R_sc2int = att.rot2(np.pi / 2)
R_ast2int = att.rot3(time * ast.omega)
body_ang_vel = np.random.rand(3)
initial_lin_mom = (dum.m1 + dum.m2) * (inertial_vel)
initial_ang_mom = R_sc2int.dot(dum.J).dot(body_ang_vel)
initial_ham_state = np.hstack(
(inertial_pos, initial_lin_mom, R_ast2int.dot(R_sc2int).reshape(9),
initial_ang_mom))
inertial_state = transform.eoms_hamilton_relative_to_inertial(
time, initial_ham_state, ast, dum)
def test_eoms_hamilton_relative_to_inertial_scalar_pos(self):
np.testing.assert_almost_equal(self.inertial_pos,
self.inertial_state[0:3])
def test_eoms_hamilton_relative_to_inertial_scalar_vel(self):
np.testing.assert_almost_equal(self.inertial_vel,
self.inertial_state[3:6])
def test_eoms_hamilton_relative_to_inertial_scalar_att(self):
np.testing.assert_almost_equal(self.R_sc2int.reshape(9),
self.inertial_state[6:15])
def test_eoms_hamilton_relative_to_inertial_scalar_ang_vel(self):
np.testing.assert_almost_equal(self.R_sc2int.dot(self.body_ang_vel),
self.inertial_state[15:18])
def blender_asteroid_frame_sim(gen_images=False):
# simulation parameters
output_path = './visualization/blender'
asteroid_name = 'itokawa_low'
# create a HDF5 dataset
hdf5_path = './data/asteroid_circumnavigate/{}_asteroid_circumnavigate.hdf5'.format(
datetime.datetime.now().strftime("%Y-%m-%dT%H:%M:%S"))
dataset_name = 'landing'
render = 'BLENDER'
image_modulus = 1
RelTol = 1e-6
AbsTol = 1e-6
ast_name = 'itokawa'
num_faces = 64
t0 = 0
dt = 1
tf = 3600 * 1
num_steps = 3600 * 1
ast = asteroid.Asteroid(ast_name,num_faces)
dum = dumbbell.Dumbbell(m1=500, m2=500, l=0.003)
# instantiate the blender scene once
camera_obj, camera, lamp_obj, lamp, itokawa_obj, scene = blender.blender_init(render_engine=render, asteroid_name=asteroid_name)
# get some of the camera parameters
K = blender_camera.get_calibration_matrix_K_from_blender(camera)
periodic_pos = np.array([1.495746722510590,0.000001002669660,0.006129720493607])
periodic_vel = np.array([0.000000302161724,-0.000899607989820,-0.000000013286327])
# set initial state for inertial EOMs
# initial_pos = np.array([2.550, 0, 0]) # km for center of mass in body frame
initial_pos = np.array([3, 0, 0])
initial_vel = periodic_vel + attitude.hat_map(ast.omega*np.array([0,0,1])).dot(initial_pos)
initial_R = attitude.rot3(np.pi/2).reshape(9) # transforms from dumbbell body frame to the inertial frame
initial_w = np.array([0.01, 0.01, 0.01])
initial_state = np.hstack((initial_pos, initial_vel, initial_R, initial_w))
# instantiate ode object
# system = integrate.ode(eoms_controlled_blender)
system = integrate.ode(eoms.eoms_controlled_relative_blender_ode)
system.set_integrator('lsoda', atol=AbsTol, rtol=RelTol, nsteps=1000)
system.set_initial_value(initial_state, t0)
system.set_f_params(dum, ast)
i_state = np.zeros((num_steps+1, 18))
time = np.zeros(num_steps+1)
i_state[0, :] = initial_state
with h5py.File(hdf5_path) as image_data:
# create a dataset
if gen_images:
images = image_data.create_dataset(dataset_name, (244, 537, 3,
num_steps/image_modulus),
dtype='uint8')
RT_blender = image_data.create_dataset('RT',
(num_steps/image_modulus,
12))
R_i2bcam = image_data.create_dataset('R_i2bcam',
(num_steps/image_modulus, 9))
ii = 1
while system.successful() and system.t < tf:
# integrate the system and save state to an array
time[ii] = (system.t + dt)
i_state[ii, :] = (system.integrate(system.t + dt))
# generate the view of the asteroid at this state
if int(time[ii]) % image_modulus== 0 and gen_images:
img, RT, R = blender.gen_image_fixed_ast(i_state[ii,0:3],
i_state[ii,6:15].reshape((3,3)),
ast.omega * (time[ii] - 3600),
camera_obj, camera,
lamp_obj, lamp,
itokawa_obj, scene,
[5, 0, 1], 'test')
images[:, :, :, ii//image_modulus - 1] = img
RT_blender[ii//image_modulus -1, :] = RT.reshape(12)
R_i2bcam[ii//image_modulus -1, :] = R.reshape(9)
# do some image processing and visual odometry
print(system.t)
ii += 1
image_data.create_dataset('K', data=K)
image_data.create_dataset('i_state', data=i_state)
image_data.create_dataset('time', data=time)
type=str)
parser.add_argument(
"-m",
"--mode",
type=int,
choices=[0, 1],
help=
"Choose which inertial energy mode to run. 0 - inertial energy, 1 - \Delta E behavior"
)
args = parser.parse_args()
print("Starting the simulation...")
# instantiate the asteroid and dumbbell objects
ast = asteroid.Asteroid(args.ast_name, args.num_faces)
dum = dumbbell.Dumbbell(m1=500, m2=500, l=0.003)
# initialize simulation parameters
time = np.linspace(0, args.tf, args.num_steps)
initial_pos = periodic_pos # km for center of mass in body frame
initial_vel = periodic_vel + attitude.hat_map(
ast.omega * np.array([0, 0, 1])).dot(initial_pos)
initial_R = attitude.rot2(0).reshape(
9) # transforms from dumbbell body frame to the inertial frame
initial_w = np.array([0.01, 0.01, 0.01])
initial_state = np.hstack((initial_pos, initial_vel, initial_R, initial_w))
# echo back all the input parameters
print("This will run a long simulation with the following parameters!")
print(" ast_name - %s" % args.ast_name)
print(" num_faces - %s" % args.num_faces)
def create_plots(filename, plot_flags):
# logic to change the desired controller function
if plot_flags.mission == 'lissajous':
desired_attitude_func = controller.body_fixed_pointing_attitude
desired_translation_func = controller.inertial_lissajous_yz_plane
elif plot_flags.mission == 'circumnavigate':
desired_attitude_func = controller.body_fixed_pointing_attitude
desired_translation_func = controller.inertial_circumnavigate
# load the h5py file with all the imagery and simulation data
with h5py.File(filename, 'r') as sim_data:
sim_data.visit(printname)
K = sim_data['K']
i_state = sim_data['i_state']
time = sim_data['time']
images = sim_data['landing']
RT_vector = sim_data['RT']
R_bcam2i_vector = sim_data[
'R_i2bcam'] # the name is incorrect - actually it's bcamera to inertial frame
# R_ast2int = sim_data['Rast2inertial']
# define the asteroid and dumbbell objects like the simulation driver
ast_name = 'itokawa'
num_faces = 64
ast = asteroid.Asteroid(ast_name, num_faces)
dum = dumbbell.Dumbbell(m1=500, m2=500, l=0.003)
# draw some of the features from an example image
if plot_flags.feature_matching:
sift_flann_matching_image(
images[:, :, :, 3000],
images[:, :, :, 3200],
ratio=0.3,
plot=True,
filename='/tmp/itokawa_feature_matching.png',
save_fig=plot_flags.save_plots)
# draw the true and estimated trajectory
if plot_flags.simulation_plots:
if plot_flags.frame == 'inertial':
plotting.plot_controlled_blender_inertial(
time, i_state, ast, dum, plot_flags.save_plots, 1,
controller.traverse_then_land_vertically,
controller.body_fixed_pointing_attitude)
elif plot_flags.frame == 'asteroid':
plotting.plot_controlled_blender_inertial_fixed_asteroid(
time, i_state, ast, dum, plot_flags.save_plots, 1,
desired_translation_func, desired_attitude_func)
# create animation
if plot_flags.animation:
if plot_flags.frame == 'inertial':
plotting.animate_inertial_trajectory(time, i_state, ast, dum,
3600,
plot_flags.save_plots)
elif plot_flags.frame == 'asteroid':
plotting.animate_relative_trajectory(time, i_state, ast, dum,
plot_flags.save_plots)
if plot_flags.keyframe:
# plot_keyframe_trajectory(time, i_state, R_ast2int, R_bcam2i_vector,
# plot_flags.save_plots, fwidth=1,
# filename='/tmp/keyframe_estimate.eps')
if plot_flags.mission == 'asteroid':
plot_keyframe_original_asteroid(
time,
i_state,
R_bcam2i_vector,
plot_flags.save_plots,
fwidth=1,
filename='/tmp/keyframe_estimate.eps',
kf_path=plot_flags.kf_path)
elif plot_flags.mission == 'lissajous':
plot_keyframe_original_lissajous(
time,
i_state,
R_bcam2i_vector,
plot_flags.save_plots,
fwidth=1,
filename='/tmp/keyframe_estimate.eps',
kf_path=plot_flags.kf_path)
if plot_flags.blender_png: # generate blender images
output_path = './visualization/blender'
num_images = images.shape[3]
for ii in range(num_images):
cv2.imwrite(
output_path + '/test' + str.zfill(str(ii), 6) + '.png',
images[:, :, :, ii])
print("Saving image {0}/{1}".format(ii, num_images))
print("Finished extracting all the images")
class TestInertialandRelativeEOMS():
"""Compare the inertial and relative eoms against one another
"""
RelTol = 1e-9
AbsTol = 1e-9
ast_name = 'castalia'
num_faces = 64
tf = 1e2
num_steps = 1e2
time = np.linspace(0, tf, num_steps)
periodic_pos = np.array(
[1.495746722510590, 0.000001002669660, 0.006129720493607])
periodic_vel = np.array(
[0.000000302161724, -0.000899607989820, -0.000000013286327])
ast = asteroid.Asteroid(ast_name, num_faces)
dum = dumbbell.Dumbbell(m1=500, m2=500, l=0.003)
# set initial state for inertial EOMs
initial_pos = periodic_pos # km for center of mass in body frame
initial_vel = periodic_vel + att.hat_map(
ast.omega * np.array([0, 0, 1])).dot(initial_pos)
initial_R = att.rot2(np.pi / 2).reshape(
9) # transforms from dumbbell body frame to the inertial frame
initial_w = np.array([0.01, 0.01, 0.01])
initial_state = np.hstack((initial_pos, initial_vel, initial_R, initial_w))
i_state = integrate.odeint(dum.eoms_inertial,
initial_state,
time,
args=(ast, ),
atol=AbsTol,
rtol=RelTol)
# (i_time, i_state) = idriver.eom_inertial_driver(initial_state, time, ast, dum, AbsTol=1e-9, RelTol=1e-9)
initial_lin_mom = (dum.m1 + dum.m2) * (periodic_vel + att.hat_map(
ast.omega * np.array([0, 0, 1])).dot(initial_pos))
initial_ang_mom = initial_R.reshape((3, 3)).dot(dum.J).dot(initial_w)
initial_ham_state = np.hstack(
(initial_pos, initial_lin_mom, initial_R, initial_ang_mom))
rh_state = integrate.odeint(dum.eoms_hamilton_relative,
initial_ham_state,
time,
args=(ast, ),
atol=AbsTol,
rtol=RelTol)
# now convert both into the inertial frame
istate_ham = transform.eoms_hamilton_relative_to_inertial(
time, rh_state, ast, dum)
istate_int = transform.eoms_inertial_to_inertial(time, i_state, ast, dum)
# also convert both into the asteroid frame and compare
astate_ham = transform.eoms_hamilton_relative_to_asteroid(
time, rh_state, ast, dum)
astate_int = transform.eoms_inertial_to_asteroid(time, i_state, ast, dum)
def test_inertial_frame_comparison_pos(self):
np.testing.assert_array_almost_equal(self.istate_ham[:, 0:3],
self.istate_int[:, 0:3])
def test_inertial_frame_comparison_vel(self):
np.testing.assert_array_almost_equal(self.istate_ham[:, 3:6],
self.istate_int[:, 3:6])
def test_inertial_frame_comparison_att(self):
np.testing.assert_array_almost_equal(self.istate_ham[:, 6:15],
self.istate_int[:, 6:15])
def test_inertial_frame_comparison_ang_vel(self):
np.testing.assert_array_almost_equal(self.istate_ham[:, 15:18],
self.istate_int[:, 15:18])
def test_asteroid_frame_comparison_pos(self):
np.testing.assert_array_almost_equal(self.astate_ham[:, 0:3],
self.astate_int[:, 0:3])
def test_asteroid_frame_comparison_vel(self):
np.testing.assert_array_almost_equal(self.astate_ham[:, 3:6],
self.astate_int[:, 3:6])
def test_asteroid_frame_comparison_att(self):
np.testing.assert_array_almost_equal(self.astate_ham[:, 6:15],
self.astate_int[:, 6:15])
def test_asteroid_frame_comparison_ang_vel(self):
np.testing.assert_array_almost_equal(self.astate_ham[:, 15:18],
self.astate_int[:, 15:18])
from __future__ import absolute_import, division, print_function, unicode_literals
from dynamics import dumbbell, asteroid, controller, eoms
from kinematics import attitude
import numpy as np
import pdb
angle = (2 * np.pi - 0) * np.random.rand(1) + 0
# generate a random initial state that is outside of the asteroid
pos = np.random.rand(3) + np.array([2, 2, 2])
R = attitude.rot1(angle).reshape(9)
vel = np.random.rand(3)
ang_vel = np.random.rand(3)
t = np.random.rand() * 100
state = np.hstack((pos, vel, R, ang_vel))
ast = asteroid.Asteroid('castalia', 32)
dum = dumbbell.Dumbbell()
def test_eoms_controlled_relative_blender_ode_size():
state_dot = eoms.eoms_controlled_relative_blender_ode(t, state, dum, ast)
np.testing.assert_equal(state_dot.shape, (18, ))
def incremental_reconstruction(input_filename,
output_filename,
asteroid_name='castalia'):
"""Incrementally update the mesh
Now we'll use the radial mesh reconstruction.
"""
logger = logging.getLogger(__name__)
# output_filename = './data/raycasting/20180226_castalia_reconstruct_highres_45deg_cone.hdf5'
logger.info('Loading {}'.format(input_filename))
data = np.load(input_filename)
point_cloud = data['point_cloud'][()]
# define the asteroid and dumbbell objects
asteroid_faces = 0
asteroid_type = 'obj'
m1, m2, l = 500, 500, 0.003
ellipsoid_min_angle = 10
ellipsoid_max_radius = 0.03
ellipsoid_max_distance = 0.5
surf_area = 0.01
ast = asteroid.Asteroid(asteroid_name, asteroid_faces, asteroid_type)
dum = dumbbell.Dumbbell(m1=m1, m2=m2, l=l)
logger.info('Creating ellipsoid mesh')
# define a simple mesh to start
ellipsoid = surface_mesh.SurfMesh(ast.axes[0], ast.axes[1], ast.axes[2],
ellipsoid_min_angle,
ellipsoid_max_radius,
ellipsoid_max_distance)
v_est, f_est = ellipsoid.verts(), ellipsoid.faces()
vert_weight = np.full(v_est.shape[0], (np.pi * np.max(ast.axes))**2)
max_angle = wavefront.spherical_surface_area(np.max(ast.axes), surf_area)
# extract out all the points in the asteroid frame
time = point_cloud['time'][::1]
ast_ints = point_cloud['ast_ints'][::1]
logger.info('Create HDF5 file {}'.format(output_filename))
with h5py.File(output_filename, 'w') as fout:
# store some extra data about teh simulation
v_group = fout.create_group('reconstructed_vertex')
f_group = fout.create_group('reconstructed_face')
w_group = fout.create_group('reconstructed_weight')
sim_data = fout.create_group('simulation_data')
sim_data.attrs['asteroid_name'] = np.string_(asteroid_name)
sim_data.attrs['asteroid_faces'] = asteroid_faces
sim_data.attrs['asteroid_type'] = np.string_(asteroid_type)
sim_data.attrs['m1'] = dum.m1
sim_data.attrs['m2'] = dum.m2
sim_data.attrs['l'] = dum.l
sim_data.attrs['ellipsoid_axes'] = ast.axes
sim_data.attrs['ellipsoid_min_angle'] = ellipsoid_min_angle
sim_data.attrs['ellipsoid_max_radius'] = ellipsoid_max_radius
sim_data.attrs['ellipsoid_max_distance'] = ellipsoid_max_distance
sim_data.attrs['surf_area'] = surf_area
sim_data.attrs['max_angle'] = max_angle
fout.create_dataset('truth_vertex', data=ast.V)
fout.create_dataset('truth_faces', data=ast.F)
fout.create_dataset('estimate_faces', data=f_est)
fout.create_dataset('initial_vertex', data=v_est)
fout.create_dataset('initial_faces', data=f_est)
fout.create_dataset('initial_weight', data=vert_weight)
logger.info('Starting loop over point cloud')
for ii, (t, points) in enumerate(zip(time, ast_ints)):
# check if points is empty
logger.info('Current : t = {} with {} points'.format(
t, len(points)))
for pt in points:
# incremental update for each point in points
# check to make sure each pt is not nan
if not np.any(np.isnan(pt)):
v_est, vert_weight = wavefront.spherical_incremental_mesh_update(
pt,
v_est,
f_est,
vertex_weight=vert_weight,
max_angle=max_angle)
# use HD5PY instead
# save every so often and delete v_array,f_array to save memory
if (ii % 1) == 0:
logger.info('Saving data to HDF5. ii = {}, t = {}'.format(
ii, t))
v_group.create_dataset(str(ii), data=v_est)
f_group.create_dataset(str(ii), data=f_est)
w_group.create_dataset(str(ii), data=vert_weight)
logger.info('Completed the reconstruction')
return 0
