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