class RatSLAM:
""" A complete rat slam instance
(Matlab version: equivalent to rs_main)
"""
def __init__(self, **kwargs):
pose_cell_shape = kwargs.pop("pose_cell_dimensions", (100,100,60))
shift_match = kwargs.pop("odo_shift_match", 20)
# self.odometer = SimpleVisualOdometer()
self.odometer = SimpleVisualOdometer(fov_deg = kwargs.pop("fov_deg",50),
im_trans_y_range = kwargs.pop("im_trans_y_range"),
im_rot_y_range = kwargs.pop("im_rot_y_range"),
im_odo_x_range = kwargs.pop("im_odo_x_range"),
shift_match = shift_match,
trans_scale = kwargs.pop("odo_trans_scale",100.))
# vt_match_threshold = kwargs.pop('vt_match_threshold', None)
# self.view_templates = VisualTemplateCollection()
self.view_templates = VisualTemplateCollection( template_decay = kwargs.pop('template_decay', 1.0),
match_y_range = kwargs.pop('im_vt_y_range', None),
match_x_range = kwargs.pop('im_vt_x_range', None),
shift_match = shift_match,
match_threshold = kwargs.pop('vt_match_threshold', None))
self.pose_cell_network = PoseCellNetwork(pose_cell_shape)#,
# self.view_templates)
self.experience_map = ExperienceMap(self.pose_cell_network,
self.view_templates,
delta_pc_threshold = kwargs.pop("exp_delta_pc_threshold",1.0),
correction_factor = kwargs.pop("exp_correction_factor",0.5),
exp_loops = kwargs.pop("exp_loops",100))
def update(self, video_frame):
# TODO save experience map
# convert video_frame to grayscale # done outside
# plt.imshow(video_frame)
# plt.show()
# get odometry from video
speed, rotation, odo = self.odometer.estimate_odometry(video_frame)
# get most active visual template (bassed on current video_frame)
vt_id = self.view_templates.match(video_frame, odo[0], odo[1], odo[2])
# update pose cells
self.pose_cell_network.update(self.view_templates.templates[vt_id], speed, rotation)
# get 'best' center of pose cell activity
px, py, pr = self.pose_cell_network.activty_packet_center()
# run an iteration fo the experience map
experience_map.update(vt_id, speed, rotation, (px, py, pr))
# store current odometry for next update
# pass
print self.experience_map.get_current_experience()
