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