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