def __init__(self): self.last_image = None self.last_image_timestamp = None self.last_draw_image = None self.image_sub = sub8_ros_tools.Image_Subscriber( '/down/left/image_raw', self.image_cb) self.image_pub = sub8_ros_tools.Image_Publisher( "down/left/target_info") self.occ_grid = MarkerOccGrid(self.image_sub, grid_res=.05, grid_width=500, grid_height=500, grid_starting_pose=Pose2D(x=250, y=250, theta=0)) self.range = sub8_ros_tools.get_parameter_range('/color/channel_guide') self.channel_width = sub8_ros_tools.wait_for_param( '/vision/channel_width') self.pose_service = rospy.Service("vision/channel_marker/2D", VisionRequest2D, self.request_pipe) # Occasional status publisher self.timer = rospy.Timer(rospy.Duration(.5), self.publish_target_info)
def __init__(self): self.transformer = tf.TransformListener() rospy.sleep(1.0) self.done_once = False self.last_image = None self.last_draw_image = None self.last_poop_image = None self.last_image_time = None self.camera_model = None self.last_t = None self.observations = deque() self.pose_pairs = deque() self.rviz = rviz.RvizVisualizer() self.pose2d_service = rospy.Service('vision/buoys/2D', VisionRequest2D, self.request_buoy) self.pose_service = rospy.Service('vision/buoys/pose', VisionRequest, self.request_buoy3d) self.image_sub = sub8_ros_tools.Image_Subscriber('/stereo/right/image_rect_color', self.image_cb) self.image_pub = sub8_ros_tools.Image_Publisher('/vision/buoy_2d/target_info') # Occasional status publisher self.timer = rospy.Timer(rospy.Duration(1.0), self.publish_target_info) rospack = rospkg.RosPack() boost_path = os.path.join( rospack.get_path('sub8_perception'), 'sub8_vision_tools', 'classifiers', 'boost.cv2' ) self.boost = cv2.Boost() rospy.loginfo("Loading boost") self.boost.load(boost_path) rospy.loginfo("Boost loaded") self.buoys = { 'green': '/color/buoy/green', 'red': '/color/buoy/red', 'yellow': '/color/buoy/yellow', } self.ppf = None self.multi_obs = None self.draw_colors = { 'green': (0.0, 1.0, 0.0, 1.0), 'red': (1.0, 0.0, 0.0, 1.0), 'yellow': (1.0, 1.0, 0.0, 1.0), }
def __init__(self): self.transformer = tf.TransformListener() rospy.sleep(1.0) self.done_once = False self.last_image = None self.last_draw_image = None self.last_poop_image = None self.last_image_time = None self.camera_model = None self.last_t = None self.rviz = rviz.RvizVisualizer() self.pose_service = rospy.Service('vision/buoy/2D', VisionRequest2D, self.request_buoy) self.image_sub = sub8_ros_tools.Image_Subscriber( '/stereo/right/image_rect_color', self.image_cb) self.image_pub = sub8_ros_tools.Image_Publisher( '/vision/buoy_2d/target_info') # Occasional status publisher self.timer = rospy.Timer(rospy.Duration(1.0), self.publish_target_info) self.buoys = { 'green': '/color/buoy/green', 'red': '/color/buoy/red', 'yellow': '/color/buoy/yellow', } self.ppf = None self.draw_colors = { 'green': (0.0, 1.0, 0.0, 1.0), 'red': (1.0, 0.0, 0.0, 1.0), 'yellow': (1.0, 1.0, 0.0, 1.0), }
def main(): """Here's some stupid demo code """ import sub8_ros_tools import time import rospy rospy.init_node('test_estimation') q = sub8_ros_tools.Image_Subscriber('/stereo/left/image_rect_color') while (q.camera_info is None): time.sleep(0.1) camera_model = image_geometry.PinholeCameraModel() camera_model.fromCameraInfo(q.camera_info) ppf = ProjectionParticleFilter(camera_model, 100000) real = np.array([1.0, 3.0, 7.0]) p_wrong = 0.4 projected_h = ppf.K.dot(real) projected = projected_h[:2] / projected_h[2] print 'starting' R = np.diag([1.0, 1.0, 1.0]) camera_t = np.array([0.0, 0.0, 0.0]) cameras = [] observations = [] cov = np.array([100, 100, 100]) max_k = 15 for k in range(max_k): if k < 1: camera_t = np.array([0.0, -8.0, 7.0]) R = np.array([ [1.0, 0.0, 0.0], [0.0, 0.0, 1.0], [0.0, -1.0, 0.0], ]) else: R = np.diag([1.0, 1.0, 1.0]) camera_t = np.hstack([(np.random.random(2) - 0.5) * 5, 0.0]) if (np.random.random() < p_wrong) and (k > 1): print "Doing a random observation" projected = np.random.random(2) * np.array([640., 480.]) else: projected_h = ppf.K.dot( np.dot(R.transpose(), real) - R.transpose().dot(camera_t)) projected = projected_h[:2] / projected_h[2] obs_final = projected + np.random.normal(scale=5.0, size=2) cameras.append((camera_t, R)) observations.append(ppf.get_ray(obs_final)) # draw_cameras(observations, cameras) # draw_particles(ppf, color_hsv=((k + 1) / (max_k + 1), 0.7, 0.8), scale=0.1 * ((k + 1) / max_k)) # mlab.show() # Interpolate along hsv to get a cool heatmap effect ppf.set_pose(camera_t, R) ppf.observe(np.reshape(obs_final, (2, 1))) best_v, cov = ppf.get_best() print 'best', best_v print 'cov', cov draw_cameras(observations, cameras) draw_particles(ppf, color_hsv=((k + 1) / (max_k + 1), 0.7, 0.8), scale=0.1 * ((k + 1) / max_k)) mlab.show()
def test(): """Here's some stupid demo code TODO: Make this an actual unit test """ import sub8_ros_tools import time import rospy from mayavi import mlab rospy.init_node('test_estimation') q = sub8_ros_tools.Image_Subscriber('/stereo/left/image_rect_color') while (q.camera_info is None): time.sleep(0.1) camera_model = image_geometry.PinholeCameraModel() camera_model.fromCameraInfo(q.camera_info) MO = MultiObservation(camera_model) # K = np.array(camera_model.fullIntrinsicMatrix(), dtype=np.float32) real = np.array([1.0, 3.0, 7.0]) p_wrong = 0.0 projected_h = MO.K.dot(real) projected = projected_h[:2] / projected_h[2] print 'starting' R = np.diag([1.0, 1.0, 1.0]) camera_t = np.array([0.0, 0.0, 0.0]) cameras = [] rays = [] observations = [] max_k = 4 for k in range(max_k): if k < 1: camera_t = np.array([0.0, -8.0, 7.0]) R = np.array([ [1.0, 0.0, 0.0], [0.0, 0.0, 1.0], [0.0, -1.0, 0.0], ]) else: R = np.diag([1.0, 1.0, 1.0]) camera_t = np.hstack([(np.random.random(2) - 0.5) * 5, 0.0]) if (np.random.random() < p_wrong) and (k > 1): print "Doing a random observation" projected = np.random.random(2) * np.array([640., 480.]) else: projected_h = MO.K.dot( np.dot(R.transpose(), real) - R.transpose().dot(camera_t)) projected = projected_h[:2] / projected_h[2] obs_final = projected + np.random.normal(scale=2.0, size=2) cameras.append((camera_t, R)) rays.append(MO.get_ray(obs_final)) observations.append(obs_final) best_p = MO.multilaterate(observations, cameras) mlab.points3d(*map(np.array, best_p), scale_factor=0.3) estimation.draw_cameras(rays, cameras) mlab.show()