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 __init__( self ):
self.im_count = 0
# Initialize the Pose Cell Network
self.pcn = PoseCellNetwork(shape=POSE_SIZE)
self.pc_count = 0
self.twist_data = deque()
self.odom_freq = ODOM_FREQ
midpoint = (math.floor(POSE_SIZE[0]/2),math.floor(POSE_SIZE[1]/2),math.floor(POSE_SIZE[2]/2))
self.pcn.inject( 1, midpoint )
# Set up layout for ROS message
dim = [ MultiArrayDimension( label="x", size=POSE_SIZE[0], stride=POSE_SIZE[0] * POSE_SIZE[1] * POSE_SIZE[2] ),
MultiArrayDimension( label="y", size=POSE_SIZE[1], stride=POSE_SIZE[1] * POSE_SIZE[2] ),
MultiArrayDimension( label="th", size=POSE_SIZE[2], stride=POSE_SIZE[2] ) ]
self.pc_layout = MultiArrayLayout(dim=dim)
# Initialize the View Templates
# TODO: put reasonable values here
self.vts = ViewTemplates( x_range=X_RANGE, y_range=Y_RANGE,
x_step=X_STEP, y_step=Y_STEP,
im_x=IM_SIZE[0], im_y=IM_SIZE[1],
match_threshold=MATCH_THRESHOLD)
self.vt_count = 0
self.bridge = CvBridge()
# Initialize the Experience Map
self.em = ExperienceMap()
self.em_count = 0
self.vis_odom_data = deque()
rospy.init_node( 'posecells', anonymous=True )
if not USE_VISUAL_ODOMETRY:
self.sub_odom = rospy.Subscriber( 'navbot/odom',Odometry,self.odom_callback )
self.sub_vis = rospy.Subscriber( 'navbot/camera/image',Image,self.vis_callback )
self.pub_pc = rospy.Publisher( 'navbot/posecells', numpy_msg(Float64MultiArray) )
self.pub_em = rospy.Publisher( 'navbot/experiencemap', Pose2D )
# Set up publisher for the template matches (sends just the index value of a match)
self.pub_tm = rospy.Publisher( 'navbot/templatematches', Int32 )
# Set up a visual odometer
self.vis_odom = SimpleVisualOdometer()
if RECORD_ROSBAG:
date = datetime.datetime.now()
self.bag = rosbag.Bag( '../testdata/Output-{0}-{1}-{2}-{3}.bag'.format( date.month, date.day,
date.hour, date.minute ),'w' )
