def get_matrix(self):
qx = cgtypes.quat().fromAngleAxis(self.r_x * D2R,
cgtypes.vec3(1, 0, 0))
qy = cgtypes.quat().fromAngleAxis(self.r_y * D2R,
cgtypes.vec3(0, 1, 0))
qz = cgtypes.quat().fromAngleAxis(self.r_z * D2R,
cgtypes.vec3(0, 0, 1))
Rx = cgmat2np(qx.toMat3())
Ry = cgmat2np(qy.toMat3())
Rz = cgmat2np(qz.toMat3())
R = np.dot(Rx, np.dot(Ry, Rz))
t = np.array([self.tx, self.ty, self.tz], np.float)
s = self.s
T = np.zeros((4, 4), dtype=np.float)
T[:3, :3] = s * R
T[:3, 3] = t
T[3, 3] = 1.0
# convert bool to -1 or 1
fx = fy = fz = 1
if self.flip_x:
fx = -1
if self.flip_y:
fy = -1
if self.flip_z:
fz = -1
flip = np.array(
[[fx, 0, 0, 0], [0, fy, 0, 0], [0, 0, fz, 0], [0, 0, 0, 1]],
dtype=np.float)
T = np.dot(flip, T)
# T = np.dot(T,flip)
return T
def get_pos_ori(self, t):
file_ts = self.data[:, 0]
tdiff = file_ts[1:] - file_ts[:-1]
if tdiff.min() < 0.0:
raise ValueError("animation path times go backwards!")
t = t % file_ts[-1] # wrap around
lower_idx = numpy.nonzero((file_ts <= t))[0][-1]
upper_idx = lower_idx + 1
lower_t = file_ts[lower_idx]
file_dt = file_ts[upper_idx] - lower_t
frac = (t - lower_t) / file_dt
pos_lower = self.data[lower_idx, 1:4]
ori_lower = cgtypes.quat(self.data[lower_idx, 4:8])
pos_upper = self.data[upper_idx, 1:4]
ori_upper = cgtypes.quat(self.data[upper_idx, 4:8])
pos = frac * (pos_upper - pos_lower) + pos_lower
if ori_lower != ori_upper:
ori = cgtypes.slerp(frac, ori_lower, ori_upper)
else:
ori = ori_lower
if np.isnan(ori.w):
raise ValueError('orientation is nan')
return pos, ori
def orientation_to_quat(U, roll_angle=0, force_pitch_0=False):
"""convert world coordinates to body-relative coordinates
inputs:
U is a unit vector indicating directon of body long axis
roll_angle is the roll angle (in radians)
output:
quaternion (4 tuple)
"""
if roll_angle != 0:
# I presume this has an effect on the b component of the quaternion
raise NotImplementedError('')
#if type(U) == nx.NumArray and len(U.shape)==2: # array of vectors
if 0:
result = QuatSeq()
for u in U:
if numpy.isnan(u[0]):
result.append(cgtypes.quat((nan, nan, nan, nan)))
else:
yaw, pitch = orientation_to_euler(u)
result.append(
euler_to_quat(yaw=yaw, pitch=pitch, roll=roll_angle))
return result
else:
if numpy.isnan(U[0]):
return cgtypes.quat((nan, nan, nan, nan))
yaw, pitch = orientation_to_euler(U)
if force_pitch_0:
pitch = 0
return euler_to_quat(yaw=yaw, pitch=pitch, roll=roll_angle)
def as_dict(self):
qx = cgtypes.quat().fromAngleAxis(self.r_x * D2R,
cgtypes.vec3(1, 0, 0))
qy = cgtypes.quat().fromAngleAxis(self.r_y * D2R,
cgtypes.vec3(0, 1, 0))
qz = cgtypes.quat().fromAngleAxis(self.r_z * D2R,
cgtypes.vec3(0, 0, 1))
Rx = cgmat2np(qx.toMat3())
Ry = cgmat2np(qy.toMat3())
Rz = cgmat2np(qz.toMat3())
_R = np.dot(Rx, np.dot(Ry, Rz))
# convert bool to -1 or 1
fx = fy = fz = 1
if self.flip_x:
fx = -1
if self.flip_y:
fy = -1
if self.flip_z:
fz = -1
flip = np.array([[fx, 0, 0], [0, fy, 0], [0, 0, fz]], dtype=np.float)
_R = np.dot(flip, _R)
s = float(self.s)
t = map(float, [self.tx, self.ty, self.tz])
R = []
for row in _R:
R.append(map(float, [i for i in row]))
result = {"s": s, "t": t, "R": R}
return result
def eval_pd(self, i):
# evaluate 3 values (perturbations in x,y,z directions)
PERTURB = 1e-6 # perturbation amount (must be less than sqrt(pi))
#PERTURB = 1e-10 # perturbation amount (must be less than sqrt(pi))
q_i = self.Q[i]
q_i_inverse = q_i.inverse()
qx = q_i * cgtypes.quat(0, PERTURB, 0, 0).exp()
self.Q[i] = qx
G_Qx = self.objective_function.eval(self.Q, changed_idx=i)
dist_x = abs((q_i_inverse * qx).log())
qy = q_i * cgtypes.quat(0, 0, PERTURB, 0).exp()
self.Q[i] = qy
G_Qy = self.objective_function.eval(self.Q, changed_idx=i)
dist_y = abs((q_i_inverse * qy).log())
qz = q_i * cgtypes.quat(0, 0, 0, PERTURB).exp()
self.Q[i] = qz
G_Qz = self.objective_function.eval(self.Q, changed_idx=i)
dist_z = abs((q_i_inverse * qz).log())
self.Q[i] = q_i
qdir = cgtypes.quat(0, (G_Qx - self.G_Q) / dist_x,
(G_Qy - self.G_Q) / dist_y,
(G_Qz - self.G_Q) / dist_z)
return qdir
def step(self, im_getter, pos_vec3, ori_quat ):
self.last_environment_map = None # clear cached image if it exists...
if type(pos_vec3) != cgtypes.vec3:
pos_vec3 = cgtypes.vec3( *pos_vec3 )
if type(ori_quat) != cgtypes.quat:
ori_quat = cgtypes.quat( *ori_quat )
# get direction of each face of the environment map cube as quat
for fn, viewdir in self.viewdirs.iteritems():
face_dir_quat = ori_quat*viewdir
im=im_getter( pos_vec3, face_dir_quat )
self.last_optical_images[fn] = im
whole_env_mapG = numpy.concatenate( [numpy.ravel(self.last_optical_images[fn][:,:,1])
for fn in self.cube_order], axis=0 )
self.responsesG = self.rweights*whole_env_mapG # sparse matrix times vector = vector
if self.full_spectrum:
whole_env_mapR = numpy.concatenate( [numpy.ravel(self.last_optical_images[fn][:,:,0])
for fn in self.cube_order], axis=0 )
self.responsesR = self.rweights * whole_env_mapR
whole_env_mapB = numpy.concatenate( [numpy.ravel(self.last_optical_images[fn][:,:,2])
for fn in self.cube_order], axis=0 )
self.responsesB = self.rweights * whole_env_mapB
self.emd_sim.step( self.responsesG )
def turnBy(self, angle, direction='lr'):
"""
Crucial function for turtle movement - turtle rotating around axis that depends on both
type of movement and turtle's direction and rotation - using vector multiplication and
in one place quaternions.
"""
if direction == 'lr':
# Left-right movement is around the rotation axis.
rot_axis_vec = self._rot
elif direction == 'tilt':
# Tilting up or down occurs around the axis perpendicular to the direction vector
# and the rotation axis (ie. the cross product of the two vectors).
rot_axis_vec = self._rot.cross(self._dir)
else:
raise RuntimeError("Unsuppoorted turning direction")
# Using the angle and the rotation axis unit vector, we create a quaternion for the
# rotation.
# See http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation for more info.
angle_rad = math.radians(angle)
q_rotate = quat(angle_rad, rot_axis_vec)
# Use the quaternion to rotate the direction vector around the rotation axis by the given
# angle.
new_dir_vec = q_rotate.rotateVec(self._dir)
self._dir = self.setDir(*new_dir_vec)
if direction == 'tilt':
# Recalculate the rotation axis vector. This is the cross product of the newly
# calulated direction vector and the rotation vector we just used.
# TODO: move into direction=='tilt' block above, but watch out for pass by ref issue
self._rot = self._dir.cross(rot_axis_vec)
def quat_to_orient(S3):
"""returns x, y, z for unit quaternions
Parameters
----------
S3 : {quaternion, QuatSeq instance}
The quaternion or sequence of quaternions to transform into an
orientation vector.
Returns
-------
orient : {tuple, ndarray}
If the input was a single quaternion, the output is (x,y,z). If
the input was a QuatSeq instance of length N, the output is an
ndarray of shape (N,3).
"""
u = cgtypes.quat(0, 1, 0, 0)
if type(S3)() == QuatSeq(): # XXX isinstance(S3,QuatSeq)
V = [q * u * q.inverse() for q in S3]
return nx.array([(v.x, v.y, v.z) for v in V])
else:
V = S3 * u * S3.inverse()
return V.x, V.y, V.z
def step(self, pos_vec3, ori_quat):
if type(pos_vec3) != cgtypes.vec3:
pos_vec3 = cgtypes.vec3( *pos_vec3 )
if type(ori_quat) != cgtypes.quat:
ori_quat = cgtypes.quat( *ori_quat )
# calculate photoreceptors, EMDs, etc...
self.cvs.step( self.sim.get_flyview, pos_vec3, ori_quat )
self.last_pos_vec3 = pos_vec3
self.last_ori_quat = ori_quat
if self.plot_realtime:
if self.full_spectrum:
R = self.get_last_retinal_imageR()
G = self.get_last_retinal_imageG()
B = self.get_last_retinal_imageB()
else:
R = self.get_last_retinal_imageG()
G = self.get_last_retinal_imageG()
B = self.get_last_retinal_imageG()
A = numpy.ones( B.shape, dtype=numpy.float32 )
RGBA = numpy.vstack([R,G,B,A]).T
RGBA[:,:3] = RGBA[:,:3]/255.0
for num,eye_name in enumerate(self.rp.get_eye_names()):
slicer = self.rp.get_slicer( eye_name )
self.sim.set_eyemap_face_colors(num,RGBA[slicer,:])
def turnBy(self, angle, direction='lr'):
"""
Crucial function for turtle movement - turtle rotating around axis that depends on both
type of movement and turtle's direction and rotation - using vector multiplication and
in one place quaternions.
"""
if direction == 'lr':
# Left-right movement is around the rotation axis.
rot_axis_vec = self._rot
elif direction == 'tilt':
# Tilting up or down occurs around the axis perpendicular to the direction vector
# and the rotation axis (ie. the cross product of the two vectors).
rot_axis_vec = self._rot.cross(self._dir)
else:
raise RuntimeError("Unsuppoorted turning direction")
# Using the angle and the rotation axis unit vector, we create a quaternion for the
# rotation.
# See http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation for more info.
angle_rad = math.radians(angle)
q_rotate = quat(angle_rad, rot_axis_vec)
# Use the quaternion to rotate the direction vector around the rotation axis by the given
# angle.
new_dir_vec = q_rotate.rotateVec(self._dir)
self._dir = self.setDir(*new_dir_vec)
if direction == 'tilt':
# Recalculate the rotation axis vector. This is the cross product of the newly
# calulated direction vector and the rotation vector we just used.
# TODO: move into direction=='tilt' block above, but watch out for pass by ref issue
self._rot = self._dir.cross(rot_axis_vec)
def __init__(self,
pos_0=cgtypes.vec3(0,0,0),
ori_0=cgtypes.quat(1,0,0,0),
vel_0=cgtypes.vec3(0,0,0),
):
self.pos_0 = pos_0
self.ori_0 = ori_0
self.vel_0 = vel_0
self.reset()
def go_render(vision, json, compute_mu, write_binary):
position = numpy.array(json["position"])
attitude = numpy.array(json["attitude"])
# XXX hack to try orientation theory
# correcting for left-handeness
# should use a reflection instead, but it's all good because rotz
attitude = attitude.T
linear_velocity_body = numpy.array(json["linear_velocity_body"])
angular_velocity_body = numpy.array(json["angular_velocity_body"])
linear_velocity_body = cgtypes.vec3(linear_velocity_body)
angular_velocity_body = cgtypes.vec3(angular_velocity_body)
position = cgtypes.vec3(position)
orientation = cgtypes.quat.fromMat(cgtypes.quat(), cgtypes.mat3(attitude))
vision.step(position, orientation)
R = vision.get_last_retinal_imageR()
G = vision.get_last_retinal_imageG()
B = vision.get_last_retinal_imageB()
y = (R + G + B) / 3.0 / 255
answer = {}
answer["position"] = list(position)
answer["orientation"] = numpy.array(orientation.toMat3()).tolist()
answer["linear_velocity_body"] = list(linear_velocity_body)
answer["angular_velocity_body"] = list(angular_velocity_body)
answer["luminance"] = y.tolist()
# Get Q_dot and mu from body velocities
# WARNING: Getting Q_dot and mu can be very slow
if compute_mu:
lvel = json["linear_velocity_body"]
avel = json["angular_velocity_body"]
linear_velocity_body = cgtypes.vec3(numpy.array(lvel))
angular_velocity_body = cgtypes.vec3(numpy.array(avel))
qdot, mu = vision.get_last_retinal_velocities(linear_velocity_body, angular_velocity_body)
answer["nearness"] = mu
answer["retinal_velocities"] = qdot
if write_binary:
what = answer["luminance"]
n = len(what)
s = struct.pack(">" + "f" * n, *what)
positive_answer({"binary_length": len(s), "mean": numpy.mean(what)}, "Sending binary data (n * 4 bytes)")
sys.stdout.write(s)
sys.stdout.flush()
else:
positive_answer(answer)
def test_em(position, orientation, fp_good, vu_good):
ori = cgtypes.quat().fromAngleAxis(
orientation[0] * D2R,
(orientation[1], orientation[2], orientation[3]))
if 0:
print 'ori.toAngleAxis()', ori.toAngleAxis()
print ori
o2 = cgtypes.quat().fromAngleAxis(*(ori.toAngleAxis()))
print o2
fp, vu = pos_ori2fu(position, ori)
# focal point direction is important, not FP itself...
fpdir_good = np.array(fp_good) - position
fpdir = np.array(fp) - position
print 'focal_point direction',
assert check_close(fpdir, fpdir_good)
print 'view_up',
assert check_close(vu, vu_good)
print
def test_cgmat2np():
point1 = (1, 0, 0)
point1_out = (0, 1, 0)
cg_quat = cgtypes.quat().fromAngleAxis(90.0 * D2R, (0, 0, 1))
cg_in = cgtypes.vec3(point1)
m_cg = cg_quat.toMat3()
cg_out = m_cg * cg_in
cg_out_tup = (cg_out[0], cg_out[1], cg_out[2])
assert np.allclose(cg_out_tup, point1_out)
m_np = cgmat2np(m_cg)
np_out = np.dot(m_np, point1)
assert np.allclose(np_out, point1_out)
def pos_ori2fu(pos, ori):
"""convert position (xyz) vector and orientation (wxyz) quat into focal_point and view_up"""
pos = cgtypes.vec3(pos)
ori = cgtypes.quat(ori)
if np.isnan(ori.w):
raise ValueError('ori is nan')
forward = cgtypes.vec3((0, 0, -1))
up = cgtypes.vec3((0, 1, 0))
mori = ori.toMat4().inverse()
#view_forward_dir = rotate(ori,forward)
view_forward_dir = mori * forward
focal_point = pos + view_forward_dir
if np.isnan(focal_point[0]):
raise ValueError('focal point is nan')
#view_up_dir = rotate(ori,up)
view_up_dir = mori * up
return focal_point, view_up_dir
def __mul__(self, other):
if isinstance(other, QuatSeq):
assert len(self) == len(other)
return QuatSeq([p * q for p, q in zip(self, other)])
elif hasattr(other, 'shape'): # ndarray
assert len(other.shape) == 2
if other.shape[1] == 3:
other = nx.concatenate((nx.zeros((other.shape[0], 1)), other),
axis=1)
assert other.shape[1] == 4
other = QuatSeq([cgtypes.quat(o) for o in other])
return self * other
else:
try:
other = float(other)
except (ValueError, TypeError):
raise TypeError(
'only know how to multiply QuatSeq with QuatSeq, n*3 array or float'
)
return QuatSeq([p * other for p in self])
def test_repr():
x = repr_vec3(1, 2, 3.0000001)
ra = repr(x)
x2 = eval(ra)
assert x2.z == x.z
y = [cgtypes.vec3(1, 2, 3.0000001)]
y2 = map(make_repr_able, y)
assert y[0].z == y2[0].z
x = repr_quat(0.1, 1, 2, 3.0000001)
ra = repr(x)
x2 = eval(ra)
assert x2.z == x.z
y = [cgtypes.quat(0.1, 1, 2, 3.0000001)]
y2 = map(make_repr_able, y)
assert y[0].z == y2[0].z
y3 = [y]
y4 = map(make_repr_able, y3)
assert y3[0][0].z == y4[0][0].z
def euler_to_quat(roll=0.0, pitch=0.0, yaw=0.0):
# see http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
# Supposedly the order is heading first, then attitude, the bank.
heading = yaw
attitude = pitch
bank = roll
c1 = math.cos(heading / 2)
s1 = math.sin(heading / 2)
c2 = math.cos(attitude / 2)
s2 = math.sin(attitude / 2)
c3 = math.cos(bank / 2)
s3 = math.sin(bank / 2)
c1c2 = c1 * c2
s1s2 = s1 * s2
w = c1c2 * c3 - s1s2 * s3
x = c1c2 * s3 + s1s2 * c3
y = s1 * c2 * c3 + c1 * s2 * s3
z = c1 * s2 * c3 - s1 * c2 * s3
z, y = y, -z
return cgtypes.quat(w, x, y, z)
def _calc_idxs_and_fixup_q(no_distance_penalty_idxs, q):
valid_cond = None
if no_distance_penalty_idxs is not None:
valid_cond = np.ones(len(q), dtype=np.bool)
for i in no_distance_penalty_idxs:
valid_cond[i] = 0
# Set missing quaternion to some arbitrary value. The
# _getEnergy() cost term should push it around to
# minimize accelerations and it won't get counted in
# the distance penalty.
q[i] = cgtypes.quat(1, 0, 0, 0)
else:
no_distance_penalty_idxs = []
for i, qi in enumerate(q):
if numpy.any(numpy.isnan([qi.w, qi.x, qi.y, qi.z])):
if not i in no_distance_penalty_idxs:
raise ValueError("you are passing data with 'nan' "
"values, but have not set the "
"no_distance_penalty_idxs argument")
return valid_cond, no_distance_penalty_idxs, q
def test_repr():
x = repr_vec3(1,2,3.0000001)
ra = repr(x)
x2 = eval(ra)
assert x2.z == x.z
y = [cgtypes.vec3(1,2,3.0000001)]
y2 = map(make_repr_able,y)
assert y[0].z == y2[0].z
x = repr_quat(0.1,1,2,3.0000001)
ra = repr(x)
x2 = eval(ra)
assert x2.z == x.z
y = [cgtypes.quat(0.1,1,2,3.0000001)]
y2 = map(make_repr_able,y)
assert y[0].z == y2[0].z
y3 = [y]
y4 = map(make_repr_able,y3)
assert y3[0][0].z == y4[0][0].z
def main():
hz = 200.0
tau_emd = 0.1
# get IIR filter coefficients for tau and sample freq
emd_lp_ba = fsee.EMDSim.FilterMaker(hz).iir_lowpass1(tau_emd)
pos_vec3 = cgtypes.vec3( 0, 0, 0)
#ori_quat = cgtypes.quat( 1, 0, 0, 0)
#ori_quat = cgtypes.quat().fromAngleAxis( math.pi*0.5, (0,0,1) )
model_path = os.path.join(fsee.data_dir,"models/alice_cylinder/alice_cylinder.osg")
vision = fsee.Observer.Observer(model_path=model_path,
scale=1000.0, # make cylinder model 1000x bigger
hz=hz,
skybox_basename=None, # no skybox
emd_lp_ba = emd_lp_ba,
full_spectrum=PLOT_RECEPTORS,
optics='buchner71',
do_luminance_adaptation=False,
)
vel = 0.5 # meters/sec
dt = 1/hz
mean_emds = {}
z = 2 # 2 mm
posname = 'alice'
# startpos = cgtypes.vec3( x_mm[0], y_mm[0], z))
if 1:
## for posname,startpos in [ ('left',cgtypes.vec3( 0, 110, 0)),
## #('center',cgtypes.vec3( 0, 0, 0)),
## #('right',cgtypes.vec3( 0, -110, 0)),
## ]:
sum_vec = None
tnow = 0.0
#pos = startpos
#posinc = cgtypes.vec3( vel*dt*1000.0, 0, 0) # mm per step
#posnow = startpos
count = 0
while count < len(x_mm):
ori_quat = cgtypes.quat().fromAngleAxis(theta[count],(0,0,1))
posnow = cgtypes.vec3(( x_mm[count], y_mm[count], z))
vision.step(posnow,ori_quat)
if 1:
#if (count-1)%20==0:
if 1:
fname = 'alice_envmap_%04d.png'%(count,)
vision.save_last_environment_map(fname)
EMDs = vision.get_last_emd_outputs()
if PLOT_RECEPTORS:
if (count-1)%20==0:
R=vision.get_last_retinal_imageR()
G=vision.get_last_retinal_imageG()
B=vision.get_last_retinal_imageB()
fsee.plot_utils.plot_receptor_and_emd_fig(
R=R,G=G,B=B,
emds=EMDs,
scale=5e-3,
figsize=(10,10),
dpi=100,
save_fname='receptors_%s_%04d.png'%(posname,count),
optics=vision.get_optics(),
proj='stere',
)
if sum_vec is None:
sum_vec = EMDs
else:
sum_vec += EMDs
#posnow = posnow + posinc
tnow += dt
count += 1
mean_emds[posname]= (sum_vec/count)
fd = open('full3D_emds.pkl','wb')
pickle.dump( mean_emds, fd )
fd.close()
vision = fsee.Observer.Observer(model_path=model_path,
hz=200.0,
full_spectrum=True,
optics='buchner71',
do_luminance_adaptation=False,
skybox_basename=os.path.join(fsee.data_dir,'Images/osgviewer_cubemap/'),
)
if 1:
angle = 30*D2R
dist2 = -1.5
dist3 = 0.5
ext = 'svg'
# view from fly position
pos_vec3 = cgtypes.vec3(0,0,10.0)
ori_quat = cgtypes.quat().fromAngleAxis( angle,(0,0,1))
vision.step(pos_vec3,ori_quat)
vision.save_last_environment_map('expanding_wall1.png')
R=vision.get_last_retinal_imageR()
G=vision.get_last_retinal_imageG()
B=vision.get_last_retinal_imageB()
#emds = vision.get_last_emd_outputs()
fsee.plot_utils.plot_receptor_and_emd_fig(
R=R,G=G,B=B,#emds=emds,
save_fname='expanding_wall_flyeye1.%s'%ext,
optics = vision.get_optics(),
proj='stere',
dpi=200)
pos_vec3 = cgtypes.vec3(dist2*np.cos(angle),dist2*np.sin(angle),10.0)
def make_quat(angle_radians, axis_of_rotation):
half_angle = angle_radians / 2.0
a = math.cos(half_angle)
b, c, d = math.sin(half_angle) * norm_vec(axis_of_rotation)
return cgtypes.quat(a, b, c, d)
from __future__ import absolute_import
from .PQmath import orientation_to_quat, quat_to_orient
import numpy
import cgtypes
q = orientation_to_quat((1, 0, 0))
angles = numpy.linspace(0, 2 * numpy.pi, 10)
def rotate_quat(quat, rotation_quat):
res = quat * rotation_quat * quat * quat.inverse()
return res
for angle in angles:
rotation_quat = cgtypes.quat().fromAngleAxis(angle, (0, 0, 1))
q2 = rotate_quat(q, rotation_quat)
o = quat_to_orient(q2)
print(o)
print("-=" * 20)
# 1 rev per second
angular_velocity = cgtypes.vec3((0, 0, 2 * numpy.pi))
times = numpy.linspace(0, 1.0, 10)
for t in times:
ang_change = float(t) * angular_velocity
angle = abs(ang_change)
if angle == 0.0:
axis = cgtypes.vec3((1, 0, 0))
else:
def statespace2cgtypes_quat(x):
return cgtypes.quat(x[6], x[3], x[4], x[5])
def get_last_retinal_velocities(self, vel_vec3, angular_vel, direction='ommatidia'):
# NOTE: Both vel_vec3 and angular_vel should be in body frame
x=self.last_pos_vec3 # eye origin
q=self.last_ori_quat
qi=q.inverse()
vel_vecs = []
mu_list = []
dir_body_list = []
dir_world_list = []
if direction == 'ommatidia':
dir_body_list = self.cvs.precomputed_optics_module.receptor_dirs
for dir_body in dir_body_list:
dir_world_quat = q*cgtypes.quat(0,dir_body.x, dir_body.y, dir_body.z)*qi
dir_world_list.append(cgtypes.vec3(dir_world_quat.x,
dir_world_quat.y,
dir_world_quat.z))
elif direction == 'emds':
dir_body_quats = self.emd_dirs_unrotated_quats
for dir_body_quat in dir_body_quats:
dir_world_quat=q*dir_body_quat*qi # dir_body_quat.rotate(q)
dir_world_list.append(cgtypes.vec3(dir_world_quat.x,
dir_world_quat.y,
dir_world_quat.z))
dir_body_list.append(cgtypes.vec3(dir_body_quat.x,
dir_body_quat.y,
dir_body_quat.z))
else:
print 'ERROR: Directions need to be either ommatidia or EMDs.'
quit()
# Need a more efficient way to do this loop
for n in range(len(dir_body_list)):
dir_body = dir_body_list[n]
dir_world = dir_world_list[n]
vend = x + (dir_world*1e5) # XXX should figure out maximum length in OSG
vstart = x + (dir_world*1e-5) # avoid intesecting with the origin
rx, ry, rz, is_hit = self.sim.get_world_point(vstart, vend)
if is_hit == 1: # is_hit is always 1 in the presence of skybox
sigma = (cgtypes.vec3(rx,ry,rz) - x).length() # distance
mu = 1.0/sigma
else:
mu = 0.0 # objects are at infinity
# Use the formula in Sean Humbert's paper to calculate retinal velocity
vr = - angular_vel.cross(dir_body) - mu * (vel_vec3 - dir_body * (dir_body*vel_vec3))
# Convert vr into spherical coordinates
# Equation: cf http://en.wikipedia.org/wiki/Vector_fields_in_cylindrical_and_spherical_coordinates
sx = dir_body.x
sy = dir_body.y
sz = dir_body.z
phi = numpy.math.atan2(sy,sx)
theta = numpy.math.acos(sz)
cphi = numpy.cos(phi)
sphi = numpy.sin(phi)
ctheta = numpy.cos(theta)
stheta = numpy.sin(theta)
R = cgtypes.mat3(stheta*cphi, stheta*sphi, ctheta,
ctheta*cphi, ctheta*sphi, -stheta,
-sphi, cphi, 0) # Row-major order
vr = R*vr
# vr[0]: r (should be zero), vr[1]: theta, vr[2]: phi
vel_vecs.append([vr[1],vr[2]])
mu_list.append(mu)
return vel_vecs, mu_list
def make_receptor_sensitivities(all_d_q,delta_rho_q=None,res=64):
"""
all_d_q are visual element directions as a 3-vector
delta_rho_q (angular sensitivity) is in radians
"""
if delta_rho_q is None:
raise ValueError('must specify delta_rho_q (in radians)')
if isinstance( delta_rho_q, float):
all_delta_rho_qs = delta_rho_q*numpy.ones( (len(all_d_q),), dtype=numpy.float64)
else:
all_delta_rho_qs = numpy.asarray(delta_rho_q)
if len(all_delta_rho_qs.shape) != 1:
raise ValueError("delta_rho_q must be scalar or vector")
if all_delta_rho_qs.shape[0] != len(all_d_q):
raise ValueError("if delta_rho_q is a vector, "
"it must have the same number of "
"elements as receptors")
def G_q(zeta,delta_rho_q):
# gaussian
# From Snyder (1979) as cited in Burton & Laughlin (2003)
return numpy.exp( -4*math.log(2)*abs(zeta)**2 / delta_rho_q**2 )
half_res = res//2
vals = (numpy.arange(res)-half_res)/half_res
weight_maps = []
# setup vectors for initial face (posx)
face_vecs = {}
face_vecs['posx'] = []
x = 1
for z in vals:
this_row_vecs = []
for y in vals:
on_cube_3d = (x,y,z)
#print('on_cube_3d %5.2f %5.2f %5.2f'%on_cube_3d)
v3norm = normalize(on_cube_3d) # get direction of each pixel
p_p = cgtypes.quat(0.0, v3norm[0], v3norm[1], v3norm[2])
this_row_vecs.append(p_p)
this_row_vecs.reverse()
face_vecs['posx'].append( this_row_vecs )
def rot_face( facedict, facename, rotq):
facedict[facename] = []
for row in facedict['posx']:
this_row_vecs = []
for col in row:
this_row_vecs.append( rotq*col*rotq.inverse() )
facedict[facename].append( this_row_vecs )
rotq = cgtypes.quat()
rotq = rotq.fromAngleAxis(math.pi/2.0,cgtypes.vec3(0,0,1))
rot_face( face_vecs, 'posy', rotq)
rotq = cgtypes.quat()
rotq = rotq.fromAngleAxis(math.pi,cgtypes.vec3(0,0,1))
rot_face( face_vecs, 'negx', rotq)
rotq = cgtypes.quat()
rotq = rotq.fromAngleAxis(-math.pi/2.0,cgtypes.vec3(0,0,1))
rot_face( face_vecs, 'negy', rotq)
rotq = cgtypes.quat()
rotq = rotq.fromAngleAxis(math.pi/2.0,cgtypes.vec3(0,-1,0))
rot_face( face_vecs, 'posz', rotq)
rotq = cgtypes.quat()
rotq = rotq.fromAngleAxis(math.pi/2.0,cgtypes.vec3(0,1,0))
rot_face( face_vecs, 'negz', rotq)
# convert from quat to vec3
rfv = {}
for key in face_vecs:
rows = face_vecs[key]
rfv[key] = []
for row in rows:
this_row = [ cgtypes.vec3(col.x, col.y, col.z) for col in row ] # convert to vec3
rfv[key].append( this_row )
def get_weight_map(fn, rfv, d_q, delta_rho_q):
angles = numpy.zeros( (vals.shape[0], vals.shape[0]), dtype=numpy.float64 )
for i, row_vecs in enumerate(rfv[fn]):
for j, ovec in enumerate(row_vecs):
angles[i,j] = d_q.angle(ovec)
wm = G_q(angles,delta_rho_q)
return wm
for dqi,(d_q,this_delta_rho_q) in enumerate(zip(all_d_q,all_delta_rho_qs)):
weight_maps_d_q = {}
ssf = 0.0
for fn in cube_order:
wm = get_weight_map(fn, rfv, d_q, this_delta_rho_q)
weight_maps_d_q[fn] = wm
ssf += numpy.sum( wm.flat )
# normalize
for mapname in weight_maps_d_q:
wm = weight_maps_d_q[mapname]
weight_maps_d_q[mapname] = wm/ssf
# save maps by receptor direction
weight_maps.append( weight_maps_d_q )
return weight_maps
def test():
if 1:
pos = cgtypes.vec3(0, 0, 0)
forward = cgtypes.vec3(0, 0, -1)
up = cgtypes.vec3(0, 1, 0)
q = cgtypes.quat().fromAngleAxis(0, cgtypes.vec3(1, 0, 0))
fp = rotate(q, forward)
vu = rotate(q, up)
assert check_close(fp, forward)
assert check_close(vu, up)
q = cgtypes.quat().fromAngleAxis(np.pi, cgtypes.vec3(1, 0, 0))
fp = rotate(q, forward)
vu = rotate(q, up)
assert check_close(fp, (0, 0, 1))
assert check_close(vu, (0, -1, 0))
q = cgtypes.quat().fromAngleAxis(np.pi / 2, cgtypes.vec3(0, 1, 0))
q_mat = q.toMat4()
fp = rotate(q, forward)
vu = rotate(q, up)
assert check_close(fp, (-1, 0, 0))
assert check_close(vu, (0, 1, 0))
fp2 = q_mat * forward
assert check_close(fp, fp2)
vu2 = q_mat * up
assert check_close(vu, vu2)
print '*' * 40
if 0:
def test_rotate(q, x):
print 'x', x
r1 = rotate(q, x)
qmat = q.toMat4()
print 'qmat'
print qmat
r2 = qmat * x
assert check_close(r1, r2)
print
q = cgtypes.quat().fromAngleAxis(
46.891594344015928 * D2R,
(-0.99933050325884276, -0.028458760896155278,
0.022992263583291334))
a = cgtypes.vec3(1, 0, 0)
b = cgtypes.vec3(0, 1, 0)
c = cgtypes.vec3(0, 0, 1)
test_rotate(q, a)
test_rotate(q, b)
test_rotate(q, c)
if 1:
pos = (0.40262157300195972, 0.12141447782035097, 1.0)
ori = cgtypes.quat().fromAngleAxis(0.0, (0.0, 0.0, 1.0))
print 'ori.toAngleAxis()', ori.toAngleAxis()
print ori
fp_good = np.array((0.40262157300195972, 0.12141447782035097, 0.0))
vu_good = np.array((0, 1, 0))
#fp,vu = pos_ori2fu(pos,ori)
fp, vu = pos_ori2fu(pos, ori)
print 'fp_good', fp_good
print 'fp', fp
print
print 'vu_good', vu_good
print 'vu', vu
assert check_close(fp, fp_good)
assert check_close(vu, vu_good)
if 1:
def test_em(position, orientation, fp_good, vu_good):
ori = cgtypes.quat().fromAngleAxis(
orientation[0] * D2R,
(orientation[1], orientation[2], orientation[3]))
if 0:
print 'ori.toAngleAxis()', ori.toAngleAxis()
print ori
o2 = cgtypes.quat().fromAngleAxis(*(ori.toAngleAxis()))
print o2
fp, vu = pos_ori2fu(position, ori)
# focal point direction is important, not FP itself...
fpdir_good = np.array(fp_good) - position
fpdir = np.array(fp) - position
print 'focal_point direction',
assert check_close(fpdir, fpdir_good)
print 'view_up',
assert check_close(vu, vu_good)
print
# values from VTK's camera.position, camera.orientation_wxyz, camera.focal_point, camera.view_up
position = (0.4272878670512405, -0.55393877027170979,
0.68354826464002871)
orientation = (46.891594344015928, -0.99933050325884276,
-0.028458760896155278, 0.022992263583291334)
focal_point = (0.41378612268658882, 0.17584165753292449, 0.0)
view_up = (0.025790333690774738, 0.68363731594647714,
0.72936608019129556)
test_em(position, orientation, focal_point, view_up)
def QuatSlot(value=quat(), flags=0):
return _core.QuatSlot(value, flags)
def make_receptor_sensitivities(all_d_q, delta_rho_q=None, res=64):
"""
all_d_q are visual element directions as a 3-vector
delta_rho_q (angular sensitivity) is in radians
"""
if delta_rho_q is None:
raise ValueError('must specify delta_rho_q (in radians)')
if isinstance(delta_rho_q, float):
all_delta_rho_qs = delta_rho_q * numpy.ones(
(len(all_d_q), ), dtype=numpy.float64)
else:
all_delta_rho_qs = numpy.asarray(delta_rho_q)
if len(all_delta_rho_qs.shape) != 1:
raise ValueError("delta_rho_q must be scalar or vector")
if all_delta_rho_qs.shape[0] != len(all_d_q):
raise ValueError("if delta_rho_q is a vector, "
"it must have the same number of "
"elements as receptors")
def G_q(zeta, delta_rho_q):
# gaussian
# From Snyder (1979) as cited in Burton & Laughlin (2003)
return numpy.exp(-4 * math.log(2) * abs(zeta)**2 / delta_rho_q**2)
half_res = res // 2
vals = (numpy.arange(res) - half_res) / half_res
weight_maps = []
# setup vectors for initial face (posx)
face_vecs = {}
face_vecs['posx'] = []
x = 1
for z in vals:
this_row_vecs = []
for y in vals:
on_cube_3d = (x, y, z)
#print('on_cube_3d %5.2f %5.2f %5.2f'%on_cube_3d)
v3norm = normalize(on_cube_3d) # get direction of each pixel
p_p = cgtypes.quat(0.0, v3norm[0], v3norm[1], v3norm[2])
this_row_vecs.append(p_p)
this_row_vecs.reverse()
face_vecs['posx'].append(this_row_vecs)
def rot_face(facedict, facename, rotq):
facedict[facename] = []
for row in facedict['posx']:
this_row_vecs = []
for col in row:
this_row_vecs.append(rotq * col * rotq.inverse())
facedict[facename].append(this_row_vecs)
rotq = cgtypes.quat()
rotq = rotq.fromAngleAxis(math.pi / 2.0, cgtypes.vec3(0, 0, 1))
rot_face(face_vecs, 'posy', rotq)
rotq = cgtypes.quat()
rotq = rotq.fromAngleAxis(math.pi, cgtypes.vec3(0, 0, 1))
rot_face(face_vecs, 'negx', rotq)
rotq = cgtypes.quat()
rotq = rotq.fromAngleAxis(-math.pi / 2.0, cgtypes.vec3(0, 0, 1))
rot_face(face_vecs, 'negy', rotq)
rotq = cgtypes.quat()
rotq = rotq.fromAngleAxis(math.pi / 2.0, cgtypes.vec3(0, -1, 0))
rot_face(face_vecs, 'posz', rotq)
rotq = cgtypes.quat()
rotq = rotq.fromAngleAxis(math.pi / 2.0, cgtypes.vec3(0, 1, 0))
rot_face(face_vecs, 'negz', rotq)
# convert from quat to vec3
rfv = {}
for key in face_vecs:
rows = face_vecs[key]
rfv[key] = []
for row in rows:
this_row = [cgtypes.vec3(col.x, col.y, col.z)
for col in row] # convert to vec3
rfv[key].append(this_row)
def get_weight_map(fn, rfv, d_q, delta_rho_q):
angles = numpy.zeros((vals.shape[0], vals.shape[0]),
dtype=numpy.float64)
for i, row_vecs in enumerate(rfv[fn]):
for j, ovec in enumerate(row_vecs):
angles[i, j] = d_q.angle(ovec)
wm = G_q(angles, delta_rho_q)
return wm
for dqi, (d_q,
this_delta_rho_q) in enumerate(zip(all_d_q, all_delta_rho_qs)):
weight_maps_d_q = {}
ssf = 0.0
for fn in cube_order:
wm = get_weight_map(fn, rfv, d_q, this_delta_rho_q)
weight_maps_d_q[fn] = wm
ssf += numpy.sum(wm.flat)
# normalize
for mapname in weight_maps_d_q:
wm = weight_maps_d_q[mapname]
weight_maps_d_q[mapname] = wm / ssf
# save maps by receptor direction
weight_maps.append(weight_maps_d_q)
return weight_maps
vision = fsee.Observer.Observer(model_path=model_path,
hz=200.0,
full_spectrum=True,
optics='buchner71',
do_luminance_adaptation=False,
skybox_basename=os.path.join(fsee.data_dir,'Images/osgviewer_cubemap/'),
)
if 1:
angle = 30*D2R
dist2 = -1.5
dist3 = 0.5
ext = 'svg'
# view from fly position
pos_vec3 = cgtypes.vec3(0,0,10.0)
ori_quat = cgtypes.quat().fromAngleAxis( angle,(0,0,1))
vision.step(pos_vec3,ori_quat)
vision.save_last_environment_map('expanding_wall1.png')
R=vision.get_last_retinal_imageR()
G=vision.get_last_retinal_imageG()
B=vision.get_last_retinal_imageB()
#emds = vision.get_last_emd_outputs()
fsee.plot_utils.plot_receptor_and_emd_fig(
R=R,G=G,B=B,#emds=emds,
save_fname='expanding_wall_flyeye1.%s'%ext,
optics = vision.get_optics(),
proj='stere',
dpi=200)
pos_vec3 = cgtypes.vec3(dist2*np.cos(angle),dist2*np.sin(angle),10.0)
def __init__(self, proc):
_core.QuatSlot.__init__(self, quat(), 2) # 2 = NO_INPUT_CONNECTIONS
self._proc = proc
def main():
if 1:
# load some data from Alice's arena
data=scipy.io.loadmat('alice_data')
d2=data['DATA']
x=d2[0,:]
y=d2[1,:]
D2R = math.pi/180.0
theta=d2[2,:]*D2R
xoffset = 753
yoffset = 597
radius_pix = 753-188 # pixels
# 10 inch = 25.4 cm = 254 mm = diameter = 2*radius
pix_per_mm = 2.0*radius_pix/254.0
mm_per_pixel = 1.0/pix_per_mm
xgain = mm_per_pixel
ygain = mm_per_pixel
x_mm=(x-xoffset)*xgain
y_mm=(y-yoffset)*ygain
hz = 200.0
tau_emd = 0.1
# get IIR filter coefficients for tau and sample freq
emd_lp_ba = fsee.EMDSim.FilterMaker(hz).iir_lowpass1(tau_emd)
model_path = os.path.join(fsee.data_dir,"models/alice_cylinder/alice_cylinder.osg")
vision = fsee.Observer.Observer(model_path=model_path,
scale=1000.0, # make cylinder model 1000x bigger (put in mm instead of m)
hz=hz,
skybox_basename=None, # no skybox
emd_lp_ba = emd_lp_ba,
full_spectrum=True,
optics='buchner71',
do_luminance_adaptation=False,
)
dt = 1/hz
z = 2 # 2 mm
if 1:
count = 0
tstart = time.time()
try:
while count < len(x_mm):
ori_quat = cgtypes.quat().fromAngleAxis(theta[count],(0,0,1))
posnow = cgtypes.vec3(( x_mm[count], y_mm[count], z))
vision.step(posnow,ori_quat)
count += 1
finally:
tstop = time.time()
dur = tstop-tstart
fps = count/dur
print '%d frames rendered in %.1f seconds (%.1f fps)'%(count,dur,fps)
def copy(self):
"""return a deep copy of self"""
return QuatSeq([cgtypes.quat(q) for q in self])
def main():
hz = 200.0
tau_emd = 0.1
# get IIR filter coefficients for tau and sample freq
emd_lp_ba = fsee.EMDSim.FilterMaker(hz).iir_lowpass1(tau_emd)
pos_vec3 = cgtypes.vec3(0, 0, 0)
#ori_quat = cgtypes.quat( 1, 0, 0, 0)
#ori_quat = cgtypes.quat().fromAngleAxis( math.pi*0.5, (0,0,1) )
model_path = os.path.join(fsee.data_dir,
"models/alice_cylinder/alice_cylinder.osg")
vision = fsee.Observer.Observer(
model_path=model_path,
scale=1000.0, # make cylinder model 1000x bigger
hz=hz,
skybox_basename=None, # no skybox
emd_lp_ba=emd_lp_ba,
full_spectrum=PLOT_RECEPTORS,
optics='buchner71',
do_luminance_adaptation=False,
)
vel = 0.5 # meters/sec
dt = 1 / hz
mean_emds = {}
z = 2 # 2 mm
posname = 'alice'
# startpos = cgtypes.vec3( x_mm[0], y_mm[0], z))
if 1:
## for posname,startpos in [ ('left',cgtypes.vec3( 0, 110, 0)),
## #('center',cgtypes.vec3( 0, 0, 0)),
## #('right',cgtypes.vec3( 0, -110, 0)),
## ]:
sum_vec = None
tnow = 0.0
#pos = startpos
#posinc = cgtypes.vec3( vel*dt*1000.0, 0, 0) # mm per step
#posnow = startpos
count = 0
while count < len(x_mm):
ori_quat = cgtypes.quat().fromAngleAxis(theta[count], (0, 0, 1))
posnow = cgtypes.vec3((x_mm[count], y_mm[count], z))
vision.step(posnow, ori_quat)
if 1:
#if (count-1)%20==0:
if 1:
fname = 'alice_envmap_%04d.png' % (count, )
vision.save_last_environment_map(fname)
EMDs = vision.get_last_emd_outputs()
if PLOT_RECEPTORS:
if (count - 1) % 20 == 0:
R = vision.get_last_retinal_imageR()
G = vision.get_last_retinal_imageG()
B = vision.get_last_retinal_imageB()
fsee.plot_utils.plot_receptor_and_emd_fig(
R=R,
G=G,
B=B,
emds=EMDs,
scale=5e-3,
figsize=(10, 10),
dpi=100,
save_fname='receptors_%s_%04d.png' % (posname, count),
optics=vision.get_optics(),
proj='stere',
)
if sum_vec is None:
sum_vec = EMDs
else:
sum_vec += EMDs
#posnow = posnow + posinc
tnow += dt
count += 1
mean_emds[posname] = (sum_vec / count)
fd = open('full3D_emds.pkl', 'wb')
pickle.dump(mean_emds, fd)
fd.close()
def __init__(self,
# OpenSceneGraph parameters
model_path=None,
scale=1.0,
zmin=0.001,
zmax=1e10,
cuberes=64,
# EMD parameters
hz=200.0,
earlyvis_ba = None, # early vision filter (can be None)
early_contrast_saturation_params=None,
emd_lp_ba = None, # EMD arm 2
emd_hp_ba = None, # EMD arm 1 (can be None)
subtraction_imbalance = 1.0,
# mean_luminance_value = 127.5,
# other simulation parameters
skybox_basename=fsee.default_skybox,
full_spectrum=False,
optics=None,
do_luminance_adaptation=None,
plot_realtime=False
):
# AC: This is initialized only if we want to use it (later)
# self.rp = RapidPlotter(optics=optics)
self.rp = None
self.plot_realtime = plot_realtime
self.optics = optics
self.cubemap_face_xres=cuberes
self.cubemap_face_yres=cuberes
self.sim = Simulation.Simulation(
model_path=model_path,
scale=scale,
skybox_basename=skybox_basename,
zmin=zmin,
zmax=zmax,
xres=self.cubemap_face_xres,
yres=self.cubemap_face_yres,
)
## self.nodes = [world_node,
## self.sim.get_root_node(),
## self.sim.get_skybox_node(),
## ]
## self.nodes2names = {world_node:'world',
## self.sim.get_root_node():'root',
## self.sim.get_skybox_node():'skybox',
## }
## self.nodes2vels = {world_node:cgtypes.vec3(0,0,0),
## self.sim.get_root_node():cgtypes.vec3(0,0,0),
## self.sim.get_skybox_node():None, # no relative translation
## }
self.cvs = CoreVisualSystem(hz=hz,
cubemap_face_xres=self.cubemap_face_xres,
cubemap_face_yres=self.cubemap_face_yres,
earlyvis_ba = earlyvis_ba,
early_contrast_saturation_params=early_contrast_saturation_params,
emd_lp_ba = emd_lp_ba,
emd_hp_ba = emd_hp_ba,
subtraction_imbalance=subtraction_imbalance,
debug=False,
full_spectrum=full_spectrum,
#mean_luminance_value = mean_luminance_value,
optics=optics,
do_luminance_adaptation=do_luminance_adaptation,
)
precomputed = fsee.eye_geometry.switcher.get_module_for_optics(optics=optics)
for attr in ['get_last_emd_outputs',
'get_last_retinal_imageR',
'get_last_retinal_imageG',
'get_last_retinal_imageB',
'get_last_optical_image',
'get_last_environment_map',
'save_last_environment_map',
'get_values',
'get_optics',
'full_spectrum',
]:
# 'subclassing' without subclassing
setattr(self,attr, getattr(self.cvs,attr))
#self.receptor_dirs = precomputed.receptor_dirs
#self.emd_edges =
self.emd_dirs_unrotated_quats = []
for (vi1,vi2) in precomputed.edges:
v1 = precomputed.receptor_dirs[vi1]
v2 = precomputed.receptor_dirs[vi2]
v = (v1+v2)*0.5 # average
v = v.normalize() # make unit length
self.emd_dirs_unrotated_quats.append( cgtypes.quat(0,v.x,v.y,v.z))
if self.plot_realtime:
if self.rp is None:
from fsee.eye_geometry.projected_eye_coords import RapidPlotter
self.rp = RapidPlotter(optics=self.optics)
# strictly optional - realtime plotting stuff
minx = numpy.inf
maxx = -numpy.inf
miny = numpy.inf
maxy = -numpy.inf
for num,eye_name in enumerate(self.rp.get_eye_names()):
fans = self.rp.get_tri_fans(eye_name)
x=[];y=[];f=[]
for xs_ys in fans:
if xs_ys is None:
f.append(0)
continue
xs,ys=xs_ys
x.extend( xs )
y.extend( ys )
f.append( len(xs) )
x = numpy.array(x)
y = numpy.array(y)
minx = min(minx,x.min())
maxx = max(maxx,x.max())
miny = min(miny,y.min())
maxy = max(maxy,y.max())
self.sim.set_eyemap_geometry(num, x, y, f)
magx = maxx-minx
if self.get_optics() == 'buchner71':
if self.rp.flip_lon():
self.sim.set_eyemap_projection(0, maxx, minx-0.1*magx, miny, maxy)
self.sim.set_eyemap_projection(1, maxx+0.1*magx, minx, miny, maxy)
else:
self.sim.set_eyemap_projection(0, minx-0.1*magx, maxx, miny, maxy)
self.sim.set_eyemap_projection(1, minx, maxx+0.1*magx, miny, maxy)
elif self.get_optics() == 'synthetic':
self.sim.set_eyemap_projection(0, minx-1*magx, maxx, miny, maxy)
self.sim.set_eyemap_projection(1, minx, maxx+1*magx, miny, maxy)
def QuatSlot(value=quat(), flags=0):
return _core.QuatSlot(value, flags)
# Copyright (C) 2005-2008 California Institute of Technology, All rights reserved
import os
import cgtypes
import fsee
import fsee.Observer
pos_vec3 = cgtypes.vec3( 0, 0, 0)
ori_quat = cgtypes.quat( 1, 0, 0, 0)
model_path = os.path.join(fsee.data_dir,"models/tunnel_leftturn/tunnel.osg")
vision = fsee.Observer.Observer(model_path=model_path,
hz=200.0,
optics='synthetic',
do_luminance_adaptation=False,
)
vision.step(pos_vec3,ori_quat)
def __init__(self,
hz=200.0,
earlyvis_ba = None, # early vision filter (can be None)
early_contrast_saturation_params=None,
emd_lp_ba = None, # EMD arm 2
emd_hp_ba = None, # EMD arm 1 (can be None)
subtraction_imbalance = 1.0,
debug = False,
full_spectrum=False,
cubemap_face_xres=64,
cubemap_face_yres=64,
optics=None,
do_luminance_adaptation=None,
):
precomputed = fsee.eye_geometry.switcher.get_module_for_optics(optics=optics)
self.optics = optics
self.precomputed_optics_module = precomputed
self.cubemap_face_xres=cubemap_face_xres
self.cubemap_face_yres=cubemap_face_yres
self.full_spectrum=full_spectrum # do R and B channels (G always)
self.cube_order = precomputed.cube_order
# convert fsee body coordinates (ahead = +x, up = +z, right = -y)
# to OSG eye coordinates (ahead = -z, up = +y, right = +x)
self.viewdirs = {'posx':(cgtypes.quat().fromAngleAxis(-pi/2,(0,1,0))*
cgtypes.quat().fromAngleAxis(-pi/2,(0,0,1))),
'negx':(cgtypes.quat().fromAngleAxis(pi/2,(0,1,0))*
cgtypes.quat().fromAngleAxis(pi/2,(0,0,1))
),
'posy':cgtypes.quat().fromAngleAxis(pi/2,(1,0,0)),
'negy':(cgtypes.quat().fromAngleAxis(-pi/2,(1,0,0))*
cgtypes.quat().fromAngleAxis(pi,(0,0,1))),
'posz':(cgtypes.quat().fromAngleAxis(pi/2,(0,0,1))*
cgtypes.quat().fromAngleAxis(pi,(0,1,0))
),
'negz':cgtypes.quat().fromAngleAxis(-pi/2,(0,0,1)),
}
self.last_optical_images = {}
for fn in self.viewdirs.keys():
self.last_optical_images[fn] = None
self.debug = debug
if self.debug:
self.responses_fd = open('01_retina.txt','wb')
self.rweights = precomputed.receptor_weight_matrix_64.astype(numpy.float32)
emd_edges = precomputed.edges
n_receptors = self.rweights.shape[0]
self.emd_sim = EMDSim.EMDSim(
emd_hp_ba = emd_hp_ba, # highpass tau (can be None)
emd_lp_ba = emd_lp_ba, # delay filter of EMD
earlyvis_ba = earlyvis_ba, # if None, set to Howard, 1984
early_contrast_saturation_params=early_contrast_saturation_params,
subtraction_imbalance = subtraction_imbalance, # 1.0 for perfect subtraction
compute_typecode = numpy.float32,
hz=hz,
n_receptors=n_receptors,
emd_edges=emd_edges,
do_luminance_adaptation=do_luminance_adaptation,
)
self.last_environment_map = None
import fsee
import fsee.Observer
# these numbers specify body position and wing position (currently only body is used)
nums = """-63.944819191780439 0.95360065664418736 -1.5677184054558335
0.97592747002588576 0.00013135157421694402 0.21809381132492375
0.00080340363848455045 -64.511912229419792 2.4958009855663787
-0.91416847614060104 0.83348670845615658 -0.079986858311146714
0.50516881885673259 0.2090609331733887 -64.507322639730816
-0.59116643445214989 -0.91599543302231212 0.20749684980883201
0.50542162231080134 -0.079722030902706131 0.83374962597225588"""
nums = map(float, nums.split())
pos_vec3, ori_quat = cgtypes.vec3(nums[0:3]), cgtypes.quat(nums[3:7])
M = cgtypes.mat4().identity().translate(pos_vec3)
M = M * ori_quat.toMat4()
# grr, I hate cgtypes to numpy conversions!
M = np.array(
(
M[0, 0],
M[1, 0],
M[2, 0],
M[3, 0],
M[0, 1],
M[1, 1],
M[2, 1],
M[3, 1],
M[0, 2],
M[1, 2],
def rotate(q, p):
if isinstance(p, cgtypes.vec3):
p = cgtypes.quat((0, p[0], p[1], p[2]))
r = q * p * q.inverse()
result = cgtypes.vec3((r.x, r.y, r.z))
return result
def doit(
filename=None,
obj_only=None,
do_ransac=False,
show=False,
):
# get original 3D points -------------------------------
ca = core_analysis.get_global_CachingAnalyzer()
obj_ids, use_obj_ids, is_mat_file, data_file, extra = ca.initial_file_load(
filename)
if obj_only is not None:
use_obj_ids = np.array(obj_only)
x = []
y = []
z = []
for obj_id in use_obj_ids:
obs_rows = ca.load_dynamics_free_MLE_position(obj_id, data_file)
goodcond = ~np.isnan(obs_rows['x'])
good_rows = obs_rows[goodcond]
x.append(good_rows['x'])
y.append(good_rows['y'])
z.append(good_rows['z'])
x = np.concatenate(x)
y = np.concatenate(y)
z = np.concatenate(z)
recon = Reconstructor(cal_source=data_file)
extra['kresults'].close() # close file
data = np.empty((len(x), 3), dtype=np.float)
data[:, 0] = x
data[:, 1] = y
data[:, 2] = z
# calculate plane-of-best fit ------------
helper = PlaneModelHelper()
if not do_ransac:
plane_params = helper.fit(data)
else:
# do RANSAC
"""
n: the minimum number of data values required to fit the model
k: the maximum number of iterations allowed in the algorithm
t: a threshold value for determining when a data point fits a model
d: the number of close data values required to assert that a model fits well to data
"""
n = 20
k = 100
t = np.mean([np.std(x), np.std(y), np.std(z)])
d = 100
plane_params = ransac.ransac(data, helper, n, k, t, d, debug=False)
# Calculate rotation matrix from plane-of-best-fit to z==0 --------
orig_normal = norm(plane_params[:3])
new_normal = np.array([0, 0, 1], dtype=np.float)
rot_axis = norm(np.cross(orig_normal, new_normal))
cos_angle = np.dot(orig_normal, new_normal)
angle = np.arccos(cos_angle)
q = cgtypes.quat().fromAngleAxis(angle, rot_axis)
m = q.toMat3()
R = cgmat2np(m)
# Calculate aligned data without translation -----------------
s = 1.0
t = np.array([0, 0, 0], dtype=np.float)
aligned_data = align.align_points(s, R, t, data.T).T
# Calculate aligned data so that mean point is origin -----------------
t = -np.mean(aligned_data[:, :3], axis=0)
aligned_data = align.align_points(s, R, t, data.T).T
M = align.build_xform(s, R, t)
r2 = recon.get_aligned_copy(M)
wateri = water.WaterInterface(
refractive_index=DEFAULT_WATER_REFRACTIVE_INDEX,
water_roots_eps=WATER_ROOTS_EPS)
r2.add_water(wateri)
dst = os.path.splitext(filename)[0] + '-water-aligned.xml'
r2.save_to_xml_filename(dst)
print 'saved to', dst
if show:
import matplotlib.pyplot as plt
from pymvg.plot_utils import plot_system
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure()
ax1 = fig.add_subplot(221)
ax1.plot(data[:, 0], data[:, 1], 'b.')
ax1.set_xlabel('x')
ax1.set_ylabel('y')
ax2 = fig.add_subplot(222)
ax2.plot(data[:, 0], data[:, 2], 'b.')
ax2.set_xlabel('x')
ax2.set_ylabel('z')
ax3 = fig.add_subplot(223)
ax3.plot(aligned_data[:, 0], aligned_data[:, 1], 'b.')
ax3.set_xlabel('x')
ax3.set_ylabel('y')
ax4 = fig.add_subplot(224)
ax4.plot(aligned_data[:, 0], aligned_data[:, 2], 'b.')
ax4.set_xlabel('x')
ax4.set_ylabel('z')
fig2 = plt.figure('cameras')
ax = fig2.add_subplot(111, projection='3d')
system = r2.convert_to_pymvg(ignore_water=True)
plot_system(ax, system)
x = np.linspace(-0.1, 0.1, 10)
y = np.linspace(-0.1, 0.1, 10)
X, Y = np.meshgrid(x, y)
Z = np.zeros_like(X)
ax.plot(X.ravel(), Y.ravel(), Z.ravel(), 'b.')
ax.set_title('aligned camera positions')
plt.show()
def __init__(self, proc):
_core.QuatSlot.__init__(self, quat(), 2) # 2 = NO_INPUT_CONNECTIONS
self._proc = proc
