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')
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))
