def __init__(self, filename, orig_flydra_R, out_fname=None):
self.srcR = flydra.reconstruct.Reconstructor(orig_flydra_R)
self.orig_XCR_e = MultiCalSelfCam.read_calibration_result(
orig_flydra_R)
self.out_flydra_R = orig_flydra_R + '.aligned'
print self.out_flydra_R
assert not os.path.exists(self.out_flydra_R)
pcd = pcl.PointCloud()
pcd.from_file(filename)
self.verts = pcd.to_array()
height = 1.0
self.cyl = Cylinder(base=dict(x=0, y=0, z=0),
axis=dict(x=0, y=0, z=height),
radius=0.5)
self._centers = np.zeros_like(self.verts)
self._centers[:, 2] = height / 2.0
if out_fname is None:
base = os.path.splitext(filename)[0]
self.out_fname = base + '-cyl.pcd'
else:
self.out_fname = out_fname
assert self.out_fname.endswith('.pcd')
def __init__(self, filename, orig_flydra_R, out_fname=None):
self.srcR = flydra.reconstruct.Reconstructor( orig_flydra_R )
self.orig_XCR_e = MultiCalSelfCam.read_calibration_result( orig_flydra_R )
self.out_flydra_R = orig_flydra_R + '.aligned'
print self.out_flydra_R
assert not os.path.exists(self.out_flydra_R)
pcd = pcl.PointCloud()
pcd.from_file( filename )
self.verts = pcd.to_array()
height = 1.0
self.cyl = Cylinder( base=dict(x=0,y=0,z=0),
axis=dict(x=0,y=0,z=height),
radius=0.5)
self._centers = np.zeros_like(self.verts)
self._centers[:,2] = height/2.0
if out_fname is None:
base = os.path.splitext(filename)[0]
self.out_fname = base + '-cyl.pcd'
else:
self.out_fname = out_fname
assert self.out_fname.endswith('.pcd')
class Autofitter:
def __init__(self, filename, orig_flydra_R, out_fname=None):
self.srcR = flydra.reconstruct.Reconstructor(orig_flydra_R)
self.orig_XCR_e = MultiCalSelfCam.read_calibration_result(
orig_flydra_R)
self.out_flydra_R = orig_flydra_R + '.aligned'
print self.out_flydra_R
assert not os.path.exists(self.out_flydra_R)
pcd = pcl.PointCloud()
pcd.from_file(filename)
self.verts = pcd.to_array()
height = 1.0
self.cyl = Cylinder(base=dict(x=0, y=0, z=0),
axis=dict(x=0, y=0, z=height),
radius=0.5)
self._centers = np.zeros_like(self.verts)
self._centers[:, 2] = height / 2.0
if out_fname is None:
base = os.path.splitext(filename)[0]
self.out_fname = base + '-cyl.pcd'
else:
self.out_fname = out_fname
assert self.out_fname.endswith('.pcd')
def run(self):
# make some initial guesses.
mean_v = np.mean(self.verts, axis=0)
std_v = np.std(self.verts, axis=0)
# FIXME: this is hard-coded to a unit cylinder.
target_center = np.array([0, 0, 0.5])
translate = target_center - mean_v
scale = np.mean(0.5 / std_v) # this is hacky...
quat = tf.transformations.quaternion_from_euler(0, 0, 0)
p0 = np.array([scale] + translate.tolist() + quat.tolist())
print 'p0'
print p0
self.orig_scale = scale
if 1:
recon0 = self.get_recon_from_param_vec(p0)
M0 = recon0.get_transformation_matrix()
recon0.save_as_json('recon0.json')
if 1:
# debug xform before opt
xe, ce, re = self.orig_XCR_e
MultiCalSelfCam.transform_and_save(xe, ce, re, '/tmp', recon0)
start_err = self.calc_error(p0)
print 'start_err', start_err
p01 = np.asfarray(p0)
p02 = p01.flatten()
all_out = fmin(self.calc_error, p0, full_output=True)
popt = all_out[0]
final_err = self.calc_error(popt)
print 'final_err', final_err
print popt
print repr(popt)
recon_opt = self.get_recon_from_param_vec(popt)
if 1:
recon_opt.save_as_json('recon_opt.json')
aligned_verts = recon_opt.move_cloud(self.verts)
transformed_points = pcl.PointCloud()
transformed_points.from_array(aligned_verts.astype(np.float32))
transformed_points.to_file(self.out_fname)
print 'saved transformed points to', self.out_fname
M_opt = recon_opt.get_transformation_matrix()
if 1:
alignedR = self.srcR.get_aligned_copy(M_opt)
alignedR.save_to_files_in_new_directory(self.out_flydra_R)
xe, ce, re = self.orig_XCR_e
MultiCalSelfCam.transform_and_save(xe, ce, re, self.out_flydra_R,
recon_opt)
def get_recon_from_param_vec(self, p):
s, tx, ty, tz, q0, q1, q2, q3 = p
t = np.array([[tx, ty, tz]], dtype=np.float)
R = tf.transformations.quaternion_matrix((q0, q1, q2, q3))[:3, :3]
recon = PointCloudTransformer(scale=s, translate=t, rotate=R)
return recon
def calc_error(self, p):
"""calculate the distance from the unit cylinder of the verts transformed by p"""
recon = self.get_recon_from_param_vec(p)
new_cloud = recon.move_cloud(self.verts)
surf1 = self.cyl.get_first_surface(self._centers, new_cloud)
valid_cond = ~np.isnan(surf1[:, 0])
valid_surf = surf1[valid_cond]
valid_new = new_cloud[valid_cond]
dist_squared = np.sum((valid_surf - valid_new)**2, axis=1)
n_invalid = np.sum(~valid_cond)
invalid_penalty = 1e5
this_scale = p[0]
scale_diff = this_scale - self.orig_scale
scale_penalty = 1e8 * scale_diff**4.0
error = np.mean(
dist_squared) + n_invalid * invalid_penalty + scale_penalty
return error
class Autofitter:
def __init__(self, filename, orig_flydra_R, out_fname=None):
self.srcR = flydra.reconstruct.Reconstructor( orig_flydra_R )
self.orig_XCR_e = MultiCalSelfCam.read_calibration_result( orig_flydra_R )
self.out_flydra_R = orig_flydra_R + '.aligned'
print self.out_flydra_R
assert not os.path.exists(self.out_flydra_R)
pcd = pcl.PointCloud()
pcd.from_file( filename )
self.verts = pcd.to_array()
height = 1.0
self.cyl = Cylinder( base=dict(x=0,y=0,z=0),
axis=dict(x=0,y=0,z=height),
radius=0.5)
self._centers = np.zeros_like(self.verts)
self._centers[:,2] = height/2.0
if out_fname is None:
base = os.path.splitext(filename)[0]
self.out_fname = base + '-cyl.pcd'
else:
self.out_fname = out_fname
assert self.out_fname.endswith('.pcd')
def run(self):
# make some initial guesses.
mean_v = np.mean(self.verts,axis=0)
std_v = np.std(self.verts,axis=0)
# FIXME: this is hard-coded to a unit cylinder.
target_center = np.array([0,0,0.5])
translate = target_center - mean_v
scale = np.mean(0.5/std_v) # this is hacky...
quat = tf.transformations.quaternion_from_euler(0,0,0)
p0 = np.array([scale] + translate.tolist() + quat.tolist())
print 'p0'
print p0
self.orig_scale = scale
if 1:
recon0 = self.get_recon_from_param_vec(p0)
M0 = recon0.get_transformation_matrix()
recon0.save_as_json('recon0.json')
if 1:
# debug xform before opt
xe, ce, re = self.orig_XCR_e
MultiCalSelfCam.transform_and_save( xe, ce, re, '/tmp', recon0 )
start_err = self.calc_error( p0 )
print 'start_err',start_err
p01 = np.asfarray(p0)
p02 = p01.flatten()
all_out = fmin( self.calc_error, p0, full_output=True)
popt = all_out[0]
final_err = self.calc_error( popt )
print 'final_err',final_err
print popt
print repr(popt)
recon_opt = self.get_recon_from_param_vec(popt)
if 1:
recon_opt.save_as_json('recon_opt.json')
aligned_verts = recon_opt.move_cloud(self.verts)
transformed_points = pcl.PointCloud()
transformed_points.from_array(aligned_verts.astype(np.float32))
transformed_points.to_file( self.out_fname )
print 'saved transformed points to', self.out_fname
M_opt = recon_opt.get_transformation_matrix()
if 1:
alignedR = self.srcR.get_aligned_copy(M_opt)
alignedR.save_to_files_in_new_directory(self.out_flydra_R)
xe, ce, re = self.orig_XCR_e
MultiCalSelfCam.transform_and_save( xe, ce, re, self.out_flydra_R, recon_opt )
def get_recon_from_param_vec(self,p):
s, tx, ty, tz, q0, q1, q2, q3 = p
t = np.array( [[tx,ty,tz]], dtype=np.float )
R = tf.transformations.quaternion_matrix( (q0,q1,q2,q3) )[:3,:3]
recon = PointCloudTransformer(scale=s,
translate=t,
rotate=R)
return recon
def calc_error(self, p ):
"""calculate the distance from the unit cylinder of the verts transformed by p"""
recon = self.get_recon_from_param_vec(p)
new_cloud = recon.move_cloud(self.verts)
surf1 = self.cyl.get_first_surface( self._centers, new_cloud )
valid_cond = ~np.isnan(surf1[:,0])
valid_surf = surf1[valid_cond]
valid_new = new_cloud[valid_cond]
dist_squared = np.sum((valid_surf-valid_new)**2,axis=1)
n_invalid = np.sum( ~valid_cond )
invalid_penalty = 1e5
this_scale = p[0]
scale_diff = this_scale-self.orig_scale
scale_penalty = 1e8* scale_diff**4.0
error = np.mean(dist_squared) + n_invalid*invalid_penalty + scale_penalty
return error
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import roslib
roslib.load_manifest('freemoovr')
from freemoovr.simple_geom import Cylinder, Vec3
def vec3(a, b, c):
return dict(x=a, y=b, z=c)
if 1:
cyl = Cylinder(base=vec3(0, 0, 0), axis=vec3(0, 0, 1), radius=0.5)
tc0 = []
tc1 = []
n = 32
twopi = 2 * np.pi
dt = twopi / n
def xfrac(idx):
theta = (idx * dt) % twopi
frac = theta / twopi
return frac
for i in range(n + 1):
tc0.append(xfrac(i))
tc1.append(1.0)
tc0.append(xfrac(i))
if 1:
verts_world1 = np.hstack([
make_xy_circle_at_z(0.0),
make_xy_circle_at_z(0.9),
make_xy_circle_at_z(1.0)
])
observer_pos = np.array([[0.22], [0.22], [0.9]])
#observer_pos = np.array([[0.4], [0], [0.95]])
#observer_pos = np.array([[0.0], [0.0], [0.0]])
verts_observer = verts_world1 - observer_pos
verts_observer_cube = to_cubemap(verts_observer)
cyl = Cylinder(base=vec3(0, 0, 0), axis=vec3(0, 0, 1), radius=0.5)
mode = 'cubemap'
if mode == 'cubemap':
v3, faces = generate_text_cube()
observer_pos_all = np.repeat(observer_pos, v3.shape[1], axis=1)
wc = cyl.get_first_surface(observer_pos_all.T, v3.T)
tc = cyl.worldcoord2texcoord(wc)
elif mode == 'edge':
observer_pos_all = np.repeat(observer_pos,
verts_world1.shape[1],
axis=1)
wc = cyl.get_first_surface(observer_pos_all.T, verts_world1.T)
tc = cyl.worldcoord2texcoord(wc)
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import roslib
roslib.load_manifest('freemoovr')
from freemoovr.simple_geom import Cylinder, Vec3
def vec3(a,b,c):
return dict(x=a, y=b, z=c)
if 1:
cyl = Cylinder(base=vec3(0,0,0), axis=vec3(0,0,1), radius=0.5)
tc0 = []
tc1 = []
n = 32
twopi = 2*np.pi
dt = twopi/n
def xfrac(idx):
theta = (idx*dt)%twopi
frac = theta/twopi
return frac
for i in range(n+1):
tc0.append( xfrac(i) ); tc1.append( 1.0 )
tc0.append( xfrac(i) ); tc1.append( 0.0 )
tc0.append( xfrac(i+1) ); tc1.append( 0.0 )
tc0.append( xfrac(i) ); tc1.append( 1.0 )
tc = np.array( (tc0, tc1) ).T
wc = cyl.texcoord2worldcoord(tc)
result[row,1],
result[row,2])
print ' len %.2f'%(np.sqrt(np.sum(result[row]**2)),)
result[row] = [0,0,1]
return result
if 1:
verts_world1 = np.hstack([make_xy_circle_at_z(0.0), make_xy_circle_at_z(0.9), make_xy_circle_at_z(1.0)])
observer_pos = np.array([[0.22], [0.22], [0.9]])
#observer_pos = np.array([[0.4], [0], [0.95]])
#observer_pos = np.array([[0.0], [0.0], [0.0]])
verts_observer = verts_world1-observer_pos
verts_observer_cube = to_cubemap( verts_observer )
cyl = Cylinder(base=vec3(0,0,0), axis=vec3(0,0,1), radius=0.5)
mode = 'cubemap'
if mode=='cubemap':
v3, faces = generate_text_cube()
observer_pos_all = np.repeat( observer_pos, v3.shape[1], axis=1)
wc = cyl.get_first_surface( observer_pos_all.T, v3.T )
tc = cyl.worldcoord2texcoord( wc )
elif mode=='edge':
observer_pos_all = np.repeat( observer_pos, verts_world1.shape[1], axis=1)
wc = cyl.get_first_surface( observer_pos_all.T, verts_world1.T )
tc = cyl.worldcoord2texcoord( wc )
rez = 512
V,U = np.mgrid[0:1:rez*1j,0:1:rez*1j]
