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 __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
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)
# 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()