def initPose(self):
self.pose = pose.Pose()
self.prev_pose = pose.Pose()
self.prev_pose.x = 0
self.prev_pose.y = 0
self.prev_pose.theta = 0
self.pose.x = 0
self.pose.y = 0
self.pose.theta = 0
def __draw(self):
"""Draws the world and items in it.
This will draw the markers, the obstacles,
the robots, their tracks and their sensors
"""
if self.__robot is not None and self.__center_on_robot:
if self.__orient_on_robot:
self.__renderer.set_screen_center_pose(self.__robot.get_pose())
else:
self.__renderer.set_screen_center_pose(pose.Pose(self.__robot.get_pose().x, self.__robot.get_pose().y, 0.0))
self.__renderer.clear_screen()
if self.__draw_supervisors and self.__supervisor is not None:
self.__supervisor.draw_background(self.__renderer)
for bg_object in self.__background:
bg_object.draw(self.__renderer)
# Draw the robot, tracker and sensors after obstacles
if self.__show_tracks and self.__tracker is not None:
self.__tracker.draw(self.__renderer)
if self.__robot is not None:
self.__robot.draw(self.__renderer)
if self.__show_sensors:
self.__robot.draw_sensors(self.__renderer)
if self.__draw_supervisors and self.__supervisor is not None:
self.__supervisor.draw_foreground(self.__renderer)
# update view
self.__update_view()
def main():
"""Execute the command"""
parser = argparse.ArgumentParser(
description='Run image processing over test images')
parser.add_argument('-i',
'--id',
help='marker ID to find',
required=False,
type=int,
default=2)
parser.add_argument('-r',
'--results',
help='result directory',
required=False,
default='results')
parser.add_argument('-s',
'--source',
help='source directory for images',
required=False,
default='images')
args = parser.parse_args()
# Make sure output directory exists before we start.
try:
os.mkdir(args.results)
except FileExistsError:
pass
# Process files.
p = pose.Pose()
for name in os.listdir(args.source):
frame = cv2.imread(args.source + '/' + name)
p.solve(frame, args.id)
cv2.imwrite(args.results + '/' + name, p.display_results())
def publish(self):
if self.controller.at_goal(self.pose, self.goal):
desired = pose.Pose()
else:
desired = self.controller.get_velocity(self.pose, self.goal,
self.dT)
# if self.goal is not None \
# and (desired.xVel!=0.0 or desired.thetaVel!=0.0):
# rospy.loginfo(
# 'current=(%f,%f,%f) goal=(%f,%f,%f) xVel=%f thetaVel=%f',
# self.pose.x, self.pose.y, self.pose.theta,
# self.goal.x, self.goal.y, self.goal.theta,
# desired.xVel, desired.thetaVel)
d = self.controller.get_goal_distance(self.pose, self.goal)
self.dist_pub.publish(d)
self.send_velocity(desired.xVel, desired.thetaVel)
# Forget the goal if achieved.
if self.controller.at_goal(self.pose, self.goal):
rospy.loginfo('Goal achieved')
self.goal = None
msg = Bool()
msg.data = True
self.goal_achieved_pub.publish(msg)
def run_periodic(self):
js_left = self.my_robot.js_left
js_right = self.my_robot.js_right
spenner = 1
spenner_button = 1
tank_button = 3
qt_button = 2
if js_left.getRawButton(8):
self.my_robot.drive.ahrs.reset()
pose._estimator.current_pose = pose.Pose(0, 0, 0)
if js_left.getRawButton(spenner_button) or js_right.getRawButton(
spenner_button):
spenner = 0.7
if js_left.getRawButton(tank_button) or js_right.getRawButton(
tank_button):
self.my_robot.drive.tank_drive(-spenner * js_left.getY(),
-spenner * js_right.getY())
elif js_left.getRawButton(qt_button) or js_right.getRawButton(
qt_button):
self.my_robot.drive.arcade_drive(
spenner * mathutils.signed_power(-js_left.getY(), 2),
spenner * mathutils.signed_power(-js_right.getX(), 2))
else:
self.my_robot.drive.radius_drive(
-mathutils.signed_power(js_left.getY(), 2),
mathutils.signed_power(js_right.getX(), 2), spenner)
def __init__(self, parser):
"""Initialize the flying code."""
self.pose = pose.Pose()
self.cmd = None
self.marker_id = 0
self.flying = False
self.armed = False
self.image_count = 0
self.height_pid = pid.PID(False)
self.height_pid.set_set_point(
20.0) # We want to get to height of gate.
self.height_pid.set_output_limits(-1.0, 1.0)
self.height_pid.set_initial_output(-1.0)
self.height_pid.set_tunings(0.02, 0.0, 0.0, True)
self.missed_data = 0
parser.add_argument('-i',
'--id',
help='marker ID to find',
required=False,
type=int,
default=2)
parser.add_argument('-t',
'--ttyname',
help='Serial tty to transmitter.',
required=False,
default='/dev/ttyACM0')
def __init__(self, chassis_model):
self.sim_chassis = chassis_model
self.current_pose = pose.Pose(0, 0, 0, 0, 0, 0)
self.leg_controllers = [
lc.LegController(leg, IK_3DoF, legpose, self.current_pose) for leg,
legpose in zip(self.sim_chassis.legs, self.sim_chassis.leg_poses)
]
self.step_state = True
# Shared movement state
self.current_time = 0
self.interval_estimate = ESTIMATE_REFRESH_RATE
# Pose state
self.pose_deadline = 0
self.pose_start_time = 0
self.pose_interpolator = None
# Step state
self.step_deadline = 0
self.step_start_time = 0
self.current_step = 0
# Gait: iterable of steps, each step is ((legs to pick up), deadline, progress before next step)
self.gait = (((0, 2, 4), 1., 1.), ((1, 3, 5), 1., 1.))
# Current heading/rotation
self.x_vel = 0
self.y_vel = 0
self.r_vel = 0
def leaderOdomCallback(self, newPose): self.leaderPose = pose.Pose() pos = newPose.pose.pose.position orientation = newPose.pose.pose.orientation self.leaderPose.x = pos.x self.leaderPose.y = pos.y quaternion = (orientation.x, orientation.y, orientation.z, orientation.w) euler = tf.transformations.euler_from_quaternion(quaternion) self.leaderPose.theta = euler[2]
def goalCallback(self, goal):
self.goal = pose.Pose()
pos = goal.pose.position
orientation = goal.pose.orientation
self.goal.x = pos.x
self.goal.y = pos.y
quaternion = (orientation.x, orientation.y, orientation.z,
orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
self.goal.theta = euler[2]
def get_angle_pose(self, quaternion_pose):
q = [quaternion_pose.orientation.x,
quaternion_pose.orientation.y,
quaternion_pose.orientation.z,
quaternion_pose.orientation.w]
roll, pitch, yaw = euler_from_quaternion(q)
angle_pose = pose.Pose()
angle_pose.x = quaternion_pose.position.x
angle_pose.y = quaternion_pose.position.y
angle_pose.theta = yaw
return angle_pose
def __init__(self,
leg_model,
ik_func,
pose=pose.Pose(0, 0, 0, 0, 0, 0),
base_pose=pose.Pose(0, 0, 0, 0, 0, 0)):
self.sim_leg = leg_model
self.ik_func = ik_func
self.pose = pose
self.base_pose = base_pose
self.to_frame_mat = self.pose.to_frame_mat.dot(
self.base_pose.to_frame_mat)
self.from_frame_mat = self.base_pose.from_frame_mat.dot(
self.pose.from_frame_mat)
self.home_point = self.leg_to_global(2.5, 0, 0)
# Movement state
self.deadline = 0
self.current_time = 0
self.interval_estimate = ESTIMATE_REFRESH_RATE
self.dest = None
self.move_interpolator = None
def __init__(self, legs, leg_poses):
self.legs = legs
self.leg_poses = leg_poses
self.num_legs = len(self.legs)
self.origin = pose.Pose(0, 0, 0, 0, 0, 0)
if self.num_legs > 1:
leg_points = [(lpose.x, lpose.y, lpose.z, 1.)
for lpose in leg_poses]
self.base_segments = [(leg_points[x - 1], leg_points[x])
for x in xrange(self.num_legs)]
else:
self.base_segments = []
def get_test_chassis():
import math
midwidth = 2.
topwidth = 1.5
height = 2.5
top_r_p = pose.Pose(topwidth, height, 0, math.atan2(height, topwidth), 0,
0)
mid_r_p = pose.Pose(midwidth, 0, math.atan2(0, midwidth), 0, 0, 0)
bot_r_p = pose.Pose(topwidth, -height, 0, math.atan2(-height, topwidth), 0,
0)
bot_l_p = pose.Pose(-topwidth, -height, 0, math.atan2(-height, -topwidth),
0, 0)
mid_l_p = pose.Pose(-midwidth, 0, 0, math.atan2(0, -midwidth), 0, 0)
top_l_p = pose.Pose(-topwidth, height, 0, math.atan2(height, -topwidth), 0,
0)
top_r = lm.get_test_leg()
mid_r = lm.get_test_leg()
bot_r = lm.get_test_leg()
bot_l = lm.get_test_leg()
mid_l = lm.get_test_leg()
top_l = lm.get_test_leg()
legs = [top_r, mid_r, bot_r, bot_l, mid_l, top_l]
leg_poses = [top_r_p, mid_r_p, bot_r_p, bot_l_p, mid_l_p, top_l_p]
return Chassis(legs, leg_poses)
def split_pose_by_chains(self, p):
ps = []
for c in p.chains():
p_new = pose.Pose()
s = structure.Structure([c.copy()])
p.structure = s
bps = []
for bp in p.basepairs:
pass
ps.append(p)
return ps
def _copy_info_into_pose(self, m):
p = pose.Pose()
p.structure = m.structure
p.ends = m.ends
p.end_ids = m.end_ids
p.basepairs = m.basepairs
p.name = m.name
p.path = m.path
p.beads = m.beads
p.score = m.score
p.secondary_structure = m.secondary_structure
return p
def update(self):
if self.controller.atGoal(self.pose, self.goal):
desired = pose.Pose()
# if self.track_started:
rospy.loginfo("Goal Reached")
self.stopMotor.publish(1)
else:
desired = self.controller.getVelocity(self.pose, self.goal,
self.prev_pose)
self.track_started = 1
self.prev_pose = self.pose
twist = Twist()
twist.linear.x = desired.xVel
twist.angular.z = desired.thetaVel
self.twistPub.publish(twist)
def main():
frame_size = (1280, 720)
voxel_resolution = (12, 24)
# Projection found by findboard
projection = pose.Projection(
pose.CameraIntrinsics(
cameraMatrix=numpy.float32([
[887.09763773, 0., 639.5],
[0., 887.09763773, 359.5],
[0., 0., 1.],
]),
distCoeffs=numpy.float32([0., 0., 0., 0., 0.]),
),
pose.Pose(rvec=numpy.float32([
[1.32300998],
[-1.32785091],
[1.14510022],
]),
tvec=numpy.float32([
[3.58316198],
[3.06215196],
[10.00036672],
])),
)
heatmaps = get_piece_heatmaps(frame_size, voxel_resolution, projection)
reference_heatmap = get_reference_heatmap(heatmaps)
occlusions = get_occlusions(heatmaps, projection)
board = chess.Board()
projection_shape = tuple(reversed(frame_size))
negative_composite_memo = {(): Heatmap.blank(projection_shape)}
subtractor = cv2.imread('diff.png')[:, :, 0]
normalized_subtractor = subtractor / subtractor.stdev()
move_diffs = get_move_diffs(heatmaps, reference_heatmap, occlusions,
negative_composite_memo, board)
for (move, move_diff) in move_diffs.items():
# The Pearson correlation coefficient measures the goodness of fit
score = (move_diff * normalized_subtractor).mean()
print('score', board.san(move), score)
composite = numpy.zeros((720, 1280, 3), dtype=numpy.float32)
composite[:, :, 2] += move_diff
composite[:, :, 1] += normalized_subtractor
cv2.imshow(WINNAME, composite / 20)
key = cv2.waitKey(0)
def __draw(self):
"""Draws the world and items in it.
This will draw the markers, the obstacles,
the robots, their tracks and their sensors
"""
if self.__robots and self.__center_on_robot:
# Temporary fix - center onto first robot
robot = self.__robots[0]
if self.__orient_on_robot:
self.__renderer.set_screen_center_pose(robot.get_pose())
else:
self.__renderer.set_screen_center_pose(
pose.Pose(robot.get_pose().x,
robot.get_pose().y, 0.0))
self.__renderer.clear_screen()
if self.__draw_supervisors:
for supervisor in self.__supervisors:
supervisor.draw_background(self.__renderer)
for bg_object in self.__background:
bg_object.draw(self.__renderer)
for obstacle in self.__obstacles:
obstacle.draw(self.__renderer)
# Draw the robots, trackers and sensors after obstacles
if self.__show_tracks:
for tracker in self.__trackers:
tracker.draw(self.__renderer)
for robot in self.__robots:
robot.draw(self.__renderer)
if self.__show_sensors:
robot.draw_sensors(self.__renderer)
if self.__draw_supervisors:
for supervisor in self.__supervisors:
supervisor.draw_foreground(self.__renderer)
# update view
self.__update_view()
def getKeypoints(folder):
""" Generates keypoints (frames with poses) from openpose output """
keypointFiles = glob.glob(folder + "/*.json")
keypoints = []
for i, keypointFile in enumerate(keypointFiles):
with open(keypointFile) as data:
keypoint = json.load(data)
personIndex = 0
if len(keypoint["people"]) > 1:
personIndex = pose.getBiggestPersonIndex(keypoint)
p = pose.Pose()
posekp = pose.PoseKeypoint(i, p)
#if there's no person, use last keypoint data
if personIndex in range(0, len(keypoint["people"])):
p.fromData(keypoint["people"][personIndex])
posekp = pose.PoseKeypoint(i, p)
keypoints.append(posekp)
return keypoints
def main():
"""Execute the command"""
parser = argparse.ArgumentParser(
description='Run pose processing algorithms over a single image')
parser.add_argument('-i',
'--id',
help='marker ID to find',
required=False,
type=int,
default=2)
parser.add_argument('-f',
'--filename',
help='source image',
required=False,
default='images/raw_image_0.png')
args = parser.parse_args()
p = pose.Pose()
frame = cv2.imread(args.filename)
p.solve(frame, args.id)
cv2.imshow('frame', p.display_results())
cv2.waitKey(10000)
def main():
parser = argparse.ArgumentParser()
parser.add_argument("-t",
action="store",
dest="t_p",
type=str,
help="Test path")
parser.add_argument("-f",
action="store_true",
dest="f_d",
default=False,
help="Force detection")
args = vars(parser.parse_args())
test_path = args["t_p"]
if test_path == "":
print("No test path given!")
return
kpt_file_path = os.path.join(test_path, "kpt.txt")
kpt_file_exists = os.path.isfile(kpt_file_path)
force_detection = args["f_d"]
img_format = "jpeg"
ut = util.Util(test_path, roi=300, image_format=img_format)
if not kpt_file_exists or force_detection:
ut.processImage()
demo.PRNet(test_path)
shutil.rmtree(os.path.join(test_path, "temp"))
ut.readKPT()
head_pose = pose.Pose(test_path)
head_pose.regress(curve=3)
def pose(self, index):
return pose.Pose(self.orientations.get_rotation(index),
np.array(self.position_vector(index)))
def initPose(self):
self.myPose = pose.Pose()
self.myPose.x = 0
self.myPose.y = 0
self.myPose.theta = 0
def __construct_world(self):
"""Creates objects previously loaded from the world xml file.
This function uses the world in ``self.__world``.
All the objects will be created anew, including robots and supervisors.
All of the user's code is reloaded.
"""
if self.__world is None:
return
helpers.unload_user_modules()
self.__state = DRAW_ONCE
self.__robots = []
self.__obstacles = []
self.__supervisors = []
self.__background = []
self.__trackers = []
self.__qtree = None
for thing in self.__world:
thing_type = thing[0]
if thing_type == 'robot':
robot_type, supervisor_type, robot_pose, robot_color = thing[
1:5]
try:
# Create robot
robot_class = helpers.load_by_name(robot_type, 'robots')
robot = robot_class(pose.Pose(robot_pose))
if robot_color is not None:
robot.set_color(robot_color)
elif len(self.__robots) < 8:
robot.set_color(self.__nice_colors[len(self.__robots)])
# Create supervisor
sup_class = helpers.load_by_name(supervisor_type,
'supervisors')
info = robot.get_info()
info.color = robot.get_color()
supervisor = sup_class(robot.get_pose(), info)
supervisor.set_logqueue(self.__log_queue)
name = "Robot {}: {}".format(
len(self.__robots) + 1, sup_class.__name__)
if self.__supervisor_param_cache is not None and \
len(self.__supervisor_param_cache) > len(self.__supervisors):
supervisor.set_parameters(
self.__supervisor_param_cache[len(
self.__supervisors)])
self._out_queue.put(
("make_param_window",
(robot, name, supervisor.get_ui_description())))
self.__supervisors.append(supervisor)
# append robot after supervisor for the case of exceptions
self.__robots.append(robot)
# Create trackers
self.__trackers.append(
simobject.Path(robot.get_pose(), robot))
self.__trackers[-1].set_color(robot.get_color())
except:
self.log(
"[Simulator.construct_world] Robot creation failed!")
raise
#raise Exception('[Simulator.construct_world] Unknown robot type!')
elif thing_type == 'obstacle':
obstacle_pose, obstacle_coords, obstacle_color = thing[1:4]
if obstacle_color is None:
obstacle_color = 0xFF0000
self.__obstacles.append(
simobject.Polygon(pose.Pose(obstacle_pose),
obstacle_coords, obstacle_color))
elif thing_type == 'marker':
obj_pose, obj_coords, obj_color = thing[1:4]
if obj_color is None:
obj_color = 0x00FF00
self.__background.append(
simobject.Polygon(pose.Pose(obj_pose), obj_coords,
obj_color))
else:
raise Exception(
'[Simulator.construct_world] Unknown object: ' +
str(thing_type))
self.__time = 0.0
if not self.__robots:
raise Exception('[Simulator.construct_world] No robot specified!')
else:
self.__recalculate_default_zoom()
if not self.__center_on_robot:
self.focus_on_world()
self.__supervisor_param_cache = None
self.step_simulation()
self._out_queue.put(('reset', ()))
def initPose(self):
self.leaderPose = pose.Pose()
self.myPose = pose.Pose()
self.desired = pose.Pose()
def on_goal(self, goal):
#pass
self.action_client.wait_for_server()
action_goal = GoToPoseGoal()
action_goal.pose.pose = goal.pose
self.action_client.send_goal(action_goal)
def get_angle_pose(self, quaternion_pose):
q = [quaternion_pose.orientation.x,
quaternion_pose.orientation.y,
quaternion_pose.orientation.z,
quaternion_pose.orientation.w]
roll, pitch, yaw = euler_from_quaternion(q)
angle_pose = pose.Pose()
angle_pose.x = quaternion_pose.position.x
angle_pose.y = quaternion_pose.position.y
angle_pose.theta = yaw
return angle_pose
if __name__ == '__main__':
try:
node = GoToGoalNode()
node.main()
goal = pose.Pose()
goal.x = 5
goal.y = 0
goal.theta = 0
#node.on_execute(goal)
except rospy.ROSInterruptException:
pass
def sample(self, t):
f = casadi.Function('f', [self.sym_traj.x],
[self.sym_traj.sample(t).asMX()])
Rt = np.array(f.call([self.x_value])[0])
return pose.Pose(Rt[:, :3], Rt[:, 3].reshape((3, 1)))
def main():
#cap = cv2.VideoCapture('idaho.webm')
#cap.set(cv2.CAP_PROP_POS_MSEC, 52000)
# Game from https://www.youtube.com/watch?v=jOU3tmXgB8A
#cap = cv2.VideoCapture('acerook.mp4')
#cap.set(cv2.CAP_PROP_POS_MSEC, 7000)
# Game from https://www.youtube.com/watch?v=aHZtDuUMK50
#cap = cv2.VideoCapture('armin.mp4')
#cap.set(cv2.CAP_PROP_POS_MSEC, 13000)
# Game from https://www.youtube.com/watch?v=_N5sEyVc38o
#cap = cv2.VideoCapture('Zaven Adriasian - Luke McShane, Italian game, Blitz chess.mp4')
#cap.set(cv2.CAP_PROP_POS_MSEC, 105000)
# Game from https://www.youtube.com/watch?v=fot9b08TuWc
cap = cv2.VideoCapture('Carlsen-Karjakin, World Blitz Championship 2012.mp4')
cap.set(cv2.CAP_PROP_POS_MSEC, 11000)
ret, firstrgb = cap.read()
#cv2.imshow(WINNAME, firstrgb)
#cv2.waitKey(0)
#while True:
# ret, firstrgb = cap.read()
# if firstrgb is None:
# return
# cv2.imshow(WINNAME, firstrgb)
# key = cv2.waitKey(1000 // 30) & 0xff
# if key == ord(' '):
# break
# FIXME
#corners = findboard.find_chessboard_corners(firstrgb)
#projection = findboard.get_projection(corners, firstrgb.shape)
projection = pose.Projection(cameraIntrinsics=pose.CameraIntrinsics(cameraMatrix=numpy.array([[1.60091387e+03, 0.00000000e+00, 6.39500000e+02], [0.00000000e+00, 1.60091387e+03, 3.59500000e+02], [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]), distCoeffs=numpy.array([[0.], [0.], [0.], [0.], [0.]])), pose=pose.Pose(rvec=numpy.array([[ 1.68565304], [-1.07984874], [ 0.79226459]]), tvec=numpy.array([[ 2.72539522], [ 6.02566887], [26.47004221]])))
#cap.set(cv2.CAP_PROP_POS_MSEC, 52000)
#ret, firstrgb = cap.read()
firstlab = cv2.cvtColor(firstrgb, cv2.COLOR_BGR2LAB)
frame_size = tuple(reversed(firstlab.shape[:-1]))
projection_shape = tuple(reversed(frame_size))
piece_heatmaps = heatmaps.get_piece_heatmaps(frame_size, VOXEL_RESOLUTION, projection)
depths = heatmaps.get_depths(projection)
reference_heatmap = heatmaps.get_reference_heatmap(piece_heatmaps)
reference_heatmap_numpy = numpy.tile(numpy.expand_dims(reference_heatmap.as_numpy(), axis=-1), (4,))
#cv2.imshow(WINNAME, reference_heatmap.as_numpy() / reference_heatmap.delegate.max())
#cv2.waitKey(0)
first_board = chess.Board()
white_pieces_board = chess.Board()
white_pieces_board.set_piece_map({square: piece for (square, piece) in first_board.piece_map().items() if piece.color == chess.WHITE})
black_pieces_board = chess.Board()
black_pieces_board.set_piece_map({square: piece for (square, piece) in first_board.piece_map().items() if piece.color == chess.BLACK})
# FIXME: Account for occlusion of pieces
visible_white_pieces = heatmaps.get_board_heatmap(piece_heatmaps, white_pieces_board)
visible_black_pieces = heatmaps.get_board_heatmap(piece_heatmaps, black_pieces_board)
first_lightness = firstlab[:,:,0]
white_average = numpy.average(first_lightness[visible_white_pieces.slice], weights=visible_white_pieces.delegate)
black_average = numpy.average(first_lightness[visible_black_pieces.slice], weights=visible_black_pieces.delegate)
if black_average > white_average:
# Switch white and black
projection = findboard.flip_sides(projection)
piece_heatmaps = heatmaps.flip_piece_heatmaps(piece_heatmaps)
depths = heatmaps.flip_depths(depths)
(visible_white_pieces, visible_black_pieces) = (visible_black_pieces, visible_white_pieces)
starting_pieces = heatmaps.get_board_heatmap(piece_heatmaps, first_board)
#cv2.imshow(WINNAME, starting_pieces.as_numpy())
#cv2.waitKey(0)
light_squares = surface.get_light_square_heatmap(frame_size, projection)
visible_light_squares = heatmaps.Heatmap.blend([light_squares, starting_pieces]).subtract(starting_pieces)
#cv2.imshow(WINNAME, visible_light_squares.as_numpy())
#cv2.waitKey(0)
dark_squares = surface.get_dark_square_heatmap(frame_size, projection)
visible_dark_squares = heatmaps.Heatmap.blend([dark_squares, starting_pieces]).subtract(starting_pieces)
#cv2.imshow(WINNAME, visible_dark_squares.as_numpy())
#cv2.waitKey(0)
color_classifier = sklearn.naive_bayes.GaussianNB()
class_heatmaps = [visible_light_squares, visible_dark_squares, visible_white_pieces, visible_black_pieces]
class_pixels = [firstlab[class_heatmap.slice].reshape(-1, firstlab.shape[-1]) for class_heatmap in class_heatmaps]
class_weights = [class_heatmap.delegate.ravel() for class_heatmap in class_heatmaps]
class_labels = [numpy.full(class_weight.shape, class_idx) for (class_idx, class_weight) in enumerate(class_weights)]
color_classifier.fit(
numpy.vstack(class_pixels),
numpy.hstack(class_labels),
numpy.hstack(class_weights))
first_classes = color_classifier.predict_proba(firstlab.reshape(-1, firstlab.shape[-1])).reshape(tuple(firstlab.shape[:2]) + (4,))
#classified_composite = numpy.stack([
# first_classes[:,:,0],
# first_classes[:,:,1] + first_classes[:,:,2],
# first_classes[:,:,3] + first_classes[:,:,2],
#], axis=2)
#cv2.imshow(WINNAME, classified_composite)
#cv2.waitKey(0)
composite_memo = {(): numpy.stack([
light_squares.as_numpy(),
dark_squares.as_numpy(),
heatmaps.Heatmap.blank(projection_shape).as_numpy(),
heatmaps.Heatmap.blank(projection_shape).as_numpy(),
], axis=-1)}
first_move_diffs = heatmaps.get_move_diffs(piece_heatmaps, reference_heatmap, depths, composite_memo, first_board)
#first_move_diffs = {chess.Move(chess.E2, chess.E4): first_move_diffs[chess.Move(chess.E2, chess.E4)]}
first_weight = 1.
first_particle = Particle(first_weight, first_board, first_classes, first_move_diffs)
particles = {get_board_key(first_board): first_particle}
threshold_weight = first_weight * MAX_WEIGHT_RATIO
max_weight = first_weight
history = collections.deque([firstlab] * HISTORY_LEN)
while True:
print('memory', len(particles), sum(len(particle.diffs) for particle in particles.values()), sum(move_diff.size for particle in particles.values() for move_diff in particle.diffs.values()))
#pos = cap.get(cv2.CAP_PROP_POS_MSEC)
#cap.set(cv2.CAP_PROP_POS_MSEC, pos + 100)
ret, framergb = cap.read()
if framergb is None:
break
cv2.imshow(WINNAME, framergb)
cv2.waitKey(1)
framelab = cv2.cvtColor(framergb, cv2.COLOR_BGR2LAB)
history.appendleft(framelab)
if len(history) > HISTORY_LEN:
history.pop()
stable_mask = subtractor.get_stable_mask(history)
#display_mask = cv2.bitwise_and(framergb, stable_mask)
#cv2.imshow(WINNAME, display_mask)
#cv2.waitKey(1)
# TODO: Only classify the relevant sections of the image
frame_classes = color_classifier.predict_proba(framelab.reshape(-1, framelab.shape[-1])).reshape(tuple(framelab.shape[:-1]) + (4,))
#classified_composite = numpy.stack([
# frame_classes[:,:,0],
# frame_classes[:,:,1] + frame_classes[:,:,2],
# frame_classes[:,:,3] + frame_classes[:,:,2],
#], axis=2)
#cv2.imshow(WINNAME, classified_composite)
#cv2.waitKey(1)
# FIXME: The particles are being updated inside this loop
# Instead, create a separate collection for the next generation of particles
for particle in list(particles.values()):
if not particle.diffs:
continue
frame_diff = frame_classes - particle.stable_classes
stable_diff = numpy.choose(stable_mask, [numpy.zeros_like(frame_diff), frame_diff], mode='clip')
#stable_diff = frame_diff
#stable_diff[stable_mask] = 0
#cv2.imshow(WINNAME, stable_diff)
#key = cv2.waitKey(1) & 0xff
#advance_move = key == ord(' ')
#print('*', key, '*', advance_move)
#stable_diff_gray = subtractor.lab2mag(stable_diff)
#stable_diff_heatmap = heatmaps.Heatmap(stable_diff_gray)
#stable_diff_masked = heatmaps.Heatmap.product_zeros([reference_heatmap, stable_diff_heatmap])
stable_diff_masked = stable_diff * reference_heatmap_numpy
centered_subtractor = stable_diff_masked - numpy.expand_dims(reference_heatmap.reweight(stable_diff_masked.sum()).as_numpy(), axis=-1)
#cv2.imshow(WINNAME, (centered_subtractor[:,:,2] - centered_subtractor[:,:,2].min()) / (centered_subtractor[:,:,2].max() - centered_subtractor[:,:,2].min()))
#key = cv2.waitKey(0)
# The Pearson correlation coefficient measures the goodness of fit
scores = ((move, move_diff, (move_diff * centered_subtractor).sum())
for (move, move_diff) in particle.diffs.items())
# Each particle represents a hypothesis for which frame contains the next move
# Particles don't need to represent multiple alternative moves associated with a single frame,
# because the piece detection is reliable enough, given an accurate hypothesis about the timing
best_move_item = max(scores, key=lambda item: item[2])
(best_move, best_move_diff, best_score) = best_move_item
subtractor_total_variance = (centered_subtractor**2).sum()
if subtractor_total_variance:
subtractor_denom = math.sqrt(subtractor_total_variance * centered_subtractor.size)
else:
subtractor_denom = centered_subtractor.size
best_normalized_score = best_score / subtractor_denom
#cv2.imshow(WINNAME, (best_move_diff[:,:,2] - best_move_diff[:,:,2].min()) / (best_move_diff[:,:,2].max() - best_move_diff[:,:,2].min()))
#key = cv2.waitKey(0)
weight = particle.weight * (best_normalized_score + 1 - EXPECTED_CORRELATION)
if best_score < MIN_CORRELATION or weight < threshold_weight:
#print(' rejected candidate', weight, best_move)
continue
#newstable_classes = cv2.bitwise_and(frame_classes, stable_mask)
#invmask = cv2.bitwise_not(stable_mask)
#hole_classes = cv2.bitwise_and(particle.stable_classes, invmask)
#stable_classes = cv2.bitwise_or(hole_classes, newstable_classes)
stable_classes = numpy.choose(stable_mask, [particle.stable_classes, frame_classes], mode='clip')
next_board = particle.board.copy()
next_board.push(best_move)
next_particle_key = get_board_key(next_board)
existing_particle = particles.get(next_particle_key)
if existing_particle is None:
next_move_diffs = heatmaps.get_move_diffs(piece_heatmaps, reference_heatmap, depths, composite_memo, next_board)
elif existing_particle.weight < weight:
next_move_diffs = existing_particle.diffs
else:
#print(' rejected candidate', weight, chess.Board().variation_san(next_board.move_stack))
continue
next_particle = Particle(weight, next_board, stable_classes, next_move_diffs)
particles[next_particle_key] = next_particle
#if advance_move:
# particles = dict({next_particle_key: next_particle})
# print(' accepted candidate', best_normalized_score, chess.Board().variation_san(next_board.move_stack))
#if advance_move:
if weight > max_weight:
composite = numpy.zeros(framergb.shape, dtype=numpy.float32)
composite[:,:,0] += best_move_diff[:,:,0] + best_move_diff[:,:,1]
composite[:,:,1] += centered_subtractor[:,:,2] + centered_subtractor[:,:,3]
composite[:,:,2] += best_move_diff[:,:,2] + best_move_diff[:,:,3]
for channel in range(3):
composite[:,:,channel] -= composite[:,:,channel].min()
composite[:,:,channel] /= composite[:,:,channel].max()
cv2.putText(
composite,
'SCORE: {:.3f}'.format(best_normalized_score),
(10, 40),
cv2.FONT_HERSHEY_SIMPLEX,
1,
(1, 1, 1),
)
cv2.putText(
composite,
chess.Board().variation_san(next_board.move_stack),
(10, 80),
cv2.FONT_HERSHEY_SIMPLEX,
1,
(1, 1, 1),
)
cv2.imshow(WINNAME, composite)
cv2.waitKey(500)
#advance_move = False
# Resample by removing low-weighted particles
max_weight = max(particle.weight for particle in particles.values())
threshold_weight = max_weight * MAX_WEIGHT_RATIO
#print('threshold_weight', threshold_weight)
particles = {key: particle for (key, particle) in particles.items() if particle.weight >= threshold_weight}
#print('composite_memo', len(composite_memo))
print('particles:', len(particles), ', diffs:', sum(len(particle.diffs) for particle in particles.values()),
#', unique diffs:', len(set(hashlib.sha224(numpy.ascontiguousarray(diff.as_numpy())).hexdigest() for particle in particles.values() for diff in particle.diffs.values())),
)
for particle in sorted(particles.values(), key=lambda particle: particle.weight, reverse=True):
print('', particle.weight, chess.Board().variation_san(particle.board.move_stack))
cap.release()
cv2.destroyAllWindows()
def main():
cap, ret = open_video(path) # Open the video file
ret, frame = get_frame(cap, ret) # Get the frame in gray color
feature_cache = feature.FeaturesCache(
) # Features cache for keeping 4 features for initialization
feature_cache_particle = {} # Features cache variable for particles filter
X_map_init = feature.XMapDictionary(
).X_map_dict # Features map (X) on the initialization stage
particles_filter = [] # Particles filter variable
motion_model = m_m.Motion_model() # Motion model object
fastslam = fastSLAM.FastSLAM() # SLAM object
init_pose = pose.Pose(
constants.
init_euler_angles, # Initial pose value from constants.py file
constants.init_angular_rates,
constants.init_coordinates)
frame_number = 0
# For each frame running the algorithm cycle
while ret:
"""Initialization stage. It lasts for the 4 frames, in order to create measurements map object(X)."""
frame_number += 1
print frame_number
# Passing by first 5 frames, because of OpenCV SIFT estimation method work.
if start_frame <= frame_number <= finish_of_init_frame:
print "Initialization frame number:", frame_number
if frame_number == start_frame: # Initialization frame for creating all variable
measurements_Z = Measurements_Z(
[]) # Initialize measurements Z object
current_measurements = measurements_Z.init_measurement(
frame) # Estimate and input SIFT measurements
feature_cache.init_get_measurements(
current_measurements, X_map_init,
pose) # Input in the feature cache
else: # Working until form first measurements map(X) object elements
current_measurements = measurements_Z.make_measurement(frame)
feature_cache.get_measurements(current_measurements,
X_map_init, init_pose)
# Creating the particles objects for the particle filter
if frame_number == finish_of_init_frame:
particles_filter = {}
particle_weight = 1 / float(constants.NUMBER_OF_PARTICLES)
for i in range(0, constants.NUMBER_OF_PARTICLES):
particles_filter[i] = pose.Particle(particle_weight,
copy.deepcopy(init_pose),
copy.deepcopy(X_map_init),
copy.deepcopy(X_map_init))
feature_cache_particle[i] = copy.deepcopy(
feature.FeaturesCache())
feature_cache_particle[i].measurement_dict = copy.deepcopy(
feature_cache.measurement_dict)
feature_cache_particle[i].times_observed = copy.deepcopy(
feature_cache.times_observed)
""" Algorithm work main stage. """
if frame_number > finish_of_init_frame:
current_measurements = measurements_Z.make_measurement(frame)
for i in particles_filter:
feature_cache_particle[i].get_measurements(
current_measurements, particles_filter[i].X_map_dict,
particles_filter[i].pose)
# Motion model update
motion_model.rotational_motion_model(particles_filter[i].pose)
motion_model.translational_optimization(
particles_filter[i], current_measurements)
# ***Rao-Blackwellized particle filter update***
# Time update stage
fastslam.calc_position_proposal_distribution(
particles_filter[i], current_measurements)
# Measurement update stage and particles weighting
weight_sum = 0
weight = fastslam.measurement_update(particles_filter[i],
current_measurements)
weight_sum = weight_sum + weight
# Check for resampling and resampling if condition true
if fastslam.check_for_resampling(weight_sum):
particles_filter[
i].weight = 1.0 / constants.NUMBER_OF_PARTICLES
ret, frame = get_frame(cap, ret)
else:
print "time", time.clock()
print("Video`s end or camera is not working ")
def __init__(self, segment_input, pose_update_input):
self.segment_input = segment_input
self.pose_update_input = pose_update_input
self.curr_pose = pose.Pose(0, 0, 0, 0, 0, 0)
