Exemple #1
0
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
Exemple #2
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
Exemple #4
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 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
Exemple #6
0
    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()
Exemple #7
0
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
Exemple #10
0
    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)
Exemple #11
0
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
Exemple #13
0
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
Exemple #14
0
    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
Exemple #16
0
 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
Exemple #18
0
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)
Exemple #19
0
# 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
Exemple #21
0
 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)
Exemple #22
0
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)