def check_points_in_range(current_rob_pos, other_rob_pos):
# calculate bottom right corner of the bounding rectangle
rx, ry, rth = current_rob_pos
rth = (rth / 100) + (pi / 2)
zx = round(rx - (m * sin(rth)), 3)
zy = round(ry + (m * cos(rth)), 3)
# calculating the transformation matrix of the new frame at that vertex
alpha, beta, gamma = 0, 0, (-(pi / 2 - rth))
origin, xaxis, yaxis, zaxis = [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]
Rx = transformations.rotation_matrix(alpha, xaxis)
Ry = transformations.rotation_matrix(beta, yaxis)
Rz = transformations.rotation_matrix(gamma, zaxis)
T = transformations.translation_matrix([zx, zy, 0])
R = transformations.concatenate_matrices(Rx, Ry, Rz)
Z = transformations.shear_matrix(beta, xaxis, origin, zaxis)
S = transformations.scale_matrix(1, origin)
M = transformations.concatenate_matrices(T, R, Z, S)
M = np.linalg.inv(M)
# calculating the new point in that frame
other_x = other_rob_pos[0]
other_y = other_rob_pos[1]
other_z = 0
other_h = 1
other_point_homogenous = np.array([other_x, other_y, other_z, other_h])
new_point = np.dot(M, other_point_homogenous)
new_point = np.round(new_point, 3)
# checking that the point lies within the boundaries
px, py, pz, ph = new_point
if px <= (2 * m) and px >= 0 and py <= m and py >= 0:
return 1
else:
return 0
def _make_plane_matrix(self):
r = tr.rotation_matrix(self.plane_alpha, (0, 0, 1))
s = tr.scale_matrix(1)
t = tr.translation_matrix((-1.25, .7, .05))
self.m = np.dot(np.dot(t, s), r)
self.im = la.inv(self.m)
self.im[3] = [0, 0, 0, 1]
def _x2transform(self, x):
trans, eulxyz, scale = self._unpack(x)
origin = [0, 0, 0]
T = tf.concatenate_matrices(tf.translation_matrix(trans),
tf.euler_matrix(eulxyz[0], eulxyz[1],
eulxyz[2], 'sxyz'),
tf.scale_matrix(scale, origin))
return T
def _make_plane_matrix( self ) : r = tr.rotation_matrix( self.plane_alpha , (0,0,1) ) s = tr.scale_matrix( 1 ) t = tr.translation_matrix( (-1.25,.7,.05) ) self.m = np.dot( np.dot( t , s ) , r ) self.im = la.inv( self.m ) self.im[3] = [ 0 , 0 , 0 , 1 ]
def compute_ref_accuracy(fid_path, original_corrs_path,
geo_tform):
#Load fiducial .ply
fid = open(fid_path, 'r')
fid_points = np.genfromtxt(fid, dtype=float, delimiter=' ',
skip_header=9)
fid.close()
#Load original corrs .ply
fid = open(original_corrs_path, 'r')
original_corrs = np.genfromtxt(fid, dtype=float,
delimiter=' ', skip_header=9)
fid.close()
#Load transformation
#************GEO**************"
Tfis = open(geo_tform, 'r')
lines = []
lines = Tfis.readlines()
scale_geo = float(lines[0])
Ss_geo = tf.scale_matrix(scale_geo)
quat_line = lines[1].split(" ")
quat_geo = np.array([float(quat_line[3]), float(quat_line[0]),
float(quat_line[1]), float(quat_line[2])])
Rs_geo = tf.quaternion_matrix(quat_geo)
trans_line = lines[2].split(" ")
trans_geo = np.array([float(trans_line[0]), float(trans_line[1]),
float(trans_line[2])])
Tfis.close()
Hs_geo = Rs_geo.copy()
Hs_geo[:3, 3] = trans_geo[:3]
Hs_geo = Ss_geo.dot(Hs_geo)
LOG.debug("\n******Geo***** \n Scale: \n%s \nR:\n%s \nT:\n%s \nH:\n%s",
Ss_geo, Rs_geo, trans_geo, Hs_geo)
#Compute the "reference error"
#i.e. fiducial points - geo registered correspondances
npoints, c = fid_points.shape
if npoints != 30:
LOG.warn("Number of fiducial point is NOT 30")
if c != 3:
LOG.error("Fiducial points has the wrong number of dimensions")
# import code; code.interact(local=locals())
fid_points_hom = np.hstack((fid_points, np.ones([npoints, 1]))).T
original_corrs_hom = np.hstack((original_corrs, np.ones([npoints, 1]))).T
geo_corrs_hom = Hs_geo.dot(original_corrs_hom)
geo_ref_diff = geo_corrs_hom - fid_points_hom
# import pdb; pdb.set_trace()
delta_z = np.sqrt(geo_ref_diff[2, :] * geo_ref_diff[2, :])
delta_r = np.sqrt(geo_ref_diff[0, :] * geo_ref_diff[0, :] +
geo_ref_diff[1, :] * geo_ref_diff[1, :])
return delta_z, delta_r
def _draw(self):
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
glClearColor(0.0, 0.0, 0.0, 1.0)
view_mat = translation_matrix((0, 0, -self.dist))
view_mat = view_mat.dot(self.ball.matrix())
view_mat = view_mat.dot(scale_matrix(self.zoom))
self.VMatrix = view_mat
self.draw_hook()
OpenGL.GLUT.glutSwapBuffers()
def dimensionsOLD(gltf, node, parentMatrix=transformations.scale_matrix(1)):
# pprint(node)
T = node.translation or [0, 0, 0]
R = node.rotation or [0, 0, 0, 1]
S = node.scale or [1, 1, 1]
# thisNodeMatrix = composeMatrixFromTRS(T,R,S)
R = transformations.euler_from_quaternion(R, 'szxy')
thisNodeMatrix = node.matrix or transformations.compose_matrix(
S, None, R, T)
thisNodeMatrix = np.array(thisNodeMatrix)
thisNodeMatrix.shape = (4, 4)
newMatrix = np.dot(parentMatrix, thisNodeMatrix)
# newMatrix = np.dot(thisNodeMatrix,parentMatrix)
if node.mesh != None:
accessor = gltf.accessors[gltf.meshes[
node.mesh].primitives[0].attributes.POSITION]
testMin = accessor.min
testMax = accessor.max
print(testMin)
testMin = np.matmul(newMatrix, accessor.min + [1])
print("After multply:", testMin)
print(testMax)
testMax = np.matmul(newMatrix, accessor.max + [1])
print("After multply:", testMax)
for test in [testMin, testMax]:
myMin[0] = test[0] if test[0] < myMin[0] else myMin[0]
myMin[1] = test[1] if test[1] < myMin[1] else myMin[1]
myMin[2] = test[2] if test[2] < myMin[2] else myMin[2]
myMax[0] = test[0] if test[0] > myMax[0] else myMax[0]
myMax[1] = test[1] if test[1] > myMax[1] else myMax[1]
myMax[2] = test[2] if test[2] > myMax[2] else myMax[2]
# myMin[0] = testMin[0] if testMin[0] < myMin[0] else myMin[0]
# myMin[1] = testMin[1] if testMin[1] < myMin[1] else myMin[1]
# myMin[2] = testMin[2] if testMin[2] < myMin[2] else myMin[2]
# myMax[0] = testMax[0] if testMax[0] > myMax[0] else myMax[0]
# myMax[1] = testMax[1] if testMax[1] > myMax[1] else myMax[1]
# myMax[2] = testMax[2] if testMax[2] > myMax[2] else myMax[2]
if node.children == None or len(node.children) == 0:
# pprint(thisNodeMatrix)
# print(node.mesh)
return
for child in node.children:
dimensionsOLD(gltf, gltf.nodes[child], newMatrix)
def calc_error(args, debug=False):
# angle_x, angle_y, o_x, o_y, o_z = args
angle_x, angle_y = args
x = np.array([1, 0, 0, 1])
R_x = transformations.rotation_matrix(angle_x, [1, 0, 0], eye_centre)
R_y = transformations.rotation_matrix(angle_y, [0, 1, 0], eye_centre)
S = transformations.scale_matrix(6)
T = transformations.translation_matrix(eye_centre)
# T = transformations.translation_matrix(eye_centre + np.array([o_x*100, o_y*100, o_z*100]))
T2 = transformations.translation_matrix([0,0,-12])
if debug:
trans = transformations.concatenate_matrices(*reversed([T, T2, R_x, R_y]))
pt = dehomo(trans.dot([0,0,-50,1]))
cv2.line(img,
tuple(dehomo(cam_mat.dot(coord_swap.dot(true_iris_centre_3d))).astype(int)),
tuple(dehomo(cam_mat.dot(coord_swap.dot(pt))).astype(int)),
(255, 255, 0))
est_pts = []
for t in np.linspace(0, np.pi*2, accuracy):
R = transformations.rotation_matrix(t, [0, 0, 1])
trans = transformations.concatenate_matrices(*reversed([R, S, T, T2, R_x, R_y]))
threeD_pt = coord_swap.dot(dehomo(trans.dot(x)))
pt = cam_mat.dot(threeD_pt)
if point_in_poly(dehomo(pt).astype(int), data["ldmks_lids_2d"]):
est_pts.append(dehomo(pt).astype(int))
if debug: cv2.circle(img, tuple(dehomo(pt).astype(int)), 1, (255, 0, 0), -1)
try:
D = cdist(est_pts, visible_pts, 'euclidean')
H1 = np.max(np.min(D, axis=1))
H2 = np.max(np.min(D, axis=0))
return (H1 + H2) / 2.0
except ValueError:
return 20
def __init__(self, geo_tform):
#Load transformation
Tfis = open(geo_tform, 'r')
lines = []
lines = Tfis.readlines()
self.scale_geo = float(lines[0])
self.Ss_geo = tf.scale_matrix(self.scale_geo)
quat_line = lines[1].split(" ")
quat_geo = np.array([float(quat_line[3]), float(quat_line[0]),
float(quat_line[1]), float(quat_line[2])])
self.Rs_geo = tf.quaternion_matrix(quat_geo)
trans_line = lines[2].split(" ")
self.trans_geo = np.array([float(trans_line[0]), float(trans_line[1]),
float(trans_line[2])])
Tfis.close()
self.Hs_geo = self.Rs_geo.copy()
self.Hs_geo[:3, 3] = self.trans_geo[:3]
self.Hs_geo = self.Ss_geo.dot(self.Hs_geo)
print "Loaded GeoTransformation: "
print self.Hs_geo
def update3DPoints(self, newPoints):
center = sum([np.array(roundPoint(x))
for x in newPoints]) / 2 - self.minor
diff = newPoints[0] - newPoints[1]
radius = ((diff[1]**2 + diff[0]**2)**(0.5)) / 2
scaled = np.concatenate((self.primitivePoints,
np.ones(
(1, np.shape(self.primitivePoints)[1]))),
axis=0)
theta = np.arctan2(diff[1], diff[0])
zaxis = [0, 1, 0]
rotation = trans.rotation_matrix(theta, zaxis)
scale = trans.scale_matrix(radius)
translation = trans.translation_matrix([center[0], 0, -center[1]])
complete = trans.concatenate_matrices(translation, rotation, scale)
affineTrans = np.matmul(complete, scaled)
if (self.objectPoints.any()):
self.objectPoints = np.concatenate(
(self.objectPoints, np.transpose(affineTrans)), axis=0)
else:
self.objectPoints = np.transpose(affineTrans)
def scale_data(x_data, y_data):
'''
There is a 25% chance that scaling operation will be performed.
:param x_data:
:param y_data:
:return:
'''
for curr_eg in range(x_data.shape[0]):
chance = random.uniform(0, 1)
if chance < 0.25:
origin = list(np.array(config['patch_size'], copy=True) / 2)
factor = random.uniform(0.8, 1.2)
S = scale_matrix(factor, origin=origin)
# transform the x_data
for each_mod in range(0, 4):
x_data[curr_eg, each_mod] = affine_transform(x_data[curr_eg, each_mod], S, order=1, prefilter=False)
# transform the y_data
for each_label in range(0, 3):
y_data[curr_eg, each_label] = affine_transform(y_data[curr_eg, each_label], S, order=1, prefilter=False)
return x_data, y_data
def __init__(self, T):
if ( type(T) == str):
Tfile = T
#Load transformation
Tfis = open(Tfile, 'r')
lines = []
lines = Tfis.readlines()
self.scale = float(lines[0])
self.Ss = tf.scale_matrix(self.scale)
quat_line = lines[1].split(" ")
self.quat = tf.unit_vector(np.array([float(quat_line[3]),
float(quat_line[0]),
float(quat_line[1]),
float(quat_line[2])]))
self.Hs = tf.quaternion_matrix(self.quat)
trans_line = lines[2].split(" ")
self.Ts = np.array([float(trans_line[0]),
float(trans_line[1]),
float(trans_line[2])])
Tfis.close()
self.Rs = self.Hs.copy()[:3, :3]
self.Hs[:3, 3] = self.Ts[:3]
self.Hs = self.Ss.dot(self.Hs) # to add again
elif (type(T) == np.ndarray):
self.Hs = T
scale, shear, angles, trans, persp = tf.decompose_matrix(T)
self.quat = tf.quaternion_from_euler(angles[0], angles[1], angles[2])
self.Rs = tf.quaternion_matrix(self.quat)
self.scale =scale[0]
self.Ts = trans / self.scale
print "Loaded Ground Truth Transformation: "
print self.Hs
def get_recenter_affine(src_list, dst_list):
if len(src_list) < 3:
T = transformations.translation_matrix([0.0, 0.0, 0.0])
R = np.identity(4)
S = transformations.scale_matrix(1.0)
A = transformations.concatenate_matrices(T, R, S)
else:
src = [[], [], [], []] # current camera locations
dst = [[], [], [], []] # original camera locations
for i in range(len(src_list)):
src_ned = src_list[i]
src[0].append(src_ned[0])
src[1].append(src_ned[1])
src[2].append(src_ned[2])
src[3].append(1.0)
dst_ned = dst_list[i]
dst[0].append(dst_ned[0])
dst[1].append(dst_ned[1])
dst[2].append(dst_ned[2])
dst[3].append(1.0)
print "%s <-- %s" % (dst_ned, src_ned)
A = transformations.superimposition_matrix(src, dst, scale=True)
print "A:\n", A
return A
def draw(self):
#pre draw stuff and preparation
t1 = time.time() * 1000
if GlobalSettings.read('linemode') == GL_LINE:
skirt_off = 1.0
else:
skirt_off = 0.0
#apply local transformations
selfrot = np.ascontiguousarray(
np.transpose(
np.append(
np.append(self.orientation._get_transform(),
[[0], [0], [0]], 1), [[0, 0, 0, 1]], 0)))
S = scale_matrix(self.scale)
R = selfrot
T = translation_matrix(self.position)
self.model = np.dot(T, np.dot(R, S))
#calculate mirror matrix
#normalize camera position in object space
selfrot = np.ascontiguousarray(
np.transpose(
np.append(
np.append(self.orientation._get_transform(),
[[0], [0], [0]], 1), [[0, 0, 0, 1]], 0)))
Si = scale_matrix(float(1 / self.scale))
Ri = np.transpose(selfrot)
Ti = translation_matrix(
[-self.position[0], -self.position[1], -self.position[2]])
Minv = np.dot(Si, np.dot(Ri, Ti))
self.campostr = np.ravel(
np.dot(Minv, np.append(self.ActiveCamera.position, 1)))[0:3]
self.cam_distance = np.linalg.norm(self.campostr)
GlobalSignals.set('cam distance', self.cam_distance)
#mcamera = np.dot(flip,np.dot(mirror,np.linalg.inv(self.ActiveCamera.view)))
#mview = self.ActiveCamera.view
if (self.cam_distance < 1.3) and (self.PlanetDescriptor.sea_level >
0) and (self.state == 0):
self.draw_fbo_refraction()
self.draw_fbo_reflection(self.ActiveCamera.view)
#test draw the sky if we are inside
if (self.cam_distance <= 100):
glBlendFunc(GL_SRC_ALPHA, GL_ZERO)
#glDisable(GL_CULL_FACE)
glCullFace(GL_FRONT)
self.sky.draw_inner_sky(self.ActiveCamera.proj,
self.ActiveCamera.view, self.model)
#glEnable(GL_CULL_FACE)
glCullFace(GL_BACK)
#draw the planet
glBlendFunc(GL_ONE, GL_ZERO)
MaterialManager.use_material('planetmat1')
#glUseProgram(self.program)
glEnableVertexAttribArray(self.main_attribs['position'])
glEnableVertexAttribArray(self.main_attribs['texcoord'])
glEnableVertexAttribArray(self.main_attribs['nodescale'])
glEnableVertexAttribArray(self.main_attribs['texcoord2'])
glUniformMatrix4fv(self.main_unifs['u_model'], 1, GL_TRUE, self.model)
glUniformMatrix4fv(self.main_unifs['u_view'], 1, GL_FALSE,
self.ActiveCamera.view)
glUniformMatrix4fv(
self.main_unifs['u_normal'], 1, GL_FALSE,
np.transpose(
np.linalg.inv(
np.dot(np.transpose(self.model), self.ActiveCamera.view))))
glUniformMatrix4fv(self.main_unifs['u_proj'], 1, GL_FALSE,
self.ActiveCamera.proj)
glUniform3fv(self.main_unifs['u_lposition3'], 1, self.light.get_pos())
glUniform3fv(self.main_unifs['u_lintensity3'], 1,
self.light.get_intensity())
glUniform1f(self.main_unifs['planet_radius'],
self.PlanetDescriptor.radius)
glUniform1f(self.main_unifs['planet_height'],
self.PlanetDescriptor.height)
glUniform1f(self.main_unifs['sea_level'],
self.PlanetDescriptor.sea_level)
glUniform1f(self.main_unifs['camdist'], self.cam_distance)
glUniform1f(self.main_unifs['skirt_off'], skirt_off)
glUniform1i(self.main_unifs['state'], self.state)
glUniform1f(self.main_unifs['verse'], 1.0)
glEnable(GL_TEXTURE_2D)
glActiveTexture(GL_TEXTURE0)
glBindTexture(GL_TEXTURE_2D, self.PlanetDescriptor.mixmap)
glUniform1i(glGetUniformLocation(self.program, 'surfacecolor'), 0)
#bind the texture (from texture atlas array) for heightmap / normals
glActiveTexture(GL_TEXTURE1)
glBindTexture(GL_TEXTURE_2D_ARRAY, self.TexAtlas.get_tex_id())
glUniform1i(glGetUniformLocation(self.program, 'heightmap'), 1)
glActiveTexture(GL_TEXTURE2)
glBindTexture(GL_TEXTURE_2D_ARRAY,
self.PlanetDescriptor.surface_textures.get_tex_id())
glUniform1i(glGetUniformLocation(self.program, 'surface'), 2)
#draw pass of the 6 VBOs
for index in range(0, 6):
#bind the VBO
act = self.activeVBO[index]
self.vertexVBO[act][index].bind()
self.indexVBO[act][index].bind()
#draw
glVertexAttribPointer(self.attrib_position, 3, GL_FLOAT, GL_FALSE,
36, self.vertexVBO[act][index])
glVertexAttribPointer(self.attrib_texcoord, 3, GL_FLOAT, GL_FALSE,
36, self.vertexVBO[act][index] + (3 * 4))
glVertexAttribPointer(self.attrib_scale, 1, GL_FLOAT, GL_FALSE, 36,
self.vertexVBO[act][index] + (6 * 4))
glVertexAttribPointer(self.attrib_texcoord2, 2, GL_FLOAT, GL_FALSE,
36, self.vertexVBO[act][index] + (7 * 4))
nb_elements = self.indexVBO[act][index].shape[0]
glDrawElements(GL_TRIANGLES, nb_elements, GL_UNSIGNED_INT,
self.indexVBO[act][index])
# disable/unbind the arrays
self.vertexVBO[act][index].unbind()
self.indexVBO[act][index].unbind()
glDisableVertexAttribArray(self.main_attribs['position'])
glDisableVertexAttribArray(self.main_attribs['texcoord'])
glDisableVertexAttribArray(self.main_attribs['nodescale'])
glDisableVertexAttribArray(self.main_attribs['texcoord2'])
# draw the sea
#glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
if (self.cam_distance <
1.05) and (self.PlanetDescriptor.sea_level > 0) and (
GlobalSettings.read('nosea')) and (self.state == 0):
glUseProgram(self.program_sea)
#enable attributes arrays
glEnableVertexAttribArray(self.sea_attribs['position'])
glEnableVertexAttribArray(self.sea_attribs['texcoord'])
glEnableVertexAttribArray(self.sea_attribs['nodescale'])
glEnableVertexAttribArray(self.sea_attribs['texcoord2'])
glUniformMatrix4fv(self.sea_unifs['model_m'], 1, GL_TRUE,
self.model)
glUniformMatrix4fv(self.sea_unifs['view_m'], 1, GL_FALSE,
self.ActiveCamera.view)
glUniformMatrix4fv(
self.sea_unifs['normal_m'], 1, GL_FALSE,
np.transpose(
np.linalg.inv(
np.dot(np.transpose(self.model),
self.ActiveCamera.view))))
glUniformMatrix4fv(self.sea_unifs['proj_m'], 1, GL_FALSE,
self.ActiveCamera.proj)
glUniform3fv(self.sea_unifs['u_lposition3'], 1,
self.light.get_pos())
glUniform3fv(self.sea_unifs['u_lintensity3'], 1,
self.light.get_intensity())
glUniform1f(self.sea_unifs['planetradius'],
self.PlanetDescriptor.radius)
glUniform1f(self.sea_unifs['planetheight'],
self.PlanetDescriptor.height)
glUniform1f(self.sea_unifs['sealevel'],
self.PlanetDescriptor.sea_level)
glUniform1f(self.sea_unifs['time'], time.clock())
glUniform1f(self.sea_unifs['camdist'], self.cam_distance)
glUniform1f(self.sea_unifs['skirt_off'], skirt_off)
glEnable(GL_TEXTURE_2D)
glActiveTexture(GL_TEXTURE0)
glBindTexture(GL_TEXTURE_2D_ARRAY, self.PlanetDescriptor.wavetext)
glUniform1i(glGetUniformLocation(self.program_sea, 'wavetext'), 0)
#bind the texture (from texture atlas array) for heightmap / normals
glActiveTexture(GL_TEXTURE1)
glBindTexture(GL_TEXTURE_2D_ARRAY, self.TexAtlas.get_tex_id())
glUniform1i(glGetUniformLocation(self.program_sea, 'heightmap'), 1)
glActiveTexture(GL_TEXTURE2)
glBindTexture(GL_TEXTURE_2D, self.helpfbo.rtt)
glUniform1i(
glGetUniformLocation(self.program_sea, 'refractiontexture'), 2)
glActiveTexture(GL_TEXTURE3)
glBindTexture(GL_TEXTURE_2D, self.helpfbo2.rtt)
glUniform1i(
glGetUniformLocation(self.program_sea, 'reflectiontexture'), 3)
for index in range(0, 6):
#bind the VBO
act = self.activeVBO[index]
self.vertexVBO[act][index].bind()
self.indexVBO[act][index].bind()
#draw
glVertexAttribPointer(self.attrib_position, 3, GL_FLOAT,
GL_FALSE, 36, self.vertexVBO[act][index])
glVertexAttribPointer(self.attrib_texcoord, 3, GL_FLOAT,
GL_FALSE, 36,
self.vertexVBO[act][index] + (3 * 4))
glVertexAttribPointer(self.attrib_scale, 1, GL_FLOAT, GL_FALSE,
36, self.vertexVBO[act][index] + (6 * 4))
glVertexAttribPointer(self.attrib_texcoord2, 2, GL_FLOAT,
GL_FALSE, 36,
self.vertexVBO[act][index] + (7 * 4))
nb_elements = (self.indexVBO[act][index].shape[0])
glDrawElements(GL_TRIANGLES, nb_elements, GL_UNSIGNED_INT,
self.indexVBO[act][index])
# disable/unbind the arrays
self.vertexVBO[act][index].unbind()
self.indexVBO[act][index].unbind()
glDisableVertexAttribArray(self.sea_attribs['position'])
glDisableVertexAttribArray(self.sea_attribs['texcoord'])
glDisableVertexAttribArray(self.sea_attribs['nodescale'])
glDisableVertexAttribArray(self.sea_attribs['texcoord2'])
#test draw the sky if we are outside
if (self.cam_distance >= 1.1):
glBlendFunc(GL_SRC_ALPHA, GL_ONE)
glCullFace(GL_BACK)
self.sky.draw_inner_sky(self.ActiveCamera.proj,
self.ActiveCamera.view, self.model)
glUseProgram(0)
#VBO updating / maintenance
for index in range(0, 6):
#check if current face has a VBO to complete update
if self.VBO_need_update[index] <> 3:
RequestManager.add_request(self.VBOupdate, [index])
#swap active VBO if no pending updates
if self.VBO_need_update[index] <> (1 - act):
self.activeVBO[index] = 1 - act
#debug code
t2 = time.time() * 1000 - t1
GlobalSignals.set('draw ms', t2)
GlobalSignals.set('tex slot free', self.TexAtlas.free_count)
view_camera_index = -1
while True:
if frame_index == total_frame:
frame_index = 0
frame = np.zeros([frame_size, frame_size, 3])
if view_camera_index >= 0:
view_matrix = None
projection_matrix = specific_camera_config[
view_camera_index].projection
else:
view_matrix = o_view_matrix
projection_matrix = o_projection_matrix
grid_vertices_project = grid_vertices @ (np.eye(3) if view_matrix is None
else rorate_x_90[:3, :3].T)
grid_vertices_project = grid_vertices_project @ transformations.scale_matrix(
650)[:3, :3].T
grid_vertices_project = projection_to_2d_plane(
grid_vertices_project, projection_matrix, view_matrix,
int(frame_size / 2)).reshape(-1, 4)
# draw line
for index, line in enumerate(grid_vertices_project):
cv2.line(frame, (line[0], line[1]), (line[2], line[3]),
grid_color[index].tolist())
# draw camera
for camera_index, conf in enumerate(specific_camera_config):
if view_camera_index == camera_index:
continue
m_rt = transformations.identity_matrix()
r = np.array(conf.R, dtype=np.float32).T
def scale(self, factor: Vetor2D):
cx = self.centroid.x
cy = self.centroid.y
t_matrix = (translate_matrix(-cx, -cy) @ scale_matrix(
factor.x, factor.y) @ translate_matrix(cx, cy))
self.transform(t_matrix)
def compute_error(trial, descriptor_type, niter, percentile=99):
src_scene_root = "/Users/isa/Experiments/reg3d_eval/downtown_dan/trial_" +str(trial);
src_features_dir = "/Users/isa/Experiments/reg3d_eval/downtown_dan/trial_" +str(trial)+ "/" + descriptor_type + "_30"
#read "geo-transformatiom"
#************GEO**************"
Tfile = "/data/lidar_providence/downtown_offset-1-financial-Hs.txt"
Tfis = open(Tfile, 'r')
lines=[];
lines = Tfis.readlines();
scale_geo = float(lines[0])
Ss_geo = tf.scale_matrix(scale_geo);
quat_line = lines[1].split(" ");
quat_geo = np.array([float(quat_line[3]), float(quat_line[0]), float(quat_line[1]), float(quat_line[2])])
Rs_geo = tf.quaternion_matrix(quat_geo);
trans_line = lines[2].split(" ");
trans_geo = np.array([float(trans_line[0]), float(trans_line[1]), float(trans_line[2])]);
Tfis.close();
Hs_geo = Rs_geo.copy();
Hs_geo[:3, 3] = trans_geo[:3]
Hs_geo = Ss_geo.dot(Hs_geo)
LOG.debug( "\n************Geo************** \n Scale: \n%s \nR:\n%s \nT:\n%s \nH:\n%s", Ss_geo, Rs_geo, trans_geo, Hs_geo)
#************Hs**************#
#read source to target "Ground Truth" Transformation
Tfile = src_scene_root + "/Hs.txt";
Tfis = open(Tfile, 'r')
lines=[];
lines = Tfis.readlines();
scale = float(lines[0])
Ss = tf.scale_matrix(scale);
quat_line = lines[1].split(" ");
quat = tf.unit_vector(np.array([float(quat_line[3]), float(quat_line[0]), float(quat_line[1]), float(quat_line[2])]))
Hs = tf.quaternion_matrix(quat);
trans_line = lines[2].split(" ");
Ts = np.array([float(trans_line[0]), float(trans_line[1]), float(trans_line[2])]);
Tfis.close();
Rs = Hs.copy()[:3,:3];
Hs[:3, 3] = Ts[:3]
Hs=Ss.dot(Hs)
Rs = Rs;
Ts = Ts;
LOG.debug( "\n************Hs************** \n R:\n%s \nT:\n%s \nH:\n%s", Rs, Ts, Hs)
#************Hs IA**************
#read source to target "Initial Alignment" Transformation
Tfile = src_features_dir + "/ia_transformation_" + str(percentile) + "_" + str(niter) + ".txt";
Tfis = open(Tfile, 'r')
Hs_ia = np.genfromtxt(Tfis, skip_header=1, usecols={0,1,2,3} );
Tfis.close()
Tfis = open(Tfile, 'r')
Ss_ia=np.genfromtxt(Tfis, skip_footer=4, usecols={0} );
Tfis.close()
Rs_ia = Hs_ia[:3,:3]*(1.0/Ss_ia)
Ts_ia = Hs_ia[:3,3]*(1.0/Ss_ia)
LOG.debug( "\n************Hs IA************** \n R:\n%s \nT:\n%s \nH:\n%s", Rs_ia, Ts_ia, Hs_ia)
#Initial Aligment errors
#Rotation error - half angle between the normalized quaterions
quat_ia = tf.unit_vector(tf.quaternion_from_matrix(Rs_ia));
Rs_error_ia_norm = math.acos(abs(np.dot(quat_ia, quat)));
#Translation error
# x = Rs_ia*x_ia + Ts_ia = Rs_ia(x_ia + np.dot(Rs_ia.T(), Ts_ia)
# np.dot(Rs_ia.T(), Ts_ia) correspond to trans on ia coordinate system
Ts_error_ia = (Rs_ia.T).dot(Ts_ia) - (Rs.T).dot(Ts)
Ts_error_ia_norm = scale_geo*scale*LA.norm(Ts_error_ia)
LOG.debug( "Error (R,T) %s , %s ", Rs_error_ia_norm , Ts_error_ia_norm )
#read source to target "Initial Alignment" Transformation
#************Hs ICP**************
Tfile = src_features_dir + "/icp_transformation_" + str(percentile) + "_" + str(niter) + ".txt";
Tfis = open(Tfile, 'r')
Hs_icp = np.genfromtxt(Tfis, usecols={0,1,2,3});
Tfis.close()
Hs_icp = Hs_icp.dot(Hs_ia)
Rs_icp = Hs_icp[:3,:3]*(1.0/Ss_ia)
Ts_icp = Hs_icp[:3,3]*(1.0/Ss_ia)
LOG.debug( "\n************Hs ICP************** \n R:\n%s \nT:\n%s \nH:\n%s", Rs_icp, Ts_icp, Hs_icp)
#ICP errors
#Rotation error - half angle between the normalized quaterions
quat_icp = tf.unit_vector(tf.quaternion_from_matrix(Rs_icp));
Rs_error_icp_norm = math.acos(abs(np.dot(quat_icp, quat)));
#Translation error
# x = Rs_ia*x_ia + Ts_ia = Rs_ia(x_ia + np.dot(Rs_ia.T(), Ts_ia)
# np.dot(Rs_ia.T(), Ts_ia) correspond to trans on ia coordinate system
Ts_error_icp = (Rs_icp.T).dot(Ts_icp) - (Rs.T).dot(Ts)
Ts_error_icp_norm = scale_geo*scale*LA.norm(Ts_error_icp)
LOG.debug( "Error (R,T) %s , %s ", Rs_error_icp_norm , Ts_error_icp_norm )
#read source to target "Initial Alignment" Transformation
#************Hs ICP Nprmals**************
Tfile = src_features_dir + "/icp_transformation_" + str(percentile) + "_" + str(niter) + "_n.txt";
Tfis = open(Tfile, 'r')
Hs_icn = np.genfromtxt(Tfis, usecols={0,1,2,3});
Tfis.close()
Hs_icn = Hs_icn.dot(Hs_ia)
Rs_icn = Hs_icn[:3,:3]*(1.0/Ss_ia)
Ts_icn = Hs_icn[:3,3]*(1.0/Ss_ia)
LOG.debug( "\n************Hs ICP Normals************** \n R:\n%s \nT:\n%s \nH:\n%s", Rs_icn, Ts_icn, Hs_icn)
#ICP errors
#Rotation error - half angle between the normalized quaterions
quat_icn = tf.unit_vector(tf.quaternion_from_matrix(Rs_icn));
Rs_error_icn_norm = math.acos(abs(np.dot(quat_icn, quat)));
#Translation error
# x = Rs_ia*x_ia + Ts_ia = Rs_ia(x_ia + np.dot(Rs_ia.T(), Ts_ia)
# np.dot(Rs_ia.T(), Ts_ia) correspond to trans on ia coordinate system
Ts_error_icn = (Rs_icn.T).dot(Ts_icn) - (Rs.T).dot(Ts)
Ts_error_icn_norm = scale_geo*scale*LA.norm(Ts_error_icn)
LOG.debug( "Error (R,T) %s , %s ", Rs_error_icn_norm , Ts_error_icn_norm )
ICP_error = np.array([Rs_error_icp_norm, Ts_error_icp_norm])
ICN_error = np.array([Rs_error_icn_norm, Ts_error_icn_norm]);
# import code; code.interact(local=locals())
return ICP_error,ICN_error
def startShowcase(models, indices):
pygame.init()
pygame.display.set_mode((width, height), HWSURFACE|OPENGL|DOUBLEBUF)
program = init()
x_offsets = []
y_offsets = []
z_offsets = []
x_vels = []
y_vels = []
z_vels = []
# Fill initial value
for i in range(6):
x_offsets.append(0)
y_offsets.append(0)
z_offsets.append(0)
x_vels.append(0)
y_vels.append(0)
z_vels.append(0)
running = True
angle = 0
matrix = transformations.identity_matrix()
while running:
drawModels(program, models, indices, x_offsets, y_offsets, z_offsets, matrix)
events = pygame.event.get()
keys = pygame.key.get_pressed()
if keys[K_LEFT] or keys[K_RIGHT] or keys[K_UP] or keys[K_DOWN]:
dx, dy = (0, 0)
if keys[K_LEFT]:
dx = 0.1
elif keys[K_RIGHT]:
dx = -0.1
if keys[K_UP]:
dy = -0.1
elif keys[K_DOWN]:
dy = 0.1
trans = transformations.translation_matrix([dx, dy, 0])
matrix = numpy.matmul(trans, matrix)
if keys[K_w] or keys[K_a] or keys[K_s] or keys[K_d]:
ai, aj = (0, 0)
if keys[K_a]:
aj = -DEGREE
elif keys[K_d]:
aj = DEGREE
if keys[K_w]:
ai = DEGREE
elif keys[K_s]:
ai = -DEGREE
trans = transformations.euler_matrix(ai, aj, 0)
matrix = numpy.matmul(trans, matrix)
if keys[K_r] or keys[K_f]:
scale = 1
if keys[K_r]:
scale = 1.1
if keys[K_f]:
scale = 0.9
trans = transformations.scale_matrix(scale)
matrix = numpy.matmul(trans, matrix)
# Check if certain region is pressed with mouse
if pygame.mouse.get_pressed()[0]:
mouseX, mouseY = pygame.mouse.get_pos()
ai, aj = (0, 0)
if mouseX < 100:
aj = -DEGREE
elif mouseX > (width - 100):
aj = DEGREE
if mouseY < 100:
ai = DEGREE
elif mouseY > (height - 100):
ai = -DEGREE
trans = transformations.euler_matrix(ai, aj, 0)
matrix = numpy.matmul(trans, matrix)
# wait for exit
for event in events:
if event.type == KEYDOWN:
if event.key == K_ESCAPE:
pygame.quit()
running = False
elif event.type == pygame.MOUSEBUTTONDOWN:
scale = 1
if event.button == 4:
scale = 1.1
if event.button == 5:
scale = 0.9
trans = transformations.scale_matrix(scale)
matrix = numpy.matmul(trans, matrix)
# gltf.nodes[0].translation = [1,0,1]
# gltf.nodes[0].rotation = transformations.quaternion_from_euler(45,45,0).tolist()
dimensionsOLD(gltf, gltf.nodes[0])
boundingBox = np.subtract(myMax, myMin)
scale = [1, 1, 1]
print("min ", myMin)
print("max", myMax)
print("BoundingBox", boundingBox)
newScale = [1 / boundingBox[0], 1 / boundingBox[1], 1 / boundingBox[2]]
nodeToOverride = gltf.nodes[0]
matrix = nodeToOverride.matrix if len(
nodeToOverride.matrix) != 0 else transformations.scale_matrix(1)
rotation = nodeToOverride.rotation if len(nodeToOverride.rotation) != 0 else [
0, 0, 0, 1
]
translation = nodeToOverride.translation if len(
nodeToOverride.translation) != 0 else [0, 0, 0]
newScale = np.multiply(
newScale,
nodeToOverride.scale if len(nodeToOverride.scale) != 0 else [1, 1, 1])
newMatrix = composeMatrixFromMatrixAndTRS(matrix, translation, rotation,
newScale)
nodeToOverride.matrix = None
nodeToOverride.scale = None
nodeToOverride.translation = None
nodeToOverride.rotation = None
def __init__(self, T):
if ( type(T) == str):
print "Loading Transformation from file: " + T
Tfile = T
#Load transformation
Tfis = open(Tfile, 'r')
lines = []
lines = Tfis.readlines()
format = len(lines)
Tfis.seek(0) #reset file pointer
if not (format==3 or format==4 or format==5) :
raise ValueError("Wrong number of lines in file")
# import code; code.interact(local=locals())
if format == 3:
"""Handles processing a ground truth transfomation
File saved using vxl format
x' = s *(Rx + T)
scale
quaternion
translation """
print("Reading format 3")
self.scale = float(lines[0])
self.Ss = tf.scale_matrix(self.scale)
quat_line = lines[1].split(" ")
self.quat = tf.unit_vector(np.array([float(quat_line[3]),
float(quat_line[0]),
float(quat_line[1]),
float(quat_line[2])]))
self.Hs = tf.quaternion_matrix(self.quat)
trans_line = lines[2].split(" ")
self.Ts = np.array([float(trans_line[0]),
float(trans_line[1]),
float(trans_line[2])])
Tfis.close()
self.Rs = self.Hs.copy()[:3, :3]
self.Hs[:3, 3] = self.Ts[:3]
self.Hs = self.Ss.dot(self.Hs) # to add again
if format == 4 :
"""If the transformation was saved as:
H (4x4) - = [S*R|S*T]
"""
print("Reading format 4")
self.Hs = np.genfromtxt(Tfile, usecols={0, 1, 2, 3})
Tfis.close()
scale, shear, angles, trans, persp = tf.decompose_matrix(self.Hs)
self.scale = scale[0] # assuming isotropic scaling
self.Rs = self.Hs[:3, :3] * (1.0 / self.scale)
self.Ts = self.Hs[:3, 3] * (1.0 / self.scale)
self.quat = tf.quaternion_from_euler(angles[0], angles[1], angles[2])
if format==5:
"""If the transformation was saved as:
scale
H (4x4) - = [S*R|S*T]
"""
print("Reading format 5")
self.Hs = np.genfromtxt(Tfis, skip_header=1, usecols={0, 1, 2, 3})
Tfis.close()
Tfis = open(Tfile, 'r')
self.scale = np.genfromtxt(Tfis, skip_footer=4, usecols={0})
Tfis.close()
self.Rs = self.Hs[:3, :3] * (1.0 / self.scale)
self.Ts = self.Hs[:3, 3] * (1.0 / self.scale)
scale, shear, angles, trans, persp = tf.decompose_matrix(self.Hs)
self.quat = tf.quaternion_from_euler(angles[0], angles[1], angles[2])
print "Debugging translation:"
print self.Ts
print trans/self.scale
elif (type(T) == np.ndarray):
print "Loading Transformation array"
self.Hs = T
scale, shear, angles, trans, persp = tf.decompose_matrix(T)
self.scale =scale[0]
self.Rs = self.Hs[:3, :3] * (1.0 / self.scale)
self.Ts = self.Hs[:3, 3] * (1.0 / self.scale)
self.quat = tf.quaternion_from_euler(angles[0], angles[1], angles[2])
print "Debugging translation:"
print self.Ts
print trans/self.scale
# self.Rs = tf.quaternion_matrix(self.quat)
# self.Ts = trans / self.scale
print self.Hs
def scale(self, factor: Vec2):
cx = self.centroid.x
cy = self.centroid.y
t_matrix = (offset_matrix(-cx, -cy) @ scale_matrix(factor.x, factor.y)
@ offset_matrix(cx, cy))
self.transform(t_matrix)
def dimensionsOLD(gltf, node, parentMatrix=transformations.scale_matrix(1)):
# pprint(node)
T = node.translation or [0, 0, 0]
R = node.rotation or [0, 0, 0, 1]
S = node.scale or [1, 1, 1]
# thisNodeMatrix = composeMatrixFromTRS(T,R,S)
R = transformations.euler_from_quaternion(R,'sxyz')
thisNodeMatrix = node.matrix or transformations.compose_matrix(
S, None, R, T)
thisNodeMatrix = np.array(thisNodeMatrix)
thisNodeMatrix.shape = (4,4)
newMatrix = np.dot(thisNodeMatrix,parentMatrix)
if node.mesh != None:
accessor = gltf.accessors[gltf.meshes[node.mesh]
.primitives[0].attributes.POSITION]
possiblities = [
[0,0,0],
[0,0,1],
[0,1,0],
[0,1,1],
[1,0,0],
[1,0,1],
[1,1,0],
[1,1,1],
]
testMin = accessor.min
testMax = accessor.max
for test in possiblities:
test = [
testMin[0] if test[0] == 0 else testMax[0],
testMin[1] if test[1] == 0 else testMax[1],
testMin[2] if test[2] == 0 else testMax[2],
]
print("=======",node.name)
print('before',test)
test = np.matmul(test + [1],newMatrix )
scale, shear, angles, trans, persp = transformations.decompose_matrix(newMatrix)
print("by matrix",np.multiply(angles,57))
print('after',test)
myMin[0] = test[0] if test[0] < myMin[0] else myMin[0]
myMin[1] = test[1] if test[1] < myMin[1] else myMin[1]
myMin[2] = test[2] if test[2] < myMin[2] else myMin[2]
myMax[0] = test[0] if test[0] > myMax[0] else myMax[0]
myMax[1] = test[1] if test[1] > myMax[1] else myMax[1]
myMax[2] = test[2] if test[2] > myMax[2] else myMax[2]
if node.children == None or len(node.children) == 0:
# pprint(thisNodeMatrix)
# print(node.mesh)
return
for child in node.children:
dimensionsOLD(gltf, gltf.nodes[child], newMatrix)