def GetAllPoses(): trTool = Frame() trTool.FromVctFrm3(pointerBody.measured_cp().Position()) trArm = Frame() trArm.FromVctFrm3(armBody.measured_cp().Position()) trBase = Frame() trBase.FromVctFrm3(referenceBody.measured_cp().Position()) return trTool, trArm, trBase
def __init__(self, node, name): self.node = node.createChild(name) # node self.collision = None # the added collision mesh if any self.visual = None # the added visual model if any self.dofs = None # dofs self.mass = None # mass self.frame = Frame.Frame() self.framecom = Frame.Frame() self.fixedConstraint = None # to fix the RigidBody
def __init__(self, taxis_matrix=[]): self.index = 0 if len(taxis_matrix) > 0: self.frames = [Frame(taxis_coods=taxis_matrix[0])] for i in range(1, len(taxis_matrix)): self.add_frame( Frame(previous_frame=self.frames[i - 1], taxis_coods=taxis_matrix[i]))
def run(self): oldbary = [self.bary[0], self.bary[1]] cap = cv2.VideoCapture(self.nom_video) ret, im = cap.read() for i in range(self.debut): ret, im = cap.read() frame = Frame.Frame(im, self.bary, self.bac, self.angle, self.seuil, self.horizontal) cpt = 0 while (cap.isOpened()): if ret == True: im = cv2.resize(im, (0, 0), 0, self.scale, self.scale, cv2.INTER_AREA) im = cv2.cvtColor(im, cv2.COLOR_BGR2GRAY) frame = Frame.Frame(im, frame.bary, self.bac, self.angle, self.seuil, self.horizontal) frame.mouseActualisation() self.actualiseGrid(oldbary, frame.bary, frame.im.shape[0], frame.im.shape[1]) self.dparc = self.dparc + np.sqrt( (frame.bary[0] - oldbary[0])**2 + (frame.bary[1] - oldbary[1])**2) ## Debug : voir ce qu'il se passe pour chaque Frame ############################################################## # print("oldbary-bary") # print(oldbary,frame.bary) # frame.showFrame(self.l_pix) # self.grid.plotGrid() ################################################################################################################## oldbary = [frame.bary[0], frame.bary[1]] else: break if cpt > self.fps * 15 * 60: # if cpt > 2: break ret, im = cap.read() cpt = cpt + 1 cap.release() cv2.destroyAllWindows()
def register(self, b): """ Performs rigid-body registration with respect to another point cloud b, and returns the corresponding frame transformation. :param b: The point cloud being mapped to :type b: PointCloud :return: The Frame transformation F = [r, p] from current frame to b :rtype: Frame.Frame """ # NOTE: data, b are arrays of column vectors a_bar = np.mean(self.data, axis=1, keepdims=True) b_bar = np.mean(b.data, axis=1, keepdims=True) a_tilde = self.data - a_bar b_tilde = b.data - b_bar # Method using SVD to directly solve for R H = a_tilde.dot(b_tilde.T) u, s, v_t = scialg.svd(H) u = u.T v_t = v_t.T correction = np.identity(v_t.shape[1]) correction[-1, -1] = scialg.det(v_t.dot(u)) r = v_t.dot(correction.dot(u)) p = b_bar - r.dot(a_bar) return Frame.Frame(r, p)
def tofile(meshfile, bodyA, bodyB, sampleData, outfile): """ :param meshfile: path to file that defines surface mesh :param bodyA: path to file that defines rigid body A :param bodyB: path to file that defines rigid body B :param sampleData: path to file that contains frames of sample data :param outfile: path to file to write output to :type meshfile: str :type bodyA: str :type bodyB: str :type sampleData: str :type outfile: str """ vCoords, vIndices = icpf.meshDef(meshfile) nledA, ledA, tipA = icpf.bodyDef(bodyA) nledB, ledB, tipB = icpf.bodyDef(bodyB) aFrames, bFrames = icpf.readSample(sampleData, nledA, nledB) d_kPoints = icp.findTipB(aFrames, bFrames, ledA, tipA, ledB) s_i = icp.computeSamplePoints(d_kPoints, fr.Frame(np.identity(3), np.zeros(3))) c_kPoints = icp.ICPmatch(s_i, vCoords, vIndices) dist = icp.calcDifference(c_kPoints, d_kPoints) writefile(d_kPoints, c_kPoints, dist, outfile)
def fill_buffer(self, frame_queue, buffer_size): ''' this function is to fill the buffer with initial frames. ''' prev_idx = self.detection_data[0][0] j = 0 t = buffer_size frame = None while t > 0: coordinates = (self.detection_data[j][1], self.detection_data[j][2], self.detection_data[j][3], self.detection_data[j][4]) if (self.detection_data[j][0] != prev_idx): image_path = join(self.detection_imgs_path, str(prev_idx).zfill(8) + '.jpg') # image_path = join(self.detection_imgs_path, str(prev_idx).zfill(8)) # print(image_path) img = cv2.imread(image_path) img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) frame.set_img(img) frame_queue.append(frame) t -= 1 if self.detection_data[j][0] != prev_idx or j == 0: frame = Frame(self.detection_data[j][0]) frame.add_box(Box(coordinates, self.detection_data[j][5])) prev_idx = self.detection_data[j][0] j += 1 self.last_added_frame_index = frame.frame_indx self.last_readed_row = j - 1
def GetPose(): f = Frame() pose = pointerBody.GetPositionCartesian() if pose.GetValid(): # if visible f.FromVctFrm3(pose.Position()) f.IsValid = True #else return just the identity return f
def generate_frames(self): can_create = self._memory_size % self._instructions_per_frame == 0 if can_create: index = 0 print("Creating frames...") for split in xrange(0, self._memory_size, self._instructions_per_frame): self._frames.append(Frame(index, split, split + self._instructions_per_frame - 1)) index += 1
def GetReference(): f = Frame() pose = referenceBody.measured_cp() if pose.GetValid(): # if visible f.FromVctFrm3(pose.Position()) f.IsValid = True #else return just the identity return f
def addAbsoluteOffset(self, name, offset=[0, 0, 0, 0, 0, 0, 1], isMechanical=True): ## adding a offset given in absolute coordinates to the offset return RigidBody.Offset(self.node, name, (Frame.Frame(offset) * self.frame.inv()).offset(), isMechanical)
def GetPoseInRef(): r = GetReference() f = Frame() pointerInRef = Frame() pose = pointerBody.measured_cp() if pose.GetValid(): # if visible f.FromVctFrm3(pose.Position()) pointerInRef = r.Inverse() * f pointerInRef.IsValid = r.IsValid #will return global pointer pose. if (not r.IsValid): print 'Reference not valid' elif (not r.IsValid): print 'Reference and pointer not valid' else: print 'Pointer not valid' #else return just the identity return pointerInRef
def setFromRigidInfo(self, info, offset=[0, 0, 0, 0, 0, 0, 1], inertia_forces=False): self.framecom = Frame.Frame() self.framecom.rotation = info.inertia_rotation self.framecom.translation = info.com self.frame = Frame.Frame(offset) * self.framecom self.dofs = self.frame.insert(self.node, name='dofs', template="Rigid3" + template_suffix) self.mass = self.node.createObject('RigidMass', name='mass', mass=info.mass, inertia=concat( info.diagonal_inertia), inertia_forces=inertia_forces)
def addAbsoluteMappedPoint(self, name, position=[0, 0, 0], isMechanical=True): ## adding a position given in absolute coordinates to the rigid body frame = Frame.Frame() frame.translation = position return RigidBody.MappedPoint( self.node, name, (frame * self.frame.inv()).translation, isMechanical)
def addVisualModel(self, filepath, scale3d=[1, 1, 1], offset=[0, 0, 0, 0, 0, 0, 1], name_suffix=''): ## adding a visual model to the rigid body with a relative offset # @warning the translation due to the center of mass offset is automatically removed. If necessary a function without this mechanism could be added self.visual = RigidBody.VisualModel( self.node, filepath, scale3d, (self.framecom.inv() * Frame.Frame(offset)).offset(), name_suffix) return self.visual
def addMappedPoint(self, name, relativePosition=[0, 0, 0], isMechanical=True): ## adding a relative position to the rigid body # @warning the translation due to the center of mass offset is automatically removed. If necessary a function without this mechanism could be added frame = Frame.Frame() frame.translation = relativePosition return RigidBody.MappedPoint(self.node, name, (self.framecom.inv() * frame).translation, isMechanical)
def __init__(self, size): pygame.init() self.size = size self.screen = pygame.display.set_mode(size, 0, 32) self.color = black pygame.display.set_caption('PACMAN FOREVER YOUNG') self.run = True self.frame = Frame((50, 50, 1100, 800), cyan, 4) self.pacman = Pacman(self.frame) # game speed = number of operations per sec self.game_speed = 60 self.lines = []
def addCollisionMesh(self, filepath, scale3d=[1, 1, 1], offset=[0, 0, 0, 0, 0, 0, 1], name_suffix=''): ## adding a collision mesh to the rigid body with a relative offset # (only a Triangle collision model is created, more models can be added manually) # @warning the translation due to the center of mass offset is automatically removed. If necessary a function without this mechanism could be added self.collision = RigidBody.CollisionMesh( self.node, filepath, scale3d, (self.framecom.inv() * Frame.Frame(offset)).offset(), name_suffix) return self.collision
def get_next_frame(self, frame_queue): """ fill the buffer with the next frame and remove the first one. """ i = self.last_readed_row if self.last_added_frame_index >= self.num_of_frames and len( frame_queue) > 0: frame_queue.popleft() return curr = -1 if i < len(self.detection_data): curr = self.detection_data[i][0] detection_exist = True '''print(self.last_added_frame_index) print(curr)''' if self.last_added_frame_index != curr: detection_exist = False frame = Frame(self.last_added_frame_index) image_path = join(self.detection_imgs_path, str(self.last_added_frame_index).zfill(8) + '.jpg') img = cv2.imread(image_path) img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) frame.set_img(img) if not detection_exist: self.last_added_frame_index += 1 frame_queue.append(frame) return while i < len( self.detection_data) and self.detection_data[i][0] == curr: coordinates = (self.detection_data[i][1], self.detection_data[i][2], self.detection_data[i][3], self.detection_data[i][4]) box = Box(coordinates, self.detection_data[i][5]) frame.add_box(box) i += 1 for frame_box in frame.boxes: maxx = 0 for prev_frame in frame_queue: for prev_box in prev_frame.boxes: iou = box_box_iou(frame_box, prev_box) if iou >= 0.4 and iou > maxx: frame_box.ID = prev_box.ID maxx = iou frame_queue.append(frame) self.last_added_frame_index = frame.frame_indx + 1 self.last_readed_row = i
def __init__(self, name="unnamed"): self.name = name # node name self.collision = None # collision mesh self.visual = None # visual mesh self.dofs = Frame.Frame() # initial dofs self.mass = 1 # mass self.inertia = [1, 1, 1] # inertia tensor self.color = [1, 1, 1] # not sure this is used self.offset = None # rigid offset for com/inertia axes self.inertia_forces = 0 # 0: disabled, 1: explicit, 2: implicit self.group = None self.mu = 0 # friction coefficient self.scale = [1, 1, 1]
def mass_from_mesh(self, name, density=1000.0): info = generate_rigid(name, density) self.mass = info.mass # TODO svd inertia tensor, extract rotation quaternion self.inertia = [ info.inertia[0, 0], info.inertia[1, 1], info.inertia[2, 2] ] self.offset = Frame.Frame() self.offset.translation = info.com
def initialize(self): ''' It's better to call this method after create all objects''' self.timeNow = now.time() if self.frame : print('Frame already created') else : self.frame = Frame.Frame(self) self.updateScreen() self.mouse = Mouse.Mouse(self) self.keyboard = Keyboard.Keyboard(self) self.running = True
def render(self, frame, boxsampler): """ takes a set of objects and renders the frame boxsapmler is a sampler of bounding boxes (from the prior) """ renderedboxes = [] for i in frame.boundingboxes: if random.random() < self.detectionprob: renderedboxes.append(i) if random.random() < self.illusionprob: for j in range(len(np.random.poisson(self.illusionlambda))): renderedboxes.append(boxsampler.sample()) return Frame.Frame(renderedboxes)
def setManually(self, offset=[0, 0, 0, 0, 0, 0, 1], mass=1, inertia=[1, 1, 1], inertia_forces=False): ## create the rigid body by manually giving its inertia self.frame = Frame.Frame(offset) self.dofs = self.frame.insert(self.node, name='dofs', template="Rigid3" + template_suffix) self.mass = self.node.createObject('RigidMass', name='mass', mass=mass, inertia=concat(inertia), inertia_forces=inertia_forces)
def _FindCovFrame(self, *args): """ Finds the frame of this covariance tree node. :param args: Either just the number of triangles, or the number of triangles and a list of triangles. :type args: integer and [Triangle] :return: The frame of this covariance tree node :rtype: fr.Frame """ if len(args) == 1: points = None num_triangles = args[0] for i in range(num_triangles): if i == 0: points = self.triangle_list[i].corners.data.tolist() else: for p in self.triangle_list[i].corners.data.tolist(): points.append(p) points = np.array(points).squeeze().T return self._FindCovFrame(points, num_triangles * 3) elif len(args) == 2: points = args[0][:, 0:args[1]] c = np.mean(points, axis=1, keepdims=True) u = points - c A = u.dot(u.T) evals, evecs = np.linalg.eig(A) inds = evals.argsort()[::-1] max_evec = evecs[inds[0]] coeffs = np.zeros((3, 9)) coeffs[(0, 1, 2), (0, 3, 6)] = 1.0 r = np.linalg.lstsq(coeffs, max_evec)[0].reshape((3, 3)) u, s, v = np.linalg.svd(r) correction = np.eye(3) correction[-1, -1] = scialg.det(v.T.dot(u.T)) r = u.dot(correction.dot(v)) return fr.Frame(r, c)
def main(): parser = argparse.ArgumentParser("Extract frames from a media file\n") parser.add_argument( '-O', '--out', dest='dstfmt', help="Set destination folder and format of frames extracted") parser.add_argument('-N', '--nframes', dest='nframes_requested', type=int, help="Save up to nframes") parser.add_argument('-B', '--bulksave', dest='bulksave', action='store_true', help="Enable bulk saving") parser.add_argument('-S', '--sepsave', dest='sepsave', action='store_true', help="Enable seperated save") parser.add_argument('-I', '--infile', dest='infile', help="Media file to be extracted from") args = parser.parse_args() if (args.infile == None): print(f'{sys.argv[0]}: fatal error: no input file\nTerminating.') exit(1) if (args.dstfmt == None): print( f'{sys.argv[0]}: fatal error: output format not supplied\nTerminating.' ) exit(1) f = Frame.Frame() f.open(args.infile) f.extract(dstfmt=args.dstfmt, nframes=60, skip=1) f.extract(dstfmt=args.dstfmt, nframes=args.nframes_requested, bulksave=args.bulksave, sepsave=args.sepsave) f.close()
def performTransmissions(A, K, F, e, R, rnd): f = Frame.Frame(K, F) elapsed_time = 0 frames_total = 0 frames_correct = 0 while elapsed_time < R: # transmission time is the feedback time + 1 time unit per bit elapsed_time += A + F + f.getWastedData() frames_total += 1 if f.performErrorChance(e, rnd): # Successful transmission frames_correct += 1 return [elapsed_time, frames_total, frames_correct]
def extract_frames(self, frame_count): self.cap.set(cv2.CAP_PROP_POS_FRAMES, self.start_frame) every_nth_frame = int( (self.end_frame - self.start_frame) / frame_count) LOGGER.info("Every " + str(every_nth_frame) + " frames") cur_frame = self.start_frame retry = 0 while True: LOGGER.info("Frame: " + str(cur_frame)) flag, frame = self.cap.read() if flag: # The frame is ready and already captured retry = 0 filename = self.get_frame_filename(cur_frame) self.frames.append(Frame.Frame(cur_frame, filename, frame)) self.cap.set(cv2.CAP_PROP_POS_FRAMES, cur_frame + every_nth_frame) pos_frame = self.cap.get(cv2.CAP_PROP_POS_FRAMES) cur_frame += every_nth_frame else: # The next frame is not ready, so we try to read it again self.cap.set(cv2.CAP_PROP_POS_FRAMES, cur_frame - 1) LOGGER.debug("frame is not ready") # It is better to wait for a while for the next frame to be ready retry += 1 cv2.waitKey(1000) if retry > 10: # If retry passes 10, give up LOGGER.debug( "Retried too many times. Giving up on frame " + str(cur_frame)) cur_frame += every_nth_frame if cur_frame >= self.end_frame: # If the cur frame excceeded the total number of frames, we stop break jsonfile_name = self.output_dir + "/" + self.output_name + ".json" LOGGER.info("Writing json to: " + jsonfile_name) with open(jsonfile_name, "w") as jsonfile: json.dump(self.frames, jsonfile, cls=Frame.FrameEncoder, indent=2) LOGGER.info("Wrote data to " + jsonfile_name)
def __init__(self, node, name, offset, isMechanical=True): ## @param isMechanical: ## will the Offset be used for mechanics? And then propagate forces or mass ? ## Or will it be used only as a passive measure, visualization...? self.node = node.createChild(name) self.frame = Frame.Frame( offset) # store the offset, relative position to its reference self.dofs = self.node.createObject('MechanicalObject', template="Rigid3" + template_suffix, name='dofs') self.mapping = self.node.createObject( 'AssembledRigidRigidMapping', name="mapping", source='0 ' + str(self.frame), geometricStiffness=geometric_stiffness) self.mapping.mapForces = isMechanical self.mapping.mapConstraints = isMechanical self.mapping.mapMasses = isMechanical
def rollTurn(): if Frame.framesPlayedCounter <= 9: print("\n\tFrame Number {0}".format( Frame.framesPlayedCounter + 1)) # adds to static variable which tracks number of frames played while True: ball_one = input("\tBall [1] pins knocked down :") if ball_one.isdigit(): ball_one = int(ball_one) if ball_one <= 10: break else: print("You can't drop more that 10 pins in one go") else: print("Invalid Input, please enter a valid integer") while True: if ball_one != 10: # if first ball is 10 then second ball input is not required ball_two = input("\tBall [2] pins knocked down :") if ball_two.isdigit(): ball_two = int(ball_two) break else: print("Invalid Input, please enter a valid integer") else: ball_two = 0 break if ball_one + ball_two > 10: print("To many pins were knocked down!") print("Try enter the correct values") rollTurn() else: Frame(ball_one, ball_two) else: print("All 10 frames have been played") input("Press anything to exit" ) # stops program from closing after completion exit()