def test_monocular(self):
ci = sensor_msgs.msg.CameraInfo()
ci.width = 640
ci.height = 480
print ci
cam = PinholeCameraModel()
cam.fromCameraInfo(ci)
#print cam.rectifyPoint((0, 0))
print cam.project3dToPixel((0,0,0))
def handle_img_msg(self, img_msg):
now = rospy.get_rostime()
img = None
bridge = cv_bridge.CvBridge()
try:
img = bridge.imgmsg_to_cv2(img_msg, 'bgr8')
except cv_bridge.CvBridgeError as e:
rospy.logerr( 'image message to cv conversion failed :' )
rospy.logerr( e )
print( e )
return
self.frame_index = self.timestamp_map[img_msg.header.stamp.to_nsec()]
f = self.frame_map[self.frame_index][0]
obs_centroid = np.array(f.trans) + np.array(self.offset)
R = tf.transformations.quaternion_matrix(self.rotation_offset)
rotated_centroid = R.dot(list(obs_centroid)+[1])
obs_centroid = rotated_centroid[:3]
#self.orient = list(f.rotq)
self.marker.header.stamp = now
self.marker.pose.position = Point(*list(obs_centroid))
self.marker.pose.orientation = Quaternion(*self.orient)
self.obs_marker.pose.position = Point(*list(obs_centroid))
self.obs_marker.pose.orientation = Quaternion(*self.orient)
self.add_bbox_lidar()
md = self.metadata
dims = np.array([md['l'], md['w'], md['h']])
outputName = '/image_bbox'
if obs_centroid is None:
rospy.loginfo("Couldn't find obstacle centroid")
imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
return
# print centroid info
rospy.loginfo(str(obs_centroid))
# case when obstacle is not in camera frame
if obs_centroid[0]<2.5 :
self.imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
return
# get bbox
R = tf.transformations.quaternion_matrix(self.orient)
corners = [0.5*np.array([i,j,k])*dims for i in [-1,1]
for j in [-1,1] for k in [-1,1]]
corners = [obs_centroid + R.dot(list(c)+[1])[:3] for c in corners]
projected_pts = []
cameraModel = PinholeCameraModel()
cam_info = load_cam_info(self.calib_file)
cameraModel.fromCameraInfo(cam_info)
projected_pts = [cameraModel.project3dToPixel(list(pt)+[1]) for pt in corners]
projected_pts = np.array(projected_pts)
center = np.mean(projected_pts, axis=0)
out_img = drawBbox(img, projected_pts)
self.imgOutput.publish(bridge.cv2_to_imgmsg(out_img, 'bgr8'))
def test_monocular(self):
ci = sensor_msgs.msg.CameraInfo()
ci.width = 640
ci.height = 480
print ci
cam = PinholeCameraModel()
cam.fromCameraInfo(ci)
#print cam.rectifyPoint((0, 0))
print cam.project3dToPixel((0, 0, 0))
def handle_img_msg(self, img_msg):
img = None
bridge = cv_bridge.CvBridge()
try:
img = bridge.imgmsg_to_cv2(img_msg, 'bgr8')
except cv_bridge.CvBridgeError as e:
rospy.logerr('image message to cv conversion failed :')
rospy.logerr(e)
print(e)
return
#tx, ty, tz, yaw, pitch, roll = [0.00749025, -0.40459941, -0.51372948,
# -1.66780896, -1.59875352, -3.05415572]
#translation = [tx, ty, tz, 1]
#rotationMatrix = tf.transformations.euler_matrix(roll, pitch, yaw)
#rotationMatrix[:, 3] = translation
md = self.metadata
dims = np.array([md['l'], md['w'], md['h']])
outputName = '/image_bbox'
imgOutput = rospy.Publisher(outputName, Image, queue_size=1)
obs_centroid = self.obs_centroid
if self.obs_centroid is None:
rospy.loginfo("Couldn't find obstacle centroid")
imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
return
# print centroid info
rospy.loginfo(str(obs_centroid))
# case when obstacle is not in camera frame
if obs_centroid[0] < 2.5:
imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
return
# get bbox
R = tf.transformations.quaternion_matrix(self.orient)
corners = [
0.5 * np.array([i, j, k]) * dims for i in [-1, 1] for j in [-1, 1]
for k in [-1, 1]
]
corners = [obs_centroid + R.dot(list(c) + [1])[:3] for c in corners]
projected_pts = []
cameraModel = PinholeCameraModel()
cam_info = load_cam_info(self.calib_file)
cameraModel.fromCameraInfo(cam_info)
projected_pts = [
cameraModel.project3dToPixel(list(pt) + [1]) for pt in corners
]
#for pt in corners:
# rotated_pt = rotationMatrix.dot(list(pt)+[1])
# projected_pts.append(cameraModel.project3dToPixel(rotated_pt))
projected_pts = np.array(projected_pts)
center = np.mean(projected_pts, axis=0)
out_img = drawBbox(img, projected_pts)
imgOutput.publish(bridge.cv2_to_imgmsg(out_img, 'bgr8'))
def handle_img_msg(self, img_msg):
img = None
bridge = cv_bridge.CvBridge()
try:
img = bridge.imgmsg_to_cv2(img_msg, 'bgr8')
except cv_bridge.CvBridgeError as e:
rospy.logerr( 'image message to cv conversion failed :' )
rospy.logerr( e )
print( e )
return
#tx, ty, tz, yaw, pitch, roll = [0.00749025, -0.40459941, -0.51372948,
# -1.66780896, -1.59875352, -3.05415572]
#translation = [tx, ty, tz, 1]
#rotationMatrix = tf.transformations.euler_matrix(roll, pitch, yaw)
#rotationMatrix[:, 3] = translation
md = self.metadata
dims = np.array([md['l'], md['w'], md['h']])
outputName = '/image_bbox'
imgOutput = rospy.Publisher(outputName, Image, queue_size=1)
obs_centroid = self.obs_centroid
if self.obs_centroid is None:
rospy.loginfo("Couldn't find obstacle centroid")
imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
return
# print centroid info
rospy.loginfo(str(obs_centroid))
# case when obstacle is not in camera frame
if obs_centroid[0]<2.5 :
imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
return
# get bbox
R = tf.transformations.quaternion_matrix(self.orient)
corners = [0.5*np.array([i,j,k])*dims for i in [-1,1]
for j in [-1,1] for k in [-1,1]]
corners = [obs_centroid + R.dot(list(c)+[1])[:3] for c in corners]
projected_pts = []
cameraModel = PinholeCameraModel()
cam_info = load_cam_info(self.calib_file)
cameraModel.fromCameraInfo(cam_info)
projected_pts = [cameraModel.project3dToPixel(list(pt)+[1]) for pt in corners]
#for pt in corners:
# rotated_pt = rotationMatrix.dot(list(pt)+[1])
# projected_pts.append(cameraModel.project3dToPixel(rotated_pt))
projected_pts = np.array(projected_pts)
center = np.mean(projected_pts, axis=0)
out_img = drawBbox(img, projected_pts)
imgOutput.publish(bridge.cv2_to_imgmsg(out_img, 'bgr8'))
class DrawFrame:
def __init__(self):
rospy.init_node('draw_frames', anonymous=True)
self.listener = tf.TransformListener()
camera_str = '/camera/color/image_raw'
self.bridge = CvBridge()
self.im_sub = rospy.Subscriber(camera_str, sensor_msgs.msg.Image,
self.callback)
cam_info_str = "/camera/color/camera_info"
cam_info_msg = rospy.wait_for_message(cam_info_str,
sensor_msgs.msg.CameraInfo)
self.ph = PinholeCameraModel()
self.ph.fromCameraInfo(cam_info_msg)
def callback(self, ros_data):
try:
cv_im = self.bridge.imgmsg_to_cv2(ros_data, "bgr8")
except CvBridgeError as e:
print e
(rows, cols, channels) = cv_im.shape
if cols > 60 and rows > 60:
cv2.circle(cv_im, (50, 50), 10, 255)
# get ros time
now = rospy.get_rostime()
# wait for 1/30 seconds
rospy.sleep(1. / 30.)
# get tf transform between desired frames
(trans, rot) = self.listener.lookupTransform('/camera_link',
'/jenga_tf', now)
# use project3dToPixel for the origin, get it in camera (u,v)
ps = PointStamped()
ps.point.x = trans[0]
ps.point.y = trans[1]
ps.point.z = trans[2]
ps.header = ros_data.header
ps.header.stamp = now
point = self.listener.transformPoint("/camera_link", ps)
print(trans)
print point
uv = self.ph.project3dToPixel(
(point.point.x, point.point.y, point.point.z))
print uv
# draw a circle at (u,v)
cv2.circle(cv_im, (int(uv[0]), int(uv[1])), 10, 255)
cv2.imshow('image', cv_im)
cv2.waitKey(3)
class RayTransformer():
def __init__(self, topic):
self.model = PinholeCameraModel()
rospy.Subscriber(topic, CameraInfo, self.callback_info)
def callback_info(self, info):
self.model.fromCameraInfo(info)
def projectPointCloudToPixels(self, cloud):
pixel_cloud = []
for point in cloud.points:
pixel = self.model.project3dToPixel((point.x, point.y, point.z))
pixel_cloud.append(pixel)
return pixel_cloud
class XYZToScreenPoint(object):
def __init__(self):
self.cameramodels = PinholeCameraModel()
self.is_camera_arrived = False
self.frame_id = None
self.tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0))
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
self.pub = rospy.Publisher("~output", PointStamped, queue_size=1)
rospy.Subscriber('~input/camera_info', CameraInfo, self.camera_info_cb)
rospy.Subscriber('~input', PointStamped, self.point_stamped_cb)
def camera_info_cb(self, msg):
self.cameramodels.fromCameraInfo(msg)
self.frame_id = msg.header.frame_id
self.is_camera_arrived = True
def point_stamped_cb(self, msg):
if not self.is_camera_arrived:
return
pose_stamped = PoseStamped()
pose_stamped.pose.position.x = msg.point.x
pose_stamped.pose.position.y = msg.point.y
pose_stamped.pose.position.z = msg.point.z
try:
transform = self.tf_buffer.lookup_transform(
self.frame_id, msg.header.frame_id, rospy.Time(0),
rospy.Duration(1.0))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as e:
rospy.logerr('lookup_transform failed: {}'.format(e))
return
position_transformed = tf2_geometry_msgs.do_transform_pose(
pose_stamped, transform).pose.position
pub_point = (position_transformed.x, position_transformed.y,
position_transformed.z)
u, v = self.cameramodels.project3dToPixel(pub_point)
rospy.logdebug("u, v : {}, {}".format(u, v))
pub_msg = PointStamped()
pub_msg.header = msg.header
pub_msg.header.frame_id = self.frame_id
pub_msg.point.x = u
pub_msg.point.y = v
pub_msg.point.z = 0
self.pub.publish(pub_msg)
def is_visible(self, pose, point):
"""
check if point is visible by projecting it onto 2d camera plane
todo: distance check to prevent very far away points from passing
Args:
pose (Pose): current pose of vehicle
point (Point): point to check
Returns:
bool: if point is visible
"""
transformed = self.transform_point(pose.header, point)
# pinhole camera model reverses x, y coordinates
x = -transformed.point.y # point.y = horizontal camera dimension
y = -transformed.point.z # point.z = vertical camera dimension
z = transformed.point.x # point.x = z/depth camera dimension
# todo: does this need to be more elegant?
if z > self.max_visible_distance:
return False
if not self.pinhole_camera_visible_check:
return True
# create camera info
camera = PinholeCameraModel()
camera.fromCameraInfo(self.camera_info)
# project point onto image
u, v = camera.project3dToPixel((x, y, z))
# setup bounding box
cx = camera.cx()
cy = camera.cy()
width = self.camera_info.width
height = self.camera_info.height
top = cy + height / 2
bottom = cy - height / 2
right = cx + width / 2
left = cx - width / 2
# light is visible if projected within the bounding box of the 2d image
return left <= u and u <= right and bottom <= v and v <= top
class ScanTheCodePerception(object):
"""Class that handles ScanTheCodePerception."""
def __init__(self, my_tf, debug, nh):
"""Initialize ScanTheCodePerception class."""
self.image_cache = deque()
self.bridge = CvBridge()
self.nh = nh
self.last_cam_info = None
self.debug = debug
self.pers_points = []
self.model_tracker = ScanTheCodeModelTracker()
self.camera_model = PinholeCameraModel()
self.my_tf = my_tf
self.rect_finder = RectangleFinderClustering()
self.color_finder = ColorFinder()
self.count = 0
self.depths = []
@txros.util.cancellableInlineCallbacks
def _init(self):
self.vel_sub = yield self.nh.subscribe("/velodyne_points", PointCloud2)
self.image_sub = yield self.nh.subscribe("/stereo/right/image_rect_color", Image)
defer.returnValue(self)
def add_image(self, image):
"""Add an image to the image cache."""
if len(self.image_cache) > 50:
self.image_cache.popleft()
self.image_cache.append(image)
def update_info(self, info):
"""Update the camera calibration info."""
self.last_cam_info = info
self.camera_model.fromCameraInfo(info)
def _get_top_left_point(self, points_3d):
xmin = sys.maxint
zmin = -sys.maxint
ymin = sys.maxint
for i, point in enumerate(points_3d):
if point[1] < ymin:
xmin = point[0]
zmin = point[2]
ymin = point[1]
buff = 1
for i, point in enumerate(points_3d):
if(point[1] < ymin + buff and point[1] > ymin - buff and point[0] < xmin):
xmin = point[0]
zmin = point[2]
return xmin, ymin, zmin
def _get_points_in_range(self, axis, lower, upper, points):
in_range = []
idx = 0
if axis == 'y':
idx = 1
if axis == 'z':
idx = 2
for p in points:
if p[idx] > lower and p[idx] < upper:
in_range.append(p)
return in_range
def _get_depth(self, axis, points_3d):
idx = 0
if axis == 'y':
idx = 1
if axis == 'z':
idx = 2
min_val, max_val = sys.maxint, -sys.maxint
points_3d = np.array(points_3d)
my_points = points_3d[:, idx]
mean = np.mean(my_points)
std = 1.5 * np.std(my_points)
for p in my_points:
if abs(p - mean) > std:
# print p, mean
continue
if p < min_val:
min_val = p
if p > max_val:
max_val = p
return max_val - min_val
def _get_2d_points_stc(self, points_3d):
# xmin, ymin, zmin = self._get_top_left_point(points_3d)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 10, 1.0)
points_3d = np.float32(points_3d)
ret, label, centers = cv2.kmeans(np.array(points_3d), 2, criteria, 10, 0)
data = Counter(label.flatten())
max_label = data.most_common(1)[0][0]
c = centers[max_label]
xmin, ymin, zmin = c[0], c[1], c[2]
points_3d = [[xmin - .3, ymin - .3, zmin], [xmin + .3, ymin + .3, zmin],
[xmin - .3, ymin + .3, zmin], [xmin + .3, ymin - .3, zmin]]
points_2d = map(lambda x: self.camera_model.project3dToPixel(x), points_3d)
return points_2d
def _get_bounding_rect(self, points_2d, img):
xmin = np.inf
xmax = -np.inf
ymin = np.inf
ymax = -np.inf
h, w, r = img.shape
for i, point in enumerate(points_2d):
if(point[0] < xmin):
xmin = point[0]
if(point[0] > xmax):
xmax = point[0]
if(point[1] > ymax):
ymax = point[1]
if(point[1] < ymin):
ymin = point[1]
if xmin < 0:
xmin = 1
if ymin < 0:
ymin = 1
if xmax > w:
xmax = w - 1
if ymax > h:
ymax = h - 1
return xmin, ymin, xmax, ymax
@txros.util.cancellableInlineCallbacks
def get_stc_points(self, msg, stc_pos):
trans = yield self.my_tf.get_transform("/stereo_right_cam", "/velodyne", msg.header.stamp)
trans1 = yield self.my_tf.get_transform("/stereo_right_cam", "/enu", msg.header.stamp)
stc_pos = rosmsg_to_numpy(stc_pos)
stc_pos = np.append(stc_pos, 1)
position = trans1.as_matrix().dot(stc_pos)
if position[3] < 1E-15:
raise ZeroDivisionError
position[0] /= position[3]
position[1] /= position[3]
position[2] /= position[3]
position = position[:3]
stereo_points = []
for point in pc2.read_points(msg, skip_nans=True):
stereo_points.append(np.array([point[0], point[1], point[2], 1]))
stereo_points = map(lambda x: trans.as_matrix().dot(x), stereo_points)
points = []
for p in stereo_points:
if p[3] < 1E-15:
raise ZeroDivisionError
p[0] /= p[3]
p[1] /= p[3]
p[2] /= p[3]
points.append(p[:3])
points_keep = []
for p in points:
# print npl.norm(p - poition)
if npl.norm(p - position) < 20:
points_keep.append(p)
points_keep = sorted(points_keep, key=lambda x: x[1])
keep_num = int(.1 * len(points_keep))
points_keep = points_keep[:keep_num]
# self.pers_points.extend(points_keep)
# max_num = 200
# if len(self.pers_points) > max_num:
# self.pers_points = self.pers_points[len(self.pers_points) - max_num:len(self.pers_points)]
defer.returnValue(points_keep)
@txros.util.cancellableInlineCallbacks
def search(self, scan_the_code):
"""Search for the colors in the scan the code object."""
pntcloud = yield self.vel_sub.get_next_message()
image_ros = yield self.image_sub.get_next_message()
try:
image = self.bridge.imgmsg_to_cv2(image_ros, "bgr8")
except CvBridgeError:
print "Trouble converting image"
defer.returnValue((False, None))
points_3d = yield self.get_stc_points(pntcloud, scan_the_code.position)
image_clone = image.copy()
# points_3d = yield self._get_3d_points_stereo(scan_the_code.points, image_ros.header.stamp)
# points_2d = map(lambda x: self.camera_model.project3dToPixel(x), points_3d)
points_2d = map(lambda x: self.camera_model.project3dToPixel(x), points_3d)
for p in points_2d:
po = (int(round(p[0])), int(round(p[1])))
cv2.circle(image_clone, po, 2, (0, 255, 0), -1)
points_2d = self._get_2d_points_stc(points_3d)
xmin, ymin, xmax, ymax = self._get_bounding_rect(points_2d, image)
xmin, ymin, xmax, ymax = int(xmin), int(ymin), int(xmax), int(ymax)
cv2.rectangle(image_clone, (xmin, ymin), (xmax, ymax), (0, 255, 0), 2)
self.debug.add_image(image_clone, "bounding_box", topic="bounding_box")
# defer.returnValue((False, None))
print xmin, ymin, xmax, ymax
roi = image[ymin:ymax, xmin:xmax]
succ, color_vec = self.rect_finder.get_rectangle(roi, self.debug)
if not succ:
defer.returnValue((False, None))
self.mission_complete, colors = self.color_finder.check_for_colors(image, color_vec, self.debug)
if self.mission_complete:
print "MISSION COMPLETE"
defer.returnValue((True, colors))
defer.returnValue((False, None))
@txros.util.cancellableInlineCallbacks
def correct_pose(self, scan_the_code):
"""Check to see if we are looking at the corner of scan the code."""
self.count += 1
pntcloud = yield self.vel_sub.get_next_message()
points_3d = yield self.get_stc_points(pntcloud, scan_the_code.position)
xmin, ymin, zmin = self._get_top_left_point(points_3d)
points_oi = self._get_points_in_range('y', ymin - .1, ymin + .2, points_3d)
if len(points_oi) == 0:
defer.returnValue(False)
image_ros = yield self.image_sub.get_next_message()
image_ros = self.bridge.imgmsg_to_cv2(image_ros, "bgr8").copy()
depth = self._get_depth('z', points_oi)
fprint("DEPTH: {}".format(depth), msg_color="green")
self.depths.append(depth)
# %%%%%%%%%%%%%%%%%%%%%%%%DEBUG
cv2.putText(image_ros, str(depth), (10, 30), 1, 2, (0, 0, 255))
self.debug.add_image(image_ros, "in_range", topic="in_range")
# %%%%%%%%%%%%%%%%%%%%%%%%DEBUG
if depth > .10:
defer.returnValue(False)
defer.returnValue(True)
class BoundingBoxFilter():
def __init__(self, only_record):
print only_record
self.only_record = only_record
self.file = os.environ['HOME'] + '/filter_corners_' + str(rospy.Time.now().to_nsec()) + '.pickle'
self.lis = tf.TransformListener()
self.need_info = True
self.model = PinholeCameraModel()
rospy.Subscriber('/camera/rgb/camera_info', CameraInfo, self.info_callback)
while self.need_info:
rospy.sleep(0.01)
rospy.loginfo('Got CameraInfo! Proceeding to filter.')
self.have_projections = False
rospy.Subscriber('/object_vertex_projections', PointCloud, self.projections_callback)
self.bridge = CvBridge()
self.sub_image = rospy.Subscriber('/camera/rgb/image_color', Image, self.image_callback)#, queue_size=1)
self.image_pub = rospy.Publisher('/image_out', Image)
def info_callback(self, info):
if self.need_info:
self.model.fromCameraInfo(info)
self.need_info = False
def projections_callback(self, projections):
self.projections = projections
if not self.have_projections:
rospy.loginfo('Got projections! Can now begin filtering.')
self.have_projections = True
def image_callback(self, image):
if self.have_projections:
self.projections.header.stamp = image.header.stamp
try:
#self.lis.waitForTransform('/camera_rgb_optical_frame', self.projections.header.frame_id, image.header.stamp, rospy.Duration(1.0))
projections = self.lis.transformPointCloud('/camera_rgb_optical_frame', self.projections) # transform to camera frame
except Exception:
rospy.loginfo('Skipping image!')
return # skip this image; the filter probably hasn't caught up yet
corners = []
for point in projections.points: # project rays onto camera image plane
u, v = self.model.project3dToPixel((point.x,
point.y,
point.z))
corners.append((int(u), int(v)))
if self.only_record:
with open(self.file, 'a') as f:
pickle.dump((rospy.Time.now().to_time(), corners), f)
rospy.loginfo('RECORDED FILTER POINTS!')
else:
array = self.bridge.imgmsg_to_cv2(image, "bgr8")
for i in range(0, len(corners), 4):
cv2.rectangle(array, corners[i], corners[i+2], (0,0,255), 3)
#corners_convex = cv2.convexHull(numpy.array(corners)) # convex hull algorithm
#cv2.fillConvexPoly(array, corners_convex, (0, 0, 255)) # fill convex hull
#for [u, v] in corners:
# cv2.circle(array, (int(u), int(v)), 3, (255, 0, 0)) # draw circles at vertices for debugging
image_new = self.bridge.cv2_to_imgmsg(array, "bgr8")
image_new.header.stamp = rospy.Time.now()
self.image_pub.publish(image_new)
class LidarImageOverlay():
def __init__(self, cameraInfoFile):
""" Overlay lidar points onto images by setting up the camera model, camera intrinsics, and lidar-camera extrinsics.
===============
cameraInfoFile : an opened yaml file that stores camera intrinsics and other params
used to initialize the cameraInfo which is used to help cameraModel reproject lidar points to image space.
"""
self.cameraParams = yaml.load(cameraInfoFile)
self.cameraInfo = CameraInfo()
self._fillCameraInfo()
self.cameraModel = PinholeCameraModel()
self.cameraModel.fromCameraInfo(self.cameraInfo)
print('Distortion model:', self.cameraInfo.distortion_model)
self.bridge = cv_bridge.CvBridge()
# Get transformation/extrinsics between lidar (velodyne frame) and camera (world frame)
self.tf = tf.TransformListener()
# Get the topics' names to subscribe or publish
self.inImageName = rospy.resolve_name('image')
self.outImageName = rospy.resolve_name('lidar_image')
self.lidarName = rospy.remap_name('lidar')
# Create subscribers and publishers
self.subImage = rospy.Subscriber(self.inImageName,
Image,
callback=self.imageCallback,
queue_size=1)
self.lidar = rospy.Subscriber(self.lidarName,
PointCloud2,
callback=self.lidarCallback,
queue_size=1)
self.pubImage = rospy.Publisher(self.outImageName, Image, queue_size=1)
self.lidarPoints = None
def _fillCameraInfo(self, ):
""" Fill the camera params from yaml file to the sensor_msgs/CameraInfo
"""
self.cameraInfo.width = self.cameraParams['image_width']
self.cameraInfo.height = self.cameraParams['image_height']
self.cameraInfo.distortion_model = self.cameraParams[
'distortion_model']
self.cameraInfo.D = self.cameraParams['distortion_coefficients'][
"data"]
self.cameraInfo.R = self.cameraParams['rectification_matrix']["data"]
self.cameraInfo.P = self.cameraParams['projection_matrix']["data"]
self.cameraInfo.K = self.cameraParams['camera_matrix']["data"]
def imageCallback(self, data):
""" Once received an image, first get the lidar-camera transformation, then reproject the received lidar points to the camera frame (in pixel), finally draw the projected points on the received image and publish the modified image, i.e., the overlay lidar image.
============
data : a ros image message
- header (timestamp, frame_id, ...)
- data (Image)
...
"""
print('Received an image ...')
try:
cv_image = self.bridge.imgmsg_to_cv2(data, 'bgr8')
except cv_bridge.CvBridgeError as e:
rospy.logerr(
'Failed to convert received ros image msg to CvImage: ', e)
return
t, r = self.tf.lookupTransform('world', 'velodyne', rospy.Time(0))
t += (1, )
Rq = tf.transformations.quaternion_matrix(r)
Rq[:, 3] = t
if self.lidarPoints:
for point in self.lidarPoints:
try:
if np.sum(np.array(point[:3])**2) > 16:
continue
intensity = point[3]
except IndexError:
print(point)
break
if intensity < 1e-3:
continue
rotatedPoint = Rq.dot(point)
# if point is behind the camera, don't need to reproject
if rotatedPoint[2] < 0:
continue
# if the reprojected point lands in the image space, let's draw it on the current image
uv = self.cameraModel.project3dToPixel(rotatedPoint)
if uv[0] >= 0 and uv[0] < data.width and uv[1] >= 0 and uv[
1] < data.height:
# intensityI = (255-Int(intensity)) * 255 *255
cv2.circle(cv_image, (int(uv[0]), int(uv[1])), 2,
(0, 0, 180), 2)
else:
print('No lidar data received!')
try:
self.pubImage.publish(self.bridge.cv2_to_imgmsg(cv_image, 'bgr8'))
except cv_bridge.CvBridgeError as e:
rospy.logerr('Failed to convert CvImage to ros image message:', e)
return
def lidarCallback(self, data):
""" Unpack the raw lidar data to get the [x, y, z, intensity] formated points
============
data : a ros image message
- header (timestamp, frame_id, ...)
- data (PointCloud2)
...
"""
print('Received lidar data ...')
formatString = 'ffff'
if data.is_bigendian:
formatString = '>' + formatString
else:
formatString = '<' + formatString
self.lidarPoints = []
for i in range(0, len(data.data), 16):
self.lidarPoints.append(
struct.unpack(formatString, data.data[i:i + 16]))
print(len(self.lidarPoints))
class Colorama(object):
def __init__(self):
info_topic = camera_root + "/camera_info"
image_topic = camera_root + "/image_rect_color"
self.tf_listener = tf.TransformListener()
self.status_pub = rospy.Publisher("/database_color_status", ColoramaDebug, queue_size=1)
self.odom = None
set_odom = lambda msg: setattr(self, "odom", navigator_tools.pose_to_numpy(msg.pose.pose))
rospy.Subscriber("/odom", Odometry, set_odom)
fprint("Waiting for odom...")
while self.odom is None and not rospy.is_shutdown():
rospy.sleep(1)
fprint("Odom found!", msg_color='green')
db_request = rospy.ServiceProxy("/database/requests", ObjectDBQuery)
self.make_request = lambda **kwargs: db_request(ObjectDBQueryRequest(**kwargs))
self.image_history = ImageHistory(image_topic)
# Wait for camera info, and exit if not found
fprint("Waiting for camera info on: '{}'".format(info_topic))
while not rospy.is_shutdown():
try:
camera_info_msg = rospy.wait_for_message(info_topic, CameraInfo, timeout=3)
except rospy.exceptions.ROSException:
rospy.sleep(1)
continue
break
fprint("Camera info found!", msg_color="green")
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(camera_info_msg)
self.debug = DebugImage("/colorama/debug", prd=.5)
# These are color mappings from Hue values [0, 360]
self.color_map = {'red': np.radians(0), 'yellow': np.radians(60),
'green': np.radians(120), 'blue': np.radians(200)}
# RGB map for database setting
self.database_color_map = {'red': (255, 0, 0), 'yellow': (255, 255, 0), 'green': (0, 255, 0), 'blue': (0, 0, 255)}
# ========= Some tunable parameters ===================================
self.update_time = 1 # s
# Observation parameters
self.saturation_reject = 20 # Reject color obs with below this saturation
self.value_reject = 50 # Reject color obs with below this value
self.height_remove = 0.4 # Remove points that are this percent down on the object (%)
# 1 keeps all, .4 removes the bottom 40%
# Update parameters
self.history_length = 100 # How many of each color to keep
self.min_obs = 5 # Need atleast this many observations before making a determination
self.conf_reject = .5 # When to reject an observation based on it's confidence
# Confidence weights
self.v_factor = 0.6 # Favor value values close to the nominal value
self.v_u = 200 # Mean of norm for variance error
self.v_sig = 60 # Sigma of norm for variance error
self.dist_factor = 0.3 # Favor being closer to the totem
self.dist_sig = 30 # Sigma of distance (m)
self.q_factor = 0 # Favor not looking into the sun
self.q_sig = 1.2 # Sigma of norm for quaternion error (rads)
# Maps id => observations
self.colored = {}
rospy.Timer(ros_t(self.update_time), self.do_observe)
def _compute_average_angle(self, angles, weights):
"""
Returns weighted average of angles.
Tends to break down with too many angles 180 degrees of each other - try not to do that.
"""
angles = np.array(angles)
if np.linalg.norm(weights) == 0:
return None
w = weights / np.linalg.norm(weights)
s = np.sum(w * np.sin(angles))
c = np.sum(w * np.cos(angles))
avg_angle = np.arctan2(s, c)
return avg_angle
def _get_quaternion_error(self, q, target_q):
"""
Returns an angluar differnce between q and target_q in radians
"""
dq = trns.quaternion_multiply(np.array(target_q), trns.quaternion_inverse(np.array(q)))
return 2 * np.arccos(dq[3])
def _get_closest_color(self, hue_angle):
"""
Returns a pair of the most likely color and the radian error associated with that prediction
`hue_angle` := The radian value associated with hue [0, 2*pi]
Colors are found from `self.color_map`
"""
c = np.cos(hue_angle)
s = np.sin(hue_angle)
error = np.inf
likely_color = 'undef'
for color, h_val in self.color_map.iteritems():
cg = np.cos(h_val)
sg = np.sin(h_val)
# We need a signed error for some math later on so no absolute value
this_error = np.arctan2(sg*c - cg*s, cg*c + sg*s)
if np.abs(this_error) < np.abs(error):
error = this_error
likely_color = color
fprint("Likely color: {} with an hue error of {} rads.".format(likely_color, np.round(error, 3)))
return [likely_color, error]
def do_estimate(self, totem_id):
"""Calculates best color estimate for a given totem id"""
fprint("DOING ESTIMATE", msg_color='blue')
if totem_id not in self.colored:
fprint("Totem ID {} not found!".format(totem_id), msg_color='red')
return None
t_color = self.colored[totem_id]
if len(t_color) < self.min_obs:
fprint("Only {} observations. {} required.".format(len(t_color), self.min_obs), msg_color='red')
return None
kwargs = {'v_u': self.v_u, 'v_sig': self.v_sig, 'dist_sig': self.dist_sig,
'q_factor': self.q_factor, 'q_sig': self.q_sig}
w, weights = t_color.compute_confidence([self.v_factor, self.dist_factor, self.q_factor], True, **kwargs)
fprint("CONF: {}".format(w))
if np.mean(w) < self.conf_reject:
return None
hue_angles = np.radians(np.array(t_color.hues) * 2)
angle = self._compute_average_angle(hue_angles, w)
color = self._get_closest_color(angle)
msg = t_color.as_message
msg.id = totem_id
msg.confidence = w
msg.labels = ["value_errs", "dists", "q_diffs"]
msg.weights = weights
msg.color = colors[0]
msg.est_hues = angle * 2
msg.hues = np.array(t_color.hues) * 2
self.status_pub.publish(msg)
fprint("Color: {}".format(color[0]))
return color
def got_request(self, req):
# Threading blah blah something unsafe
colored_ids = [_id for _id, color_err in self.colored.iteritems() if self.valid_color(_id) == req.color]
fprint("Colored IDs: {}".format(colored_ids), msg_color='blue')
print '\n' * 50
if len(colored_ids) == 0:
return ColorRequestResponse(found=False)
return ColorRequestResponse(found=True, ids=colored_ids)
def do_observe(self, *args):
resp = self.make_request(name='totem')
# If atleast one totem was found start observing
if resp.found:
# Time of the databse request
time_of_marker = resp.objects[0].header.stamp# - ros_t(1)
fprint("Looking for image at {}".format(time_of_marker.to_sec()), msg_color='yellow')
image_holder = self.image_history.get_around_time(time_of_marker)
if not image_holder.contains_valid_image:
t = self.image_history.newest_image.time
if t is None:
fprint("No images found.")
return
fprint("No valid image found for t={} ({}) dt: {}".format(time_of_marker.to_sec(), t.to_sec(), (rospy.Time.now() - t).to_sec()), msg_color='red')
return
header = navigator_tools.make_header(frame='/enu', stamp=image_holder.time)
image = image_holder.image
self.debug.image = np.copy(image)
cam_tf = self.camera_model.tfFrame()
try:
fprint("Getting transform between /enu and {}...".format(cam_tf))
self.tf_listener.waitForTransform("/enu", cam_tf, time_of_marker, ros_t(1))
t_mat44 = self.tf_listener.asMatrix(cam_tf, header)
except tf.ExtrapolationException as e:
fprint("TF error found and excepted: {}".format(e))
return
for obj in resp.objects:
if len(obj.points) <= 1:
fprint("No points in object")
continue
fprint("{} {}".format(obj.id, "=" * 50))
# Get object position in px coordinates to determine if it's in frame
object_cam = t_mat44.dot(np.append(p2np(obj.position), 1))
object_px = map(int, self.camera_model.project3dToPixel(object_cam[:3]))
if not self._object_in_frame(object_cam):
fprint("Object not in frame")
continue
# Get enu points associated with this totem and remove ones that are too low
points_np = np.array(map(p2np, obj.points))
height = np.max(points_np[:, 2]) - np.min(points_np[:, 2])
if height < .1:
# If the height of the object is too small, skip (units are meters)
fprint("Object too small")
continue
threshold = np.min(points_np[:, 2]) + self.height_remove * height
points_np = points_np[points_np[:, 2] > threshold]
# Shove ones in there to make homogenous points to get points in image frame
points_np_homo = np.hstack((points_np, np.ones((points_np.shape[0], 1)))).T
points_cam = t_mat44.dot(points_np_homo).T
points_px = map(self.camera_model.project3dToPixel, points_cam[:, :3])
[cv2.circle(self.debug.image, tuple(map(int, p)), 2, (255, 255, 255), -1) for p in points_px]
# Get color information from the points
roi = self._get_ROI_from_points(points_px)
h, s, v = self._get_hsv_from_ROI(roi, image)
# Compute parameters that go into the confidence
boat_q = self.odom[1]
target_q = self._get_solar_angle()
q_err = self._get_quaternion_error(boat_q, target_q)
dist = np.linalg.norm(self.odom[0] - p2np(obj.position))
fprint("H: {}, S: {}, V: {}".format(h, s, v))
fprint("q_err: {}, dist: {}".format(q_err, dist))
# Add to database and setup debug image
if s < self.saturation_reject or v < self.value_reject:
err_msg = "The colors aren't expressive enough s: {} ({}) v: {} ({}). Rejecting."
fprint(err_msg.format(s, self.saturation_reject, v, self.value_reject), msg_color='red')
else:
if obj.id not in self.colored:
self.colored[obj.id] = Observation()
# Add this observation in
self.colored[obj.id] += h, v, dist, q_err
print self.colored[obj.id]
rgb = (0, 0, 0)
color = self.do_estimate(obj.id)
# Do we have a valid color estimate
if color:
fprint("COLOR IS VALID", msg_color='green')
rgb = self.database_color_map[color[0]]
cmd = '{name}={rgb[0]},{rgb[1]},{rgb[2]},{_id}'
self.make_request(cmd=cmd.format(name=obj.name,_id=obj.id, rgb=rgb))
bgr = rgb[::-1]
cv2.circle(self.debug.image, tuple(object_px), 10, bgr, -1)
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(self.debug.image, str(obj.id), tuple(object_px), font, 1, bgr, 2)
def _get_solar_angle(self):
return [0, 0, 0, 1]
def _get_ROI_from_points(self, image_points):
cnt = np.array([image_points]).astype(np.float32)
rect = cv2.minAreaRect(cnt)
box = cv2.cv.BoxPoints(rect)
box = np.int0(box)
fprint("Drawing rectangle")
cv2.drawContours(self.debug.image, [box], 0, (0, 0, 255), 2)
return box
def _get_hsv_from_ROI(self, roi, img):
mask = np.zeros(img.shape[:2], np.uint8)
cv2.drawContours(mask, [roi], 0, 255, -1)
bgr = cv2.mean(img, mask)
# We have to treat that bgr value as a 3d array to get cvtColor to work
bgr = np.array([[bgr]])[:, :3].astype(np.uint8)
h, s, v = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)[0, 0]
return h, s, v
# Now check that s and v are in a good range
if s < self.saturation_reject or v < self.value_reject:
err_msg = "The colors aren't expressive enough s: {} ({}) v: {} ({}). Rejecting."
fprint(err_msg.format(s, self.saturation_reject, v, self.value_reject), msg_color='red')
return None
# Compute hue error in SO2
hue_angle = np.radians(h * 2)
# Display the current values
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(self.debug.image, "h_val: {}".format(np.degrees(hue_angle)),
tuple(roi[1]), font, 1, (255, 255, 255), 2)
likely_color, error = self.get_closest_color(hue_angle)
if error > self.hue_error_reject:
fprint("Closest color was {} with an error of {} rads (> {}). Rejecting.".format(likely_color, np.round(error, 3),
self.hue_error_reject), msg_color='red')
return None
fprint("Likely color: {} with an hue error of {} rads.".format(likely_color, np.round(error, 3)))
return [likely_color, error]
def _object_in_frame(self, object_point):
"""
Returns if the object is in frame given the centroid of the object.
`object_point` should be in the camera_model's frame.
"""
if object_point[2] < 0:
return False
px = np.array(self.camera_model.project3dToPixel(object_point))
resoultion = self.camera_model.fullResolution()
return not (np.any([0, 0] > px) or np.any(px > resoultion))
def main():
'''
Description: This function captures data on a given object class and
instance by having the robot randomly pick a goal and then
make a route to that goal to take a picture of the object.
Input: None
Return: None
'''
# Set up command line arguments
parser = argparse.ArgumentParser(description='Fetch Data Capture')
parser.add_argument('-c',
'--class',
dest='class_name',
action='store',
help='object class name: mug, stapler, book, etc...')
parser.add_argument('-n',
'--num',
'--number',
dest='class_number',
action='store',
help='object number in data set: mug # 6, 7, 8, etc..')
args = parser.parse_args()
num_necessary_args = 5
if (len(sys.argv) > num_necessary_args):
print "Need to provide command line arguments, use \"-help\" to see examples."
exit()
# Initialize variables
class_name = args.class_name
instance_name = class_name + '_' + str(args.class_number)
rospy.init_node('data_collector', anonymous=True)
num_published_points = 4
sample_min_radius = .6
sample_max_radius = 1
max_spine_height = .386
min_spine_height = 0.00313
spine_offset = 0.0
# set starting_image_index back to 0 when doing a new object
starting_image_index = 0
desired_num_images = 80
# Relevant paths
results_path = "/home/mccallm/catkin_ws/src/lifelong_object_learning/data_captured/"
base_filepath = results_path + class_name + "/" + instance_name
image_filepath = results_path + class_name + "/" + instance_name + "/images/"
circle_image_filepath = results_path + class_name + "/" + instance_name + "/circle_images/"
image_data_filepath = results_path + class_name + "/" + instance_name + "/metadata/"
# Path dir creation
if not os.path.exists(results_path):
os.makedirs(results_path)
if not os.path.exists(base_filepath):
os.makedirs(base_filepath)
if not os.path.exists(image_filepath):
os.makedirs(image_filepath)
if not os.path.exists(circle_image_filepath):
os.makedirs(circle_image_filepath)
if not os.path.exists(image_data_filepath):
os.makedirs(image_data_filepath)
# ROS paths
image_topic = "/head_camera/rgb/image_rect_color"
camera_info_topic = "/head_camera/rgb/camera_info"
map_frame = "/map"
camera_frame = "/head_camera_rgb_optical_frame"
published_point_num_topic = "/object_point_num"
published_point_base_topic = "/object_point"
torso_movement_topic = "/torso_controller/follow_joint_trajectory"
head_movement_topic = "/head_controller/point_head"
node = Node(image_topic, camera_info_topic, camera_frame,
published_point_num_topic, published_point_base_topic,
torso_movement_topic, head_movement_topic,
num_published_points, max_spine_height, min_spine_height,
spine_offset)
count_pub = rospy.Publisher('data_capture_index', String, queue_size=10)
camera_model = PinholeCameraModel()
while node.camera_info is None: # wait for camera info
continue
camera_model.fromCameraInfo(node.camera_info)
while (len(node.points_registered) != node.num_published_points):
rospy.loginfo(str(len(node.points_registered)))
continue
# find center of object
x_center = 0.0
y_center = 0.0
z_center = 0.0
for i in range(node.num_published_points):
x_center += node.published_points[i][0]
y_center += node.published_points[i][1]
z_center += node.published_points[i][2]
x_center = x_center / node.num_published_points
y_center = y_center / node.num_published_points
z_center = z_center / node.num_published_points
rospy.loginfo("x center: " + str(x_center))
rospy.loginfo("y center: " + str(y_center))
rospy.loginfo("z center: " + str(x_center))
# send first goal
goalID = starting_image_index
num_images_captured = starting_image_index
position = node.sample_position(x_center, y_center, sample_max_radius,
sample_min_radius)
goal_x = position[0]
goal_y = position[1]
goal_theta = position[2]
rospy.loginfo("Goal is " + str(goal_x) + " " + str(goal_y) + " " +
str(goal_theta))
rospy.loginfo("Sending goal")
node.move_base_to(goal_x, goal_y, goal_theta)
g_status = GoalStatus()
image_file_index = starting_image_index
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
if (node.base_client.get_state() == g_status.PREEMPTED) or (
(node.base_client.get_state() == g_status.ABORTED) or
(node.base_client.get_state() == g_status.REJECTED)):
position = node.sample_position(x_center, y_center,
sample_max_radius,
sample_min_radius)
goal_x = position[0]
goal_y = position[1]
goal_theta = position[2]
count_pub.publish("New goal ID is " + str(goalID))
rospy.loginfo("New goal ID is " + str(goalID))
rospy.loginfo("Goal is " + str(goal_x) + " " + str(goal_y) + " " +
str(goal_theta))
rospy.loginfo("Sending goal")
node.move_base_to(goal_x, goal_y, goal_theta)
# when base reaches position, adjust spine and point camera at object
if (node.base_client.get_state() == g_status.SUCCEEDED):
# adjust spine height
spine_height = position[3]
result = node.move_torso(spine_height)
rospy.loginfo("Adjusting spine")
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Spine adjustment succeeded")
# make robot look at object
rospy.sleep(1)
rospy.loginfo("Turning head")
node.look_at("/map", x_center, y_center, z_center)
result = node.point_head_client.wait_for_result()
rospy.loginfo(result)
if result == True:
rospy.loginfo("Head turn succeeded")
rospy.sleep(.1)
# capture and save image
img_cur = node.get_img()
rospy.sleep(.1)
if (img_cur is not None) and (
len(node.points_registered)
== node.num_published_points):
rospy.loginfo("Capturing image")
# find pixel coordinates of points of interest
# tl, tr, bl, br
ref_points = node.published_points
height, width, channels = img_cur.shape
points_to_write = []
for ref_point in ref_points:
ps = PointStamped()
ps.header.frame_id = map_frame
ps.header.stamp = node.tf.getLatestCommonTime(
camera_frame, ps.header.frame_id)
ps.point.x = ref_point[0]
ps.point.y = ref_point[1]
ps.point.z = ref_point[2]
ps_new = node.tf.transformPoint(
camera_frame, ps)
(u, v) = camera_model.project3dToPixel(
(ps_new.point.x, ps_new.point.y,
ps_new.point.z))
points_to_write.append(
[int(round(u)),
int(round(v))])
image_file = image_filepath + instance_name + \
"_" + str(image_file_index) + '.png'
circle_image_file = circle_image_filepath + \
instance_name + "_" + str(image_file_index) + '.png'
text_file = image_data_filepath + instance_name + \
"_" + str(image_file_index) + '.txt'
f = open(text_file, 'w')
f.write(image_file + "\n")
f.write(str(height) + "\t" + str(width) + "\n")
for point in points_to_write:
f.write(str(point[0]) + "\t")
f.write(str(point[1]) + "\n")
f.write(str(goal_x) + "\n")
f.write(str(goal_y) + "\n")
f.write(str(position[3]) + "\n") # spine height
f.close()
circle_img = img_cur.copy()
# visualize
for point in points_to_write:
cv2.circle(circle_img, (point[0], point[1]), 2,
(0, 0, 255), 3)
cv2.imwrite(circle_image_file, circle_img)
cv2.imwrite(image_file, img_cur)
image_file_index += 1
rospy.loginfo("Metadata and Image saved")
rospy.loginfo("Num images captured: " +
str(image_file_index))
# update goal id
goalID += 1
num_images_captured += 1
# Send next position
position = node.sample_position(x_center, y_center,
sample_max_radius,
sample_min_radius)
goal_x = position[0]
goal_y = position[1]
goal_theta = position[2]
# exit if we have tried all positions
if num_images_captured == desired_num_images:
rospy.loginfo("Total images captured: " +
str(image_file_index))
return
# move to next position
count_pub.publish("New goal ID is " + str(goalID))
rospy.loginfo("New goal ID is " + str(goalID))
rospy.loginfo("Goal is " + str(goal_x) + " " + str(goal_y) + " " +
str(goal_theta))
rospy.loginfo("Sending goal...")
node.move_base_to(goal_x, goal_y, goal_theta)
rate.sleep()
class turtleViz():
def __init__(self, topic, frames, polygons):
# The frames and shapes we want to blur
self.frameLocations = frames
self.polygonLocations = polygons
# To make the projection to a pixel
self.camModel = PinholeCameraModel()
# To look up the transforms between our points and the camera
self.listener = tf.TransformListener()
# Alex's image stuff?
self.rcv = rcv()
# Our program is driven by this callback
rospy.Subscriber(topic, Image, self.image_callback)
# To get which frame is the camera frame
rospy.Subscriber("/camera/rgb/camera_info", CameraInfo, self.camera_callback)
# A topic for the edited image data
self.pub = rospy.Publisher('turtleVision', Image)
def camera_callback(self, cameraInfo):
self.cameraInfo = cameraInfo
self.camModel.fromCameraInfo(cameraInfo)
def image_callback(self, image_in):
image_cv2 = self.rcv.toCv2(image_in)
framesList = []
with open(self.frameLocations, 'rb') as csvfile:
reader = csv.reader(csvfile, delimiter=',', quotechar="'")
for row in reader:
# Make a list of the frames
framesList.append(row[0])
for frame in framesList:
offset = self.polygonLocations + frame + ".csv"
polyPoints = []
# For visualization of polygons in rviz
polygon = PolygonStamped()
polygonPub = rospy.Publisher('polygons/' + frame, PolygonStamped)
# Polygons in rviz are relative to camera frame
polygon.header.frame_id = "camera_rgb_optical_frame"
with open(offset, 'rb') as csvfile:
reader = csv.reader(csvfile, delimiter=',', quotechar="'")
for row in reader:
newPolyPoint = PointStamped()
newPolyPoint.header.frame_id = frame
newPolyPoint.point.x = float(row[0])
newPolyPoint.point.y = float(row[1])
newPolyPoint.point.z = float(row[2])
# We don't care about orientation
try:
#Transform the point to our published privacy frame
transformedPolyPoint = self.listener.transformPoint(self.cameraInfo.header.frame_id, newPolyPoint)
# If the point is behind the camera, don't project it or add it to the polygon
if not transformedPolyPoint.point.z <= 0:
# Get the pixel from that transformed point and append it to the list of points
positionPoint = (transformedPolyPoint.point.x, transformedPolyPoint.point.y, transformedPolyPoint.point.z)
projectedPoint = self.camModel.project3dToPixel(positionPoint)
polyPoints.append(projectedPoint)
# For rviz
polygon.polygon.points.append(Point32(transformedPolyPoint.point.x, transformedPolyPoint.point.y, transformedPolyPoint.point.z))
except():
pass
#rospy.loginfo(polyPoints)
#pointArray = numpy.asarray(polyPoints, numpy.int32)
# Only draw the shape if it has more than 2 points
if len(polyPoints) > 2:
hullPoints = self.convexHull(polyPoints)
pointArray = numpy.asarray(hullPoints, numpy.int32)
#pointArray2 = numpy.asarray(polyPoints, numpy.int32)
rospy.loginfo(pointArray)
#rospy.loginfo(pointArray2)
image_cv2 = self.rcv.redactPolygon(image_cv2, pointArray)
# For rviz
polygonPub.publish(polygon)
# Convert back to ROS Image msg
image_out = self.rcv.toRos(image_cv2)
self.pub.publish(image_out)
self.rcv.imshow(image_cv2)
# Get the convex hull of a set of points
def convexHull(self, points):
cv = ConvexHull(points)
hull_indices = cv.vertices
hull_points = [points[i] for i in hull_indices]
return hull_points
class LidarToImage(object):
def __init__(self, nh, training=False, classes=None, dist=50):
self.MAX_SIZE = 74
self.IMAGE_SIZE = 100
self.max_dist = dist
self.bridge = CvBridge()
self.nh = nh
self.id_to_perist = {}
self.image_cache = deque()
self.pose = None
self.training = training
self.image = None
self.classes = classes
self.cam_info = None
self.busy = False
self.c = 0
self.debug = Debug(nh)
@util.cancellableInlineCallbacks
def init_(self, cam="r"):
image_sub = "/stereo/right/image_rect_color"
self.tf_frame = "/stereo_right_cam"
cam_info = "/stereo/right/camera_info"
if cam == 'l':
image_sub = "/stereo/left/image_rect_color"
self.tf_frame = "/stereo_left_cam"
cam_info = "/stereo/left/camera_info"
if cam == 'rr':
image_sub = "/right/right/image_rect_color"
self.tf_frame = "/right_right_cam"
cam_info = "/right/right/camera_info"
yield self.nh.subscribe(image_sub, Image, self._img_cb)
self._database = yield self.nh.get_service_client('/database/requests', ObjectDBQuery)
self._odom_sub = yield self.nh.subscribe('/odom', Odometry,
lambda odom: setattr(self, 'pose', nt.odometry_to_numpy(odom)[0]))
self.cam_info_sub = yield self.nh.subscribe(cam_info, CameraInfo, self._info_cb)
self.tf_listener = tf.TransformListener(self.nh)
defer.returnValue(self)
def _info_cb(self, info):
self.cam_info = info
@util.cancellableInlineCallbacks
def get_all_object_rois(self):
req = ObjectDBQueryRequest()
req.name = 'all'
obj = yield self._database(req)
if obj is None or not obj.found:
defer.returnValue((None, None))
rois = []
ros_img = yield self._get_closest_image(obj.objects[0].header.stamp)
if ros_img is None:
defer.returnValue((None, None))
img = self.bridge.imgmsg_to_cv2(ros_img, "mono8")
for o in obj.objects:
if o.id not in self.id_to_perist:
self.id_to_perist[o.id] = []
ppoints = self.id_to_perist[o.id]
ppoints.extend(o.points)
if len(ppoints) > 500:
ppoints = ppoints[:500]
if self.training and o.name not in self.classes:
continue
position = yield self._get_position()
o_pos = nt.rosmsg_to_numpy(o.position)
diff = np.linalg.norm(position - o_pos)
if diff > self.max_dist:
continue
points, bbox = yield self._get_bounding_box_2d(ppoints, obj.objects[0].header.stamp)
if bbox is None:
continue
xmin, ymin, xmax, ymax = bbox
h, w, r = img.shape
if xmin < 0 or xmax < 0 or xmin > w or xmax > w or xmax - xmin == 0 or ymax - ymin == 0:
continue
if ymin < 0:
ymin = 0
print "bbox", bbox
roi = img[ymin:ymax, xmin:xmax]
print "preshape", roi.shape
roi = self._resize_image(roi)
print "postshape", roi.shape
ret_obj = {}
ret_obj["id"] = o.id
ret_obj["points"] = points
ret_obj["img"] = roi
ret_obj["time"] = o.header.stamp
ret_obj["name"] = o.name
ret_obj["bbox"] = [xmin, ymin, xmax, ymax]
rois.append(ret_obj)
defer.returnValue((img, rois))
def _img_cb(self, image_ros):
"""Add an image to the image cache."""
self.image = image_ros
if len(self.image_cache) > 100:
self.image_cache.popleft()
self.image_cache.append(image_ros)
@util.cancellableInlineCallbacks
def _get_position(self):
last_odom_msg = yield self._odom_sub.get_next_message()
defer.returnValue(nt.odometry_to_numpy(last_odom_msg)[0][0])
@txros.util.cancellableInlineCallbacks
def _get_bounding_box_2d(self, points_3d_enu, time):
if self.cam_info is None:
defer.returnValue((None, None))
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(self.cam_info)
points_2d = []
try:
trans = yield self.tf_listener.get_transform(self.tf_frame, "/enu", time)
except Exception:
defer.returnValue((None, None))
transformation = trans.as_matrix()
for point in points_3d_enu:
point = nt.rosmsg_to_numpy(point)
point = np.append(point, 1.0)
t_p = transformation.dot(point)
if t_p[3] < 1E-15:
defer.returnValue((None, None))
t_p[0] /= t_p[3]
t_p[1] /= t_p[3]
t_p[2] /= t_p[3]
t_p = t_p[0:3]
if t_p[2] < 0:
defer.returnValue((None, None))
point_2d = self.camera_model.project3dToPixel(t_p)
points_2d.append((int(point_2d[0]), int(point_2d[1])))
xmin, ymin = sys.maxint, sys.maxint
xmax, ymax = -sys.maxint, -sys.maxint
for i, point in enumerate(points_2d):
if point[0] < xmin:
xmin = point[0]
if point[0] > xmax:
xmax = point[0]
if point[1] < ymin:
ymin = point[1]
if point[1] > ymax:
ymax = point[1]
defer.returnValue((points_2d, (xmin, ymin, xmax, ymax)))
@util.cancellableInlineCallbacks
def _get_closest_image(self, time):
min_img = None
for i in range(0, 20):
min_diff = genpy.Duration(sys.maxint)
for img in self.image_cache:
diff = abs(time - img.header.stamp)
if diff < min_diff:
min_diff = diff
min_img = img
print min_diff.to_sec()
if min_img is not None:
defer.returnValue(min_img)
yield self.nh.sleep(.3)
def _resize_image(self, img):
h, w, r = img.shape
if h > w:
nh = self.MAX_SIZE
nw = nh * w / h
else:
nw = self.MAX_SIZE
nh = nw * h / w
img = cv2.resize(img, (nw, nh))
# return img
rep = np.ones(nw, dtype=np.int64)
reph = np.ones(nh, dtype=np.int64)
emtpy_slots = self.IMAGE_SIZE - nw
empty_slots_h = self.IMAGE_SIZE - nh
half_empty_slots = emtpy_slots / 2 + 1
half_empty_slots_h = empty_slots_h / 2 + 1
reph[0] = half_empty_slots_h
reph[-1] = half_empty_slots_h
rep[0] = half_empty_slots
rep[-1] = half_empty_slots
if emtpy_slots % 2 == 1:
rep[-1] += 1
if empty_slots_h % 2 == 1:
reph[-1] += 1
img = np.repeat(img, reph, axis=0)
return np.repeat(img, rep, axis=1)
center38_T_cam37 = np.matmul(w_T_cam37, center38_T_w)
center35_T_cam37 = np.matmul(w_T_cam37, center35_T_w)
# create Transforms for bounding box corners
corners35 = get_corners(center35_T_cam37)
corners38 = get_corners(center38_T_cam37)
# convert to OpenCV image
car37_image = bridge.imgmsg_to_cv2(cam_37[idxImg][1], "bgr8")
# find pixel coordinates of centroids
# the Camera Pinhole model uses +x right, +y down, +z forward
center35_3d = (-center35_T_cam37[1,3], -center35_T_cam37[2,3], center35_T_cam37[0,3])
center38_3d = (-center38_T_cam37[1,3], -center38_T_cam37[2,3], center38_T_cam37[0,3])
camModel.fromCameraInfo(camInfo)
center35_2d = camModel.project3dToPixel(center35_3d)
center38_2d = camModel.project3dToPixel(center38_3d)
center35_round = (int(center35_2d[0]), int(center35_2d[1]))
center38_round = (int(center38_2d[0]), int(center38_2d[1]))
# only draw on frame if the observed robot is behind the camera
car35_rect = None
if center35_3d[2] > 0:
# draw centroid
cv2.circle(car37_image, center35_round, 3, (0,255,0), 2)
# initialize rectangle bounds to image size
# cv's image.shape is formatted as (height, width, length) a.k.a. (y, x, z)
xmin = car37_image.shape[1] + 1
xmax = -1
class ParseOpenFace:
def __init__(self):
print "in init"
self.image_geo = None
rospy.Subscriber("faces", Faces, self.openface_callback)
#rospy.Subscriber("beliefs/features", PcFeatureArray, self.hlpr_callback)
self.camera_info_sub = rospy.Subscriber("kinect/qhd/camera_info",
CameraInfo,
self.camera_info_callback)
#self.yolo_sub = rospy.Subscriber("detector_node/detections", Detections, self.yolo_callback)
self.line_pub = rospy.Publisher("gaze_gui_line", Lines)
self.obj_bridge_pub = rospy.Publisher("object_detector_bridge",
GazeTopic)
self.camera_init_done = False
self.gaze_x, self.gaze_y, self.gaze_z = None, None, None
self.hlpr_objects, self.yolo_objects = None, None
self.mutual = False
print "finished init"
def camera_info_callback(self, msg):
self.image_geo = PinholeCameraModel()
self.image_geo.fromCameraInfo(msg)
self.camera_init_done = True
self.camera_info_sub.unregister()
def hlpr_callback(self, msg):
self.hlpr_objects = msg.objects
def yolo_callback(self, msg):
self.yolo_objects = msg.objects
# rotate vector v1 by quaternion q1
def qv_mult_ros(self, q1, v1):
v1 = tf.transformations.unit_vector(v1)
q2 = list(v1)
q2.append(0.0)
return tf.transformations.quaternion_multiply(
tf.transformations.quaternion_multiply(q1, q2),
tf.transformations.quaternion_conjugate(q1))[:3]
def q_mult(self, q1, q2):
#print('q1')
#print(q1)
w1, x1, y1, z1 = q1
w2, x2, y2, z2 = q2
w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
return w, x, y, z
def q_conjugate(self, q):
w, x, y, z = q
return (w, -x, -y, -z)
def qv_mult(self, q1, v1):
q2 = (0.0, ) + v1
return self.q_mult(self.q_mult(q1, q2), self.q_conjugate(q1))[1:]
def normalize_q(self, v, tolerance=0.00001):
mag2 = sum(n * n for n in v)
if abs(mag2 - 1.0) > tolerance:
mag = math.sqrt(mag2)
v = tuple(n / mag for n in v)
return v
def axisangle_to_q(self, v, theta):
v = self.normalize_q(v)
x, y, z = v
theta /= 2
w = math.cos(theta)
x = x * math.sin(theta)
y = y * math.sin(theta)
z = z * math.sin(theta)
return w, x, y, z
def q_to_axisangle(self, q):
w, v = q[0], q[1:]
theta = math.acos(w) * 2.0
return self.normalize_q(v), theta
def normalize(self, vector):
norm_factor = math.sqrt(sum(map(lambda x: x * x, vector)))
norm_vector = map(lambda x: x / norm_factor, vector)
return norm_vector
def openface_callback(self, msg):
if len(msg.faces) > 0:
left, right = msg.faces[0].left_gaze, msg.faces[0].right_gaze
print('left, right: ', left, right)
head_msg = msg.faces[0].head_pose.position
print('head_msg: ', head_msg)
head_orient = msg.faces[0].head_pose.orientation
self.mutual_value = 0
#(msg.faces[0].landmarks_3d[36])
left_eye_corner1 = msg.faces[0].landmarks_3d[36]
left_eye_corner2 = msg.faces[0].landmarks_3d[39]
left_eye_center = [(left_eye_corner1.x + left_eye_corner2.x) / 2.0,
(left_eye_corner1.y + left_eye_corner2.y) / 2.0,
(left_eye_corner1.z + left_eye_corner2.z) / 2.0]
left_eye_center = self.normalize(left_eye_center)
right_eye_corner1 = msg.faces[0].landmarks_3d[42]
right_eye_corner2 = msg.faces[0].landmarks_3d[45]
right_eye_center = [
(right_eye_corner1.x + right_eye_corner2.x) / 2.0,
(right_eye_corner1.y + right_eye_corner2.y) / 2.0,
(right_eye_corner1.z + right_eye_corner2.z) / 2.0
]
right_eye_center = self.normalize(right_eye_center)
# normalized averaged eye gaze vector
sum_gaze = [(left.x + right.x) / 2.0, (left.y + right.y) / 2.0,
(left.z + right.z) / 2.0]
self.avg_gaze_norm = self.normalize(sum_gaze)
self.avg_gaze = map(lambda x: x * 1000,
self.avg_gaze_norm) #####SCALING FACTOR
#normalize head pose vector
self.head_pose = [head_msg.x, head_msg.y, head_msg.z]
head_pose_norm = self.normalize(self.head_pose)
# normalized gaze vector with respect to camera
gaze_camera = [(head_pose_norm[0] + sum_gaze[0]),
(head_pose_norm[1] + sum_gaze[1]),
(head_pose_norm[2] + sum_gaze[2])]
gaze_camera_norm = self.normalize(gaze_camera)
# normalized head to camera vector
head_camera = [
head_pose_norm[0], head_pose_norm[1], head_pose_norm[2]
]
head_camera_norm = self.normalize(head_camera)
# get left eye gaze vector oriented in camera frame
#rotation = self.q_conjugate((head_orient.w,head_orient.x,head_orient.y,head_orient.z))
#rotation = (head_orient.w,-head_orient.x,-head_orient.y,-head_orient.z)
#rotation = self.normalize_q(rotation)
print('head_orientation: ', head_orient)
v, theta = self.q_to_axisangle(
(head_orient.w, head_orient.x, head_orient.y, head_orient.z))
print('quaternion axis, angle: ', v, math.degrees(theta))
rotation = self.axisangle_to_q(v, -theta)
left_camera_frame = self.qv_mult(rotation,
(left.x, left.y, left.z))
left_camera_frame = self.normalize(left_camera_frame)
print('left eye camera frame: ', left_camera_frame)
# get right eye gaze vector oriented in camera frame
right_camera_frame = self.qv_mult(rotation,
(right.x, right.y, right.z))
right_camera_frame = self.normalize(right_camera_frame)
print('right eye camera frame: ', right_camera_frame)
# average and normalize eye vectors
gaze_combined = [(left_camera_frame[0]+right_camera_frame[0])/2.0, \
(left_camera_frame[1]+right_camera_frame[1])/2.0, \
(left_camera_frame[2]+right_camera_frame[2])/2.0]
gaze_camera_frame = self.normalize(gaze_combined)
dot_product = (head_camera_norm[0] * gaze_camera_frame[0]) + (
head_camera_norm[1] * gaze_camera_frame[1]) + (
head_camera_norm[2] * gaze_camera_frame[2])
#print(math.degrees(math.acos(dot_product)))
dot_product_left = (left_eye_center[0] * left_camera_frame[0]) + (
left_eye_center[1] * left_camera_frame[1]) + (
left_eye_center[2] * left_camera_frame[2])
#print(math.degrees(math.acos(dot_product_left)))
dot_product_right = (
right_eye_center[0] * right_camera_frame[0]) + (
right_eye_center[1] * right_camera_frame[1]) + (
right_eye_center[2] * right_camera_frame[2])
#print(math.degrees(math.acos(dot_product_right)))
avg_angle = (math.degrees(math.acos(dot_product_left)) +
math.degrees(math.acos(dot_product_right))) / 2.0
print(45 - avg_angle)
if (45 - avg_angle) < -20:
self.mutual = True # angle between head vector and gazze vector is less than 45 degrees
else:
self.mutual = False
print self.mutual
self.mutual_value = avg_angle
#print('debug')
#print(self.head_pose)
#print(self.avg_gaze)
self.head_gaze_pose = np.add(self.head_pose,
self.avg_gaze).tolist()
self.find_nearest_object()
self.publish_nearest_obj()
def find_nearest_object(self):
if not (self.camera_init_done or self.hlpr_objects
or self.yolo_objects):
return
# scaling_factor = 1500
# self.avg_gaze = map(lambda x: x*scaling_factor, self.avg_gaze)
def publish_nearest_obj(self):
#if not (self.hlpr_objects or self.yolo_objects or len(self.hlpr_objects)>0):
# return
# min_center = self.find_closest_hlpr_cluster().bb_center
# print min_center # DEBUG
# obj_x, obj_y = self.image_geo.project3dToPixel((min_center.x, min_center.y, min_center.z))
#yolo_object = self.find_closest_yolo_obj()
#print('debug2')
#print(self.head_gaze_pose)
gaze_x, gaze_y = self.image_geo.project3dToPixel(self.head_gaze_pose)
head_x, head_y = self.image_geo.project3dToPixel(self.head_pose)
prediction = [gaze_x, gaze_y, head_x, head_y]
self.publish_object_bridge(None, prediction)
def publish_object_bridge(self, yolo_obj, prediction):
mutual_bool = Bool(data=self.mutual)
#nearest_object_msg = String(data=yolo_obj.label)
coordinate_array = Float32MultiArray(data=prediction)
val = Float32(self.mutual_value)
msg_topic = GazeTopic(nearest_object=None,
mutual=mutual_bool,
coordinates=coordinate_array,
mutual_value=val)
self.obj_bridge_pub.publish(msg_topic)
# print yolo_obj.label # DEBUG
# def find_closest_hlpr_cluster(self):
# min_diff = float('inf')
# min_item = None
# for item in self.hlpr_objects: # Find closest hlpr object to gaze vector
# center = np.array([item.bb_center.x,item.bb_center.y,item.bb_center.z])
# l1 = np.array(self.head_pose)
# l2 = center
# p = np.array(self.avg_gaze)
# # diff = np.linalg.norm(np.cross(l2-l1, p))/np.linalg.norm(p)
# diff = np.linalg.norm(np.cross(l2-l1, p-l1))/np.linalg.norm(l2-l1)
# # diff = (self.avg_gaze[0] - center[0])**2 + (self.avg_gaze[1] - center[1])**2 + (self.avg_gaze[2] - center[2])**2
# if diff<min_diff:
# min_diff = diff
# min_item = item
# min_l1, min_l2, min_p = l1, l2, p
# head_x, head_y = self.image_geo.project3dToPixel(self.head_pose)
# print "min l2 {0}".format(min_l2)
# print "min l1 {0}".format(min_l1)
# print "min diff {0}".format(min_l2-min_l1)
# l1_x, l1_y = self.image_geo.project3dToPixel(min_l2-min_l1)
# line1 = Float32MultiArray(data=[head_x,head_y,l1_x+head_x, l1_y+head_y])
# l2_x, l2_y = self.image_geo.project3dToPixel(min_l1 - 1000*min_p)
# line2 = Float32MultiArray(data=[head_x,head_y,l2_x, l2_y])
# self.line_pub.publish([line1, line2])
# return min_item
def find_closest_yolo_obj(self):
min_diff = float('inf')
yolo_obj = None
head_x, head_y = self.image_geo.project3dToPixel(self.head_pose)
head_gaze_x, head_gaze_y = self.image_geo.project3dToPixel(
self.head_gaze_pose)
for yolo_object in self.yolo_objects:
if yolo_object.label != "spoon":
head = np.array([head_x, head_y])
head_gaze = np.array([head_gaze_x, head_gaze_y])
obj = np.array(
[yolo_object.centroid_x, yolo_object.centroid_y])
# diff = np.linalg.norm(np.cross(l2-l1, p))/np.linalg.norm(p)
# diff = np.linalg.norm(np.cross(l2-l1, l1-p))/np.linalg.norm(l2-l1)
diff = np.linalg.norm(
np.cross(head_gaze - head,
head - obj)) / np.linalg.norm(head_gaze - head)
# diff = (self.avg_gaze[0] - center[0])**2 + (self.avg_gaze[1] - center[1])**2 + (self.avg_gaze[2] - center[2])**2
if diff < min_diff:
min_diff = diff
yolo_obj = yolo_object
# min_l1, min_l2, min_p = l1, l2, p
# print "min l2 {0}".format(min_l2)
# print "min l1 {0}".format(min_l1)
# print "min diff {0}".format(min_l2-min_l1)
line1 = Float32MultiArray(data=[0, 0, 0, 0])
line2 = Float32MultiArray(data=[
head_x, head_y, head_x + 100 * (head_gaze_x - head_x), head_y +
100 * (head_gaze_y - head_y)
])
self.line_pub.publish([line1, line2])
return yolo_obj
class Self_Supervised(object):
def __init__(self, cam):
#topic_name = '/hsrb/head_rgbd_sensor/depth_registered/image_raw'
not_read = True
while not_read:
try:
cam_info = cam.read_info_data()
if (not cam_info == None):
not_read = False
except:
rospy.logerr('info not recieved')
self.pcm = PCM()
self.cam = cam
self.pcm.fromCameraInfo(cam_info)
self.br = tf.TransformBroadcaster()
self.tl = tf.TransformListener()
def debug_images(self, pose):
c_img = self.cam.read_color_data()
dp = DrawPrediction()
image = dp.draw_prediction(np.copy(c_img), pose)
cv2.imshow('debug', image)
cv2.waitKey(30)
#IPython.embed()
def get_current_head_position(self):
pose = self.tl.lookupTransform('rgbd_sensor_rgb_frame_map', 'map',
rospy.Time(0))
M = tf.transformations.quaternion_matrix(pose[1])
M_t = tf.transformations.translation_matrix(pose[0])
M[:, 3] = M_t[:, 3]
return M
def compute_transform(self, M_g):
M_curr = self.get_current_head_position()
M_d = np.matmul(M_curr, LA.inv(self.M_base))
M_T = np.matmul(M_d, M_g)
trans = M_T[0:3, 3]
return trans
def search_head(self, whole_body, o_p, o_t):
whole_body.move_to_joint_positions({'head_pan_joint': self.c_p + o_p})
whole_body.move_to_joint_positions({'head_tilt_joint': self.c_t + o_t})
time.sleep(cfg.SS_TIME)
def learn(self, whole_body, grasp_count, cam=None):
self.d_points = []
self.M_base = self.get_current_head_position()
self.c_p = whole_body.joint_state.position[9]
self.c_t = whole_body.joint_state.position[10]
change = np.linspace(-0.05, 0.05, num=cfg.NUM_SS_DATA)
for c_t in change:
for c_p in change:
self.search_head(whole_body, c_p, c_t)
M_g = self.look_up_transform(grasp_count)
transform = M_g[0:3, 3]
pose = self.pcm.project3dToPixel(transform)
self.add_data_point(pose)
self.debug_images(pose)
return self.d_points
def add_data_point(self, pose):
d_point = {}
d_point['c_img'] = self.cam.read_color_data()
d_point['d_img'] = self.cam.read_depth_data()
d_point['pose'] = pose
self.d_points.append(d_point)
def look_up_transform(self, count):
transforms = self.tl.getFrameStrings()
for transform in transforms:
current_grasp = 'bed_i_' + str(count)
if current_grasp in transform:
print 'got here'
pose = self.tl.lookupTransform('rgbd_sensor_rgb_frame_map',
transform, rospy.Time(0))
M = tf.transformations.quaternion_matrix(pose[1])
M_t = tf.transformations.translation_matrix(pose[0])
M[:, 3] = M_t[:, 3]
return M
class LidarImage:
def __init__( self ):
self._imageInput = rospy.Subscriber( '/stereo_camera/image_rect_color', Image, callback = self.imageCallback, queue_size = 10 )
self._camera = rospy.Subscriber( '/stereo_camera/camera_info', CameraInfo, callback = self.cameraCallback, queue_size = 10 )
self._velodyne = rospy.Subscriber( '/velodyne_obstacles', PointCloud2, callback = self.velodyneCallback, queue_size = 20 )
self._imageOutput = rospy.Publisher( 'image_overlay', Image, queue_size = 10 )
self.pub_image_coordinates = rospy.Publisher('image_coordinates', Vector3, queue_size = 10)
self.pcl_pub = rospy.Publisher("/pointcloud_visual", PointCloud2,queue_size = 1000)
self.marker_publisher = rospy.Publisher('visualization_marker', Marker, queue_size=5)
self._bridge = cv_bridge.CvBridge()
self._cameraModel = PinholeCameraModel()
self._tf = tf.TransformListener()
self.point_list = []
self.image_coordinates=Vector3()
def imageCallback( self, data ):
print( 'Received an image!' )
cvImage = {}
try:
cvImage = self._bridge.imgmsg_to_cv2( data, 'bgr8' )
except cv_bridge.CvBridgeError as e:
rospy.logerr( '[lidar_image] Failed to convert image' )
rospy.logerr( '[lidar_image] ' + e )
print( e )
return
( translation, rotation ) = self._tf.lookupTransform( '/velodyne_link', '/camera_link', rospy.Time( 0 ) )
translation = tuple(translation) + ( 1, )
Rq = tf.transformations.quaternion_matrix( rotation )
Rq[ :, 3 ] = translation
if self._velodyneData:
for i in range( 0, len( self._velodyneData ) - 1 ):
try:
point = self._velodyneData[ i ][ :3 ] + ( 1, )
if math.sqrt( np.sum( np.array( point[ :3 ] ) ** 2 ) ) > 9:
continue
except IndexError:
break
self.point_list.append([self._velodyneData[i][0], self._velodyneData[i][1], self._velodyneData[i][2]])
rotatedPoint = Rq.dot( point )
uv = self._cameraModel.project3dToPixel( rotatedPoint )
if uv[ 0 ] >= 0 and uv[ 0 ] <= data.width and uv[ 1 ] >= 0 and uv[ 1 ] <= data.height:
cv2.circle( cvImage, ( int( uv[ 0 ] ), int( uv[ 1 ] ) ), 2, (255,0,0))
self.image_coordinates.x=uv[0]
self.image_coordinates.y=uv[1]
self.image_coordinates.z=point[1]
self.pub_image_coordinates.publish(self.image_coordinates)
self.points_selected=self.point_list
self.construct_pointcloud(self.points_selected)
try:
self._imageOutput.publish( self._bridge.cv2_to_imgmsg( cvImage, 'bgr8' ) )
except cv_bridge.CvBridgeError as e:
rospy.logerr( '[lidar_image] Failed to convert image to message' )
rospy.logerr( '[lidar_image] ' + e )
print( e )
return
def cameraCallback( self, data ):
print( 'Received camera info' )
self._cameraModel.fromCameraInfo( data )
def velodyneCallback( self, data ):
print( 'Received velodyne point cloud' )
formatString = 'ffff'
if data.is_bigendian:
formatString = '>' + formatString
else:
formatString = '<' + formatString
points = []
for index in range( 0, len( data.data ), 16 ):
points.append( struct.unpack( formatString, data.data[ index:index + 16 ] ) )
print( len( points ) )
self._velodyneData = points
def construct_pointcloud(self,cloud_points):
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'velodyne_link'
point_cloud = pcl2.create_cloud_xyz32(header, cloud_points)
self.pcl_pub.publish(point_cloud)
def show_text_in_rviz(self,marker_publisher, text,x,y,z):
marker = Marker(
type=Marker.TEXT_VIEW_FACING,
id=0,
lifetime=rospy.Duration(60),
pose=Pose(Point(x, y, z), Quaternion(0, 0, 0, 1)),
scale=Vector3(0.06, 0.06, 0.06),
header=Header(frame_id='velodyne_link'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8),
text=text)
marker_publisher.publish(marker)
class DisplayTransformedPoints(object):
def __init__(self, image_topic_in, camera_info_topic_in, image_topic_out):
self.cameramodels = PinholeCameraModel()
self.is_camera_arrived = False
self.frame_id = None
self.tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0))
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
self.bridge = CvBridge()
self.header_frame_id = ""
self.u_v_list = []
self.state = "sleep"
self.cursor = 0
# self.pub_point_stamped = rospy.Publisher("~output/point_stamped", PointStamped, queue_size=1)
self.pub_image = rospy.Publisher(image_topic_out, Image, queue_size=1)
rospy.Subscriber(camera_info_topic_in, CameraInfo, self.camera_info_cb)
rospy.Subscriber(image_topic_in, Image, self.image_cb)
def camera_info_cb(self, msg):
self.cameramodels.fromCameraInfo(msg)
self.frame_id = msg.header.frame_id
self.is_camera_arrived = True
# when state==choosing : add circles to image and publish it
# when state == otherwise : only publish original image
def image_cb(self, msg):
self.header_frame_id = msg.header.frame_id
if self.state == "choosing":
try:
img = self.bridge.imgmsg_to_cv2(msg, msg.encoding)
except CvBridgeError as e:
rospy.logerr('image_cb failed: {}'.format(e))
for i in range(len(self.u_v_list)):
if (i < len(self.u_v_list) / 2):
color = (0, 0, 255)
else:
color = (255, 0, 0)
if (i == self.cursor
or i == self.cursor + len(self.u_v_list) / 2):
size = 16
else:
size = 10
img_circle = cv2.circle(
img, (int(self.u_v_list[i][0]), int(self.u_v_list[i][1])),
size, color, -1)
img_msg = self.bridge.cv2_to_imgmsg(img, msg.encoding)
self.pub_image.publish(img_msg)
else:
self.pub_image.publish(msg)
def transform_pca(self):
rospy.logdebug("in transform pca")
if not self.is_camera_arrived:
return
try:
# transform = self.tf_buffer.lookup_transform(self.frame_id, self.header_frame_id, rospy.Time(0), rospy.Duration(1.0))
transform = self.tf_buffer.lookup_transform(
self.frame_id, req.frame_id.data, rospy.Time(0),
rospy.Duration(
1.0)) #lleg_end_coords was noting,then use BODY frame
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as e:
rospy.logerr('lookup_transform failed: {}'.format(e))
return
u_v_list_tmp = []
points_list = l_points + req.r_points
for i in range(len(points_list)):
pose_stamped = PoseStamped()
pose_stamped.pose.position.x = points_list[i].x * 0.001
pose_stamped.pose.position.y = points_list[i].y * 0.001
pose_stamped.pose.position.z = points_list[i].z * 0.001
position_transformed = tf2_geometry_msgs.do_transform_pose(
pose_stamped, transform).pose.position
pub_point = (position_transformed.x, position_transformed.y,
position_transformed.z)
u, v = self.cameramodels.project3dToPixel(pub_point)
rospy.logdebug("u, v : {}, {}".format(u, v))
u_v_list_tmp.append([u, v])
self.u_v_list = u_v_list_tmp
print("u_v_list_tmp={}".format(u_v_list_tmp))
def pca_cb(self, msg):
points_list = []
for poses in msg.poses:
points_list.append([poses[i].x, poses[i].y, poses[i].z])
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.camera = None
self.camera_image = None
self.pose = None
self.stop_indexes = None
self.traffic_lights = None
self.waypoints = None
self.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.last_state = TrafficLight.UNKNOWN
self.state = TrafficLight.UNKNOWN
self.state_count = 0
self.last_wp = -1
config = yaml.load(rospy.get_param("/traffic_light_config"))
self.stop_lines = np.array(config['stop_line_positions'])
self.subscribers = [
rospy.Subscriber('/camera_info',
CameraInfo,
self.camera_info_cb,
queue_size=1),
rospy.Subscriber('/current_pose',
PoseStamped,
self.pose_cb,
queue_size=1),
rospy.Subscriber('/base_waypoints',
Lane,
self.waypoints_cb,
queue_size=1),
rospy.Subscriber('/image_color',
Image,
self.image_cb,
queue_size=1),
rospy.Subscriber('/vehicle/traffic_lights',
TrafficLightArray,
self.traffic_cb,
queue_size=1)
]
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
self.image_zoomed = rospy.Publisher('/image_zoomed',
Image,
queue_size=1)
# keeps python from exiting until node is stopped
rospy.spin()
def camera_info_cb(self, camera_info):
if self.camera is not None:
return
self.camera = PinholeCameraModel()
self.camera.fromCameraInfo(camera_info)
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
if self.waypoints is not None:
return
self.waypoints = np.array(
[[w.pose.pose.position.x, w.pose.pose.position.y]
for w in waypoints.waypoints])
self.stop_indexes = np.array(
[closest(self.waypoints, light) for light in self.stop_lines])
def image_cb(self, msg):
r'''Identifies red lights in the incoming camera image and publishes the index
of the waypoint closest to the red light to /traffic_waypoint
Args:
msg (Image): image from car-mounted camera
'''
self.camera_image = msg
light_wp, state = self.process_traffic_lights()
'''
Publish upcoming red lights at camera frequency.
Each predicted state has to occur `STATE_COUNT_THRESHOLD` number
of times till we start using it. Otherwise the previous stable state is
used.
'''
if self.state != state:
self.state_count = 0
self.state = state
elif self.state_count >= STATE_COUNT_THRESHOLD:
self.last_state = self.state
if (state == TrafficLight.GREEN
or (state == TrafficLight.YELLOW
and self.state_count < 2 * STATE_COUNT_THRESHOLD)):
light_wp = -1
self.last_wp = light_wp
self.upcoming_red_light_pub.publish(Int32(light_wp))
else:
self.upcoming_red_light_pub.publish(Int32(self.last_wp))
self.state_count += 1
def traffic_cb(self, msg):
if self.traffic_lights is not None:
return
def pose(light):
position = light.pose.pose.position
return [position.x, position.y, position.z]
self.traffic_lights = np.array([pose(light) for light in msg.lights])
def get_closest_waypoint(self, pose):
r'''Identifies the closest path waypoint to the given position
https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
Args:
pose (Pose): position to match a waypoint to
Returns:
int: index of the closest waypoint in self.waypoints
'''
p = np.array([pose.position.x, pose.position.y])
return closest(self.waypoints, p)
def project_to_image_plane(self, point3d):
r'''Project point from 3D world coordinates to 2D camera image location
Args:
point3d (Point): 3D location of a point in the world
Returns:
x (int): x coordinate of target point in image
y (int): y coordinate of target point in image
'''
stamp = self.camera_image.header.stamp
p_world = PointStamped()
p_world.header.seq = self.camera_image.header.seq
p_world.header.stamp = stamp
p_world.header.frame_id = '/world'
p_world.point.x = point3d[0]
p_world.point.y = point3d[1]
p_world.point.z = point3d[2]
# Transform point from world to camera frame.
self.listener.waitForTransform('/base_link', '/world', stamp,
rospy.Duration(1.0))
p_camera = self.listener.transformPoint('/base_link', p_world)
# The navigation frame has X pointing forward, Y left and Z up, whereas the
# vision frame has X pointing right, Y down and Z forward; hence the need to
# reassign axes here.
x = -p_camera.point.y
y = -p_camera.point.z
z = p_camera.point.x
return self.camera.project3dToPixel((x, y, z))
def get_light_state(self, light):
r'''Determines the current color of the traffic light
Args:
light (TrafficLight): light to classify
Returns:
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
'''
if self.camera_image is None:
self.prev_light_loc = None
return False
(x_tl, y_tl) = self.project_to_image_plane(light +
np.array([-0.7, -0.7, 1.0]))
(x_br, y_br) = self.project_to_image_plane(light +
np.array([0.7, 0.7, -1.0]))
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
shape = cv_image.shape
a = int(max(0, y_tl))
b = int(min(y_br, shape[0]))
c = int(max(0, min(x_br, x_tl)))
d = int(min(max(x_br, x_tl), shape[1]))
if (b - a) < 10 or (d - c) < 10:
return TrafficLight.UNKNOWN
# original 100, 140
zoomed = cv2.resize(cv_image[a:b, c:d], (40, 90))
state = self.light_classifier.get_classification(zoomed)
self.image_zoomed.publish(self.bridge.cv2_to_imgmsg(zoomed, 'bgr8'))
return state
def process_traffic_lights(self):
r'''Finds closest visible traffic light, if one exists, and determines its
location and color
Returns:
int: index of waypoint closes to the upcoming stop line for a traffic light (-1 if none exists)
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
'''
# Entire function has changed? what exacty is going on here?
# is it failing to classify all topics, or is it not listening at all?
# Don't process if there are no waypoints or current position.
if any(v is None
for v in [self.waypoints, self.stop_indexes, self.pose]):
return (-1, TrafficLight.UNKNOWN)
# Get car's position.
i_car = self.get_closest_waypoint(self.pose.pose)
p_car = self.waypoints[i_car]
# Get the closest stop line's index.
j_stop = closest(self.stop_lines, p_car)
i_stop = self.stop_indexes[j_stop]
# If the car is ahead of the closest stop line, get the next one.
if i_car > i_stop:
j_stop = (j_stop + 1) % len(self.stop_indexes)
i_stop = self.stop_indexes[j_stop]
# Don't process if the closest stop line is too far.
if distance(p_car, self.stop_lines[j_stop]) > 70.0:
return (-1, TrafficLight.UNKNOWN)
# Return the index and state of the traffic light.
state = self.get_light_state(self.traffic_lights[j_stop])
return (i_stop, state)
def handle_radar_msg(self, radar_msg):
md = self.metadata
now = radar_msg.header.stamp
poseArray = PoseArray()
for track in radar_msg.tracks :
r = track.range
# ignore obstacles farther than 55 m
if (track.status!=3) or (r>45) or (r<6):
continue
pose = Pose()
theta = track.angle/180.*pi
pose.position.x = (r+md['l']/2.) * math.cos(theta) + 1.5
pose.position.y = -(r+md['l']/2.) * math.sin(theta)
pose.position.z = -0.8
poseArray.poses.append(pose)
self.detected_poses = poseArray
self.radarPub.publish(poseArray)
img = None
bridge = cv_bridge.CvBridge()
if self.raw_image is None :
return
try:
img = bridge.imgmsg_to_cv2(self.raw_image, 'bgr8')
except cv_bridge.CvBridgeError as e:
rospy.logerr( 'image message to cv conversion failed :' )
rospy.logerr( e )
print( e )
return
out_img = np.copy(img)
if self.detected_poses is None:
return
for i, pose in enumerate(self.detected_poses.poses):
color = kelly_colors_list[i % len(kelly_colors_list)]
marker = self.create_marker()
marker.color.r = color[0]/255
marker.color.g = color[1]/255
marker.color.b = color[2]/255
marker.color.a = 0.5
marker.header.stamp = now
marker.pose = pose
marker.id = i
self.markerArray.markers.append(marker)
dims = np.array([md['l'], md['l'], md['h']])
obs_centroid = np.array([pose.position.x,
pose.position.y,
pose.position.z])
orient = list([pose.orientation.x,
pose.orientation.y,
pose.orientation.z,
pose.orientation.w])
# case when obstacle is not in camera frame
if obs_centroid[0]<4 :
continue
# get bbox
R = tf.transformations.quaternion_matrix(orient)
corners = [0.5*np.array([i,j,k])*dims for i in [-1,1]
for j in [-1,1] for k in [-1,1]]
corners = np.array([obs_centroid + R.dot(list(c)+[1])[:3] for c in corners])
cameraModel = PinholeCameraModel()
cam_info = load_cam_info(self.calib_file)
cameraModel.fromCameraInfo(cam_info)
projected_pts = [cameraModel.project3dToPixel(list(pt)+[1]) for pt in corners]
projected_pts = np.array(projected_pts)
center = np.mean(projected_pts, axis=0)
out_img = drawBbox(out_img, projected_pts, color=color[::-1])
self.markOutput.publish(self.markerArray)
self.imgOutput.publish(bridge.cv2_to_imgmsg(out_img, 'bgr8'))
class LidarRGB:
def __init__(self):
self._imageInputName = rospy.resolve_name('image')
print(self._imageInputName)
self._velodyneOutputName = rospy.resolve_name('velodyne_rgb_points')
print(self._velodyneOutputName)
self._cameraName = rospy.resolve_name('camera')
print(self._cameraName)
self._velodyneName = rospy.resolve_name('velodyne')
print(self._velodyneName)
self._imageInput = rospy.Subscriber(self._imageInputName,
Image,
callback=self.imageCallback,
queue_size=1)
self._camera = rospy.Subscriber(self._cameraName,
CameraInfo,
callback=self.cameraCallback,
queue_size=1)
self._velodyne = rospy.Subscriber(self._velodyneName,
PointCloud2,
callback=self.velodyneCallback,
queue_size=1)
self._velodyneOutput = rospy.Publisher(self._velodyneOutputName,
PointCloud2,
queue_size=1)
self._bridge = cv_bridge.CvBridge()
self._cameraModel = PinholeCameraModel()
self._tf = tf.TransformListener()
def imageCallback(self, data):
print('Received an image!')
self._image = {}
try:
self._image = self._bridge.imgmsg_to_cv2(data, 'bgr8')
except cv_bridge.CvBridgeError as e:
rospy.logerr('[lidar_rgb] Failed to convert image')
rospy.logerr('[lidar_rgb] ' + e)
print(e)
return
(translation,
rotation) = self._tf.lookupTransform('world', 'camera', rospy.Time(0))
# print( translation )
translation = translation + (1, )
# print( translation )
self._Rq = tf.transformations.quaternion_matrix(rotation)
# print( Rq )
self._Rq[:, 3] = translation
def cameraCallback(self, data):
print('Received camera info')
self._cameraModel.fromCameraInfo(data)
def velodyneCallback(self, data):
print('Received velodyne point cloud')
# add r, g, b fields
fields = []
fields.append(PointField('x', 0, PointField.FLOAT32, 1))
fields.append(PointField('y', 4, PointField.FLOAT32, 1))
fields.append(PointField('z', 8, PointField.FLOAT32, 1))
fields.append(PointField('intensity', 12, PointField.FLOAT32, 1))
fields.append(PointField('r', 16, PointField.FLOAT32, 1))
fields.append(PointField('g', 20, PointField.FLOAT32, 1))
fields.append(PointField('b', 24, PointField.FLOAT32, 1))
if hasattr(self, '_Rq') and hasattr(self, '_image'):
height, width, channels = self._image.shape
points = []
for dataPoint in pcl2.read_points(data):
point = [dataPoint[0], dataPoint[1], dataPoint[2], 1.0]
intensity = dataPoint[3]
rotatedPoint = self._Rq.dot(point)
# only process points in front of the camera
if rotatedPoint[2] < 0:
continue
# project point into image coordinates
uv = self._cameraModel.project3dToPixel(rotatedPoint)
# add point only if it's within the image bounds
if uv[0] >= 0 and int(uv[0]) < height and uv[1] >= 0 and int(
uv[1]) < width:
[b, g, r] = self._image[int(uv[1]), int(uv[0])]
points.append([
point[0], point[1], point[2], intensity, r / 255.0,
g / 255.0, b / 255.0
])
print('Transmitting lidar_rgb')
newMessage = pcl2.create_cloud(data.header, fields, points)
self._velodyneOutput.publish(newMessage)
class Colorama(object):
def __init__(self):
info_topic = camera_root + "/camera_info"
image_topic = camera_root + "/image_raw" # Change this to rect on boat
self.tf_listener = tf.TransformListener()
# Map id => [{color, error}, ...] for determining when a color is good
self.colored = {}
db_request = rospy.ServiceProxy("/database/requests", ObjectDBQuery)
self.make_request = lambda **kwargs: db_request(
ObjectDBQueryRequest(**kwargs))
rospy.Service("/vision/buoy_colorizer", VisionRequest,
self.got_request)
self.image_history = ImageHistory(image_topic)
# Wait for camera info, and exit if not found
try:
fprint("Waiting for camera info on: '{}'".format(info_topic))
camera_info_msg = rospy.wait_for_message(info_topic,
CameraInfo,
timeout=3)
except rospy.exceptions.ROSException:
fprint("Camera info not found! Terminating.", msg_color="red")
rospy.signal_shutdown("Camera not found!")
return
fprint("Camera info found!", msg_color="green")
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(camera_info_msg)
self.debug = DebugImage("/colorama/debug", prd=.5)
# These are color mappings from Hue values [0, 360]
self.color_map = {
'red': np.radians(0),
'yellow': np.radians(60),
'green': np.radians(120),
'blue': np.radians(240)
}
# Some tunable parameters
self.update_time = .5 # s
self.saturation_reject = 50
self.value_reject = 50
self.hue_error_reject = .4 # rads
# To decide when to accept color observations
self.error_tolerance = .4 # rads
self.spottings_req = 4
self.similarity_reject = .1 # If two colors are within this amount of error, reject
rospy.Timer(ros_t(self.update_time), self.do_update)
def valid_color(self, _id):
"""
Either returns valid color or None depending if the color is valid or not
"""
if _id not in self.colored:
return None
object_data = self.colored[_id]
print "object_data", object_data
# Maps color => [err1, err2, ..]
potential_colors = {}
for data in object_data:
color, err = data['color'], data['err']
if color not in potential_colors:
potential_colors[color] = []
potential_colors[color].append(err)
potential_colors_avg = {}
for color, errs in potential_colors.iteritems():
if len(errs) > self.spottings_req:
potential_colors_avg[color] = np.average(errs)
print "potential_colors_avg", potential_colors_avg
if len(potential_colors_avg) == 1:
# No debate about the color
color = potential_colors_avg.keys()[0]
err = potential_colors_avg[color]
fprint("Only one color found, error: {} (/ {})".format(
err, self.error_tolerance))
if len(potential_colors[color]
) > self.spottings_req and err <= self.error_tolerance:
return color
elif len(potential_colors_avg) > 1:
# More than one color, see if one of them is still valid
fprint("More than one color detected. Removing all.")
self.colored[_id] = []
return None
def got_request(self, req):
# Threading blah blah something unsafe
color = self.valid_color(req.target_id)
if color is None:
fprint("ID {} is NOT a valid color".format(req.target_id),
msg_color='red')
return VisionRequestResponse(found=False)
return VisionRequestResponse(found=True, color=color)
def do_update(self, *args):
resp = self.make_request(name='all')
if resp.found:
# temp
time_of_marker = rospy.Time.now() - ros_t(
.2) # header.stamp not filled out
image_holder = self.image_history.get_around_time(time_of_marker)
if not image_holder.contains_valid_image:
return
header = navigator_tools.make_header(
frame="/enu", stamp=image_holder.time) #resp.objects[0].header
image = image_holder.image
self.debug.image = np.copy(image)
cam_tf = self.camera_model.tfFrame()
fprint("Getting transform between /enu and {}...".format(cam_tf))
self.tf_listener.waitForTransform("/enu", cam_tf, time_of_marker,
ros_t(1))
t_mat44 = self.tf_listener.asMatrix(
cam_tf, header) # homogenous 4x4 transformation matrix
for obj in resp.objects:
if len(obj.points) > 0 and obj.name == "totem":
fprint("{} {}".format(obj.id, "=" * 50))
print obj.position
# Get objects position in camera frame and make sure it's in view
object_cam = t_mat44.dot(
np.append(navigator_tools.point_to_numpy(obj.position),
1))
object_px = map(
int,
self.camera_model.project3dToPixel(object_cam[:3]))
if not self._object_in_frame(object_cam):
continue
#print object_px
points_np = np.array(
map(navigator_tools.point_to_numpy, obj.points))
# We dont want points below a certain level
points_np = points_np[points_np[:, 2] > -2.5]
# Shove ones in there to make homogenous points
points_np_homo = np.hstack(
(points_np, np.ones((points_np.shape[0], 1)))).T
points_cam = t_mat44.dot(points_np_homo).T
points_px = map(self.camera_model.project3dToPixel,
points_cam[:, :3])
#print points_px
roi = self._get_ROI_from_points(points_px)
color_info = self._get_color_from_ROI(
roi, image) # hue, string_color, error
bgr = (0, 0, 0)
if color_info is not None:
hue, color, error = color_info
c = (int(hue), 255, 255)
hsv = np.array([[c]])[:, :3].astype(np.uint8)
bgr = cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR)[0, 0]
bgr = tuple(bgr.astype(np.uint8).tolist())
if hue is not None:
if obj.id not in self.colored:
self.colored[obj.id] = []
self.colored[obj.id].append({
'color': color,
'err': error
})
if self.valid_color(obj.id):
fprint("COLOR IS VALID", msg_color='green')
self.make_request(
cmd=
'{name}={bgr[2]},{bgr[1]},{bgr[0]},{_id}'.
format(name=obj.name, _id=obj.id, bgr=bgr))
[
cv2.circle(self.debug.image, tuple(map(int, p)), 2,
bgr, -1) for p in points_px
]
cv2.circle(self.debug.image, tuple(object_px), 10, bgr, -1)
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(self.debug.image, str(obj.id),
tuple(object_px), font, 1, bgr, 2)
print '\n' * 2
def _get_ROI_from_points(self, image_points):
# Probably will return a set of contours
cnt = np.array(image_points).astype(np.float32)
rect = cv2.minAreaRect(cnt)
box = cv2.cv.BoxPoints(rect)
box = np.int0(box)
fprint("Drawing rectangle")
cv2.drawContours(self.debug.image, [box], 0, (0, 0, 255), 2)
return box
def _get_color_from_ROI(self, roi, img):
mask = np.zeros(img.shape[:2], np.uint8)
cv2.drawContours(mask, [roi], 0, 255, -1)
bgr = cv2.mean(img, mask)
# We have to treat that bgr value as a 3d array to get cvtColor to work
bgr = np.array([[bgr]])[:, :3].astype(np.uint8)
h, s, v = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)[0, 0]
# Now check that s and v are in a good range
if s < self.saturation_reject or v < self.value_reject:
fprint("The colors aren't expressive enough. Rejecting.",
msg_color='red')
return None
# Compute hue error in SO2
hue_angle = np.radians(h * 2)
c = np.cos(hue_angle)
s = np.sin(hue_angle)
error = np.inf
likely_color = 'bazinga'
for color, h_val in self.color_map.iteritems():
cg = np.cos(h_val)
sg = np.sin(h_val)
this_error = np.abs(np.arctan2(sg * c - cg * s, cg * c + sg * s))
if this_error < error:
error = this_error
likely_color = color
if error > self.hue_error_reject:
fprint(
"Closest color was {} with an error of {} rads (> {}) Rejecting."
.format(likely_color, np.round(error, 3),
self.hue_error_reject),
msg_color='red')
return None
fprint("Likely color: {} with an hue error of {} rads.".format(
likely_color, np.round(error, 3)))
return [
np.degrees(self.color_map[likely_color]) / 2.0, likely_color, error
]
def _object_in_frame(self, object_point):
"""
Returns if the object is in frame given the centroid of the object.
`object_point` should be in the camera_model's frame.
"""
print object_point
if object_point[2] < 0:
return False
px = np.array(self.camera_model.project3dToPixel(object_point))
resoultion = self.camera_model.fullResolution()
if np.any([0, 0] > px) or np.any(px > resoultion):
return False
return True
class ARTag:
def __init__(self):
self.axis = np.float32([[0.1, 0, 0], [0, 0.1, 0], [0, 0, -0.1],
[0, 0, 0]]).reshape(-1, 3)
self.objp = np.zeros((6 * 8, 3), np.float32)
self.objp[:, :2] = np.mgrid[0:8, 0:6].T.reshape(-1, 2)
self.K = None
self.D = None
self.markers = []
self.cam_info = None
self.pinhole = PinholeCameraModel()
self.cam_info_sub = rospy.Subscriber('camera/rgb/camera_info',
CameraInfo, self.info_cb)
self.img_sub = rospy.Subscriber('camera/rgb/image_raw/compressed',
CompressedImage,
self.img_cb,
queue_size=1,
buff_size=2**24)
self.ar_sub = rospy.Subscriber('ar_pose_marker', AlvarMarkers,
self.ar_cb)
self.pub = rospy.Publisher('ar_location', String, queue_size=5)
def info_cb(self, msg):
self.cam_info = msg
self.K = np.array(msg.K).reshape(3, 3)
self.D = np.array(msg.D)
def ar_cb(self, msg):
self.markers = msg.markers
def img_cb(self, msg):
np_arr = np.fromstring(msg.data, np.uint8)
frame = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
if len(self.markers) > 0 and self.cam_info != None:
camera = self.pinhole.fromCameraInfo(self.cam_info)
mid_list = []
corners = []
for i in range(len(self.markers)):
tempCorner = self.pinhole.project3dToPixel([
self.markers[i].pose.pose.position.x,
self.markers[i].pose.pose.position.y,
self.markers[i].pose.pose.position.z
])
tmp = []
corner = [tempCorner[0], tempCorner[1]]
circle = [int(tempCorner[0]), int(tempCorner[1])]
tmp.append(np.array(corner, dtype="float32"))
mid_list.append(np.array(tmp, dtype="float32"))
corners.append(circle)
cv2.circle(frame, tuple(circle), 3, (100, 100, 100), 5)
px = self.markers[0].pose.pose.position.x
py = self.markers[0].pose.pose.position.y
pz = self.markers[0].pose.pose.position.z
ox = self.markers[0].pose.pose.orientation.x
oy = self.markers[0].pose.pose.orientation.y
oz = -self.markers[0].pose.pose.orientation.z
ow = -self.markers[0].pose.pose.orientation.w
tvecs = np.array([px, py, pz])
angle = 2 * math.acos(ow)
x = ox / math.sqrt(1 - ow * ow)
y = oy / math.sqrt(1 - ow * ow)
z = oz / math.sqrt(1 - ow * ow)
ratio = math.sqrt(x * x + y * y + z * z)
x = x / ratio * angle
y = y / ratio * angle
z = z / ratio * angle
rvecs = np.array([x, y, z])
imgpts, jac = cv2.projectPoints(self.axis, rvecs, tvecs, self.K,
self.D)
origin = tuple(imgpts[3].ravel())
X = tuple(imgpts[0].ravel())
Y = tuple(imgpts[1].ravel())
Z = tuple(imgpts[2].ravel())
image = cv2.line(frame, origin, X, (255, 0, 0), 5)
image = cv2.line(frame, origin, Y, (0, 255, 0), 5)
image = cv2.line(frame, origin, Z, (0, 0, 255), 5)
cv2.imshow('ARTag', frame)
# pos_msg = str(origin) + ' ' + str(X) + ' ' + str(Y) + ' ' + str(Z)
pos_msg = str(origin[0]) + ' ' + str(origin[1])
self.pub.publish(pos_msg)
else:
pos_msg = ''
self.pub.publish(pos_msg)
cv2.imshow('ARTag', frame)
cv2.waitKey(1)
class ScanTheCodePerception(object):
"""Class that handles ScanTheCodePerception."""
def __init__(self, my_tf, debug, nh):
"""Initialize ScanTheCodePerception class."""
self.image_cache = deque()
self.bridge = CvBridge()
self.nh = nh
self.last_cam_info = None
self.debug = debug
self.pers_points = []
self.model_tracker = ScanTheCodeModelTracker()
self.camera_model = PinholeCameraModel()
self.my_tf = my_tf
self.rect_finder = RectangleFinderClustering()
self.color_finder = ColorFinder()
self.count = 0
self.depths = []
@txros.util.cancellableInlineCallbacks
def _init(self):
self.vel_sub = yield self.nh.subscribe("/velodyne_points", PointCloud2)
self.image_sub = yield self.nh.subscribe(
"/camera/front/right/image_rect_color", Image)
defer.returnValue(self)
def add_image(self, image):
"""Add an image to the image cache."""
if len(self.image_cache) > 50:
self.image_cache.popleft()
self.image_cache.append(image)
def update_info(self, info):
"""Update the camera calibration info."""
self.last_cam_info = info
self.camera_model.fromCameraInfo(info)
def _get_top_left_point(self, points_3d):
xmin = sys.maxsize
zmin = -sys.maxsize
ymin = sys.maxsize
for i, point in enumerate(points_3d):
if point[1] < ymin:
xmin = point[0]
zmin = point[2]
ymin = point[1]
buff = 1
for i, point in enumerate(points_3d):
if (point[1] < ymin + buff and point[1] > ymin - buff
and point[0] < xmin):
xmin = point[0]
zmin = point[2]
return xmin, ymin, zmin
def _get_points_in_range(self, axis, lower, upper, points):
in_range = []
idx = 0
if axis == 'y':
idx = 1
if axis == 'z':
idx = 2
for p in points:
if p[idx] > lower and p[idx] < upper:
in_range.append(p)
return in_range
def _get_depth(self, axis, points_3d):
idx = 0
if axis == 'y':
idx = 1
if axis == 'z':
idx = 2
min_val, max_val = sys.maxsize, -sys.maxsize
points_3d = np.array(points_3d)
my_points = points_3d[:, idx]
mean = np.mean(my_points)
std = 1.5 * np.std(my_points)
for p in my_points:
if abs(p - mean) > std:
# print p, mean
continue
if p < min_val:
min_val = p
if p > max_val:
max_val = p
return max_val - min_val
def _get_2d_points_stc(self, points_3d):
# xmin, ymin, zmin = self._get_top_left_point(points_3d)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 10,
1.0)
points_3d = np.float32(points_3d)
ret, label, centers = cv2.kmeans(np.array(points_3d), 2, criteria, 10,
0)
data = Counter(label.flatten())
max_label = data.most_common(1)[0][0]
c = centers[max_label]
xmin, ymin, zmin = c[0], c[1], c[2]
points_3d = [[xmin - .3, ymin - .3,
zmin], [xmin + .3, ymin + .3, zmin],
[xmin - .3, ymin + .3, zmin],
[xmin + .3, ymin - .3, zmin]]
points_2d = map(lambda x: self.camera_model.project3dToPixel(x),
points_3d)
return points_2d
def _get_bounding_rect(self, points_2d, img):
xmin = np.inf
xmax = -np.inf
ymin = np.inf
ymax = -np.inf
h, w, r = img.shape
for i, point in enumerate(points_2d):
if (point[0] < xmin):
xmin = point[0]
if (point[0] > xmax):
xmax = point[0]
if (point[1] > ymax):
ymax = point[1]
if (point[1] < ymin):
ymin = point[1]
if xmin < 0:
xmin = 1
if ymin < 0:
ymin = 1
if xmax > w:
xmax = w - 1
if ymax > h:
ymax = h - 1
return xmin, ymin, xmax, ymax
@txros.util.cancellableInlineCallbacks
def get_stc_points(self, msg, stc_pos):
trans = yield self.my_tf.get_transform("/front_right_cam", "/velodyne",
msg.header.stamp)
trans1 = yield self.my_tf.get_transform("/front_right_cam", "/enu",
msg.header.stamp)
stc_pos = rosmsg_to_numpy(stc_pos)
stc_pos = np.append(stc_pos, 1)
position = trans1.as_matrix().dot(stc_pos)
if position[3] < 1E-15:
raise ZeroDivisionError
position[0] /= position[3]
position[1] /= position[3]
position[2] /= position[3]
position = position[:3]
stereo_points = []
for point in pc2.read_points(msg, skip_nans=True):
stereo_points.append(np.array([point[0], point[1], point[2], 1]))
stereo_points = map(lambda x: trans.as_matrix().dot(x), stereo_points)
points = []
for p in stereo_points:
if p[3] < 1E-15:
raise ZeroDivisionError
p[0] /= p[3]
p[1] /= p[3]
p[2] /= p[3]
points.append(p[:3])
points_keep = []
for p in points:
# print npl.norm(p - poition)
if npl.norm(p - position) < 20:
points_keep.append(p)
points_keep = sorted(points_keep, key=lambda x: x[1])
keep_num = int(.1 * len(points_keep))
points_keep = points_keep[:keep_num]
# self.pers_points.extend(points_keep)
# max_num = 200
# if len(self.pers_points) > max_num:
# self.pers_points = self.pers_points[len(self.pers_points) - max_num:len(self.pers_points)]
defer.returnValue(points_keep)
@txros.util.cancellableInlineCallbacks
def search(self, scan_the_code):
"""Search for the colors in the scan the code object."""
pntcloud = yield self.vel_sub.get_next_message()
image_ros = yield self.image_sub.get_next_message()
try:
image = self.bridge.imgmsg_to_cv2(image_ros, "bgr8")
except CvBridgeError:
print "Trouble converting image"
defer.returnValue((False, None))
points_3d = yield self.get_stc_points(pntcloud, scan_the_code.position)
image_clone = image.copy()
# points_3d = yield self._get_3d_points_stereo(scan_the_code.points, image_ros.header.stamp)
# points_2d = map(lambda x: self.camera_model.project3dToPixel(x), points_3d)
points_2d = map(lambda x: self.camera_model.project3dToPixel(x),
points_3d)
for p in points_2d:
po = (int(round(p[0])), int(round(p[1])))
cv2.circle(image_clone, po, 2, (0, 255, 0), -1)
points_2d = self._get_2d_points_stc(points_3d)
xmin, ymin, xmax, ymax = self._get_bounding_rect(points_2d, image)
xmin, ymin, xmax, ymax = int(xmin), int(ymin), int(xmax), int(ymax)
cv2.rectangle(image_clone, (xmin, ymin), (xmax, ymax), (0, 255, 0), 2)
self.debug.add_image(image_clone, "bounding_box", topic="bounding_box")
# defer.returnValue((False, None))
print xmin, ymin, xmax, ymax
roi = image[ymin:ymax, xmin:xmax]
succ, color_vec = self.rect_finder.get_rectangle(roi, self.debug)
if not succ:
defer.returnValue((False, None))
self.mission_complete, colors = self.color_finder.check_for_colors(
image, color_vec, self.debug)
if self.mission_complete:
print "MISSION COMPLETE"
defer.returnValue((True, colors))
defer.returnValue((False, None))
@txros.util.cancellableInlineCallbacks
def correct_pose(self, scan_the_code):
"""Check to see if we are looking at the corner of scan the code."""
self.count += 1
pntcloud = yield self.vel_sub.get_next_message()
points_3d = yield self.get_stc_points(pntcloud, scan_the_code.position)
xmin, ymin, zmin = self._get_top_left_point(points_3d)
points_oi = self._get_points_in_range('y', ymin - .1, ymin + .2,
points_3d)
if len(points_oi) == 0:
defer.returnValue(False)
image_ros = yield self.image_sub.get_next_message()
image_ros = self.bridge.imgmsg_to_cv2(image_ros, "bgr8").copy()
depth = self._get_depth('z', points_oi)
fprint("DEPTH: {}".format(depth), msg_color="green")
self.depths.append(depth)
# %%%%%%%%%%%%%%%%%%%%%%%%DEBUG
cv2.putText(image_ros, str(depth), (10, 30), 1, 2, (0, 0, 255))
self.debug.add_image(image_ros, "in_range", topic="in_range")
# %%%%%%%%%%%%%%%%%%%%%%%%DEBUG
if depth > .10:
defer.returnValue(False)
defer.returnValue(True)
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.camera = PinholeCameraModel()
self.camera.fromCameraInfo(self.load_camera_info())
self.camera_image = None
self.pose = None
self.stop_indexes = None
self.traffic_lights = None
# used to get traffic lights state ground truth from /vehicle/traffic_lights
# topic, this is used to gather training data for traffic lights classifier
self.traffic_lights_state = None
self.waypoints = None
self.previous_light_state = TrafficLight.UNKNOWN
self.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.last_state = TrafficLight.UNKNOWN
self.state = TrafficLight.UNKNOWN
self.state_count = 0
self.last_wp = -1
config = yaml.load(rospy.get_param("/traffic_light_config"))
self.stop_lines = np.array(config['stop_line_positions'])
self.subscribers = [
rospy.Subscriber('/current_pose',
PoseStamped,
self.pose_cb,
queue_size=1),
rospy.Subscriber('/base_waypoints',
Lane,
self.waypoints_cb,
queue_size=1),
rospy.Subscriber('/image_color',
Image,
self.image_cb,
queue_size=1),
rospy.Subscriber('/vehicle/traffic_lights',
TrafficLightArray,
self.traffic_cb,
queue_size=1)
]
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
self.image_zoomed = rospy.Publisher('/image_zoomed',
Image,
queue_size=1)
# keeps python from exiting until node is stopped
rospy.spin()
def load_camera_info(self):
calib_yaml_fname = "./calibration_simulator.yaml"
calib_data = None
with open(calib_yaml_fname) as f:
calib_data = yaml.load(f.read())
camera_info_msg = CameraInfo()
camera_info_msg.width = calib_data["image_width"]
camera_info_msg.height = calib_data["image_height"]
camera_info_msg.K = calib_data["camera_matrix"]["data"]
camera_info_msg.D = calib_data["distortion_coefficients"]["data"]
camera_info_msg.R = calib_data["rectification_matrix"]["data"]
camera_info_msg.P = calib_data["projection_matrix"]["data"]
camera_info_msg.distortion_model = calib_data["distortion_model"]
return camera_info_msg
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
if self.waypoints is not None:
return
self.waypoints = np.array(
[[w.pose.pose.position.x, w.pose.pose.position.y]
for w in waypoints.waypoints])
self.stop_indexes = np.array(
[closest(self.waypoints, light) for light in self.stop_lines])
def image_cb(self, msg):
r'''Identifies red lights in the incoming camera image and publishes the index
of the waypoint closest to the red light to /traffic_waypoint
Args:
msg (Image): image from car-mounted camera
'''
self.camera_image = msg
light_wp, state = self.process_traffic_lights()
'''
Publish upcoming red lights at camera frequency.
Each predicted state has to occur `STATE_COUNT_THRESHOLD` number
of times till we start using it. Otherwise the previous stable state is
used.
'''
if self.state != state:
self.state_count = 0
self.state = state
elif self.state_count >= STATE_COUNT_THRESHOLD:
self.last_state = self.state
if (state == TrafficLight.GREEN
or (state == TrafficLight.YELLOW
and self.state_count < 2 * STATE_COUNT_THRESHOLD)):
light_wp = -1
self.last_wp = light_wp
self.upcoming_red_light_pub.publish(Int32(light_wp))
else:
self.upcoming_red_light_pub.publish(Int32(self.last_wp))
self.state_count += 1
def traffic_cb(self, msg):
def pose(light):
position = light.pose.pose.position
return [position.x, position.y, position.z]
self.traffic_lights = np.array([pose(light) for light in msg.lights])
## used to gathher training data for traffic light classifier, will not work on
# the actual calar self driving car
self.traffic_lights_state = np.array(
[light.state for light in msg.lights])
def get_closest_waypoint(self, pose):
r'''Identifies the closest path waypoint to the given position
https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
Args:
pose (Pose): position to match a waypoint to
Returns:
int: index of the closest waypoint in self.waypoints
'''
p = np.array([pose.position.x, pose.position.y])
return closest(self.waypoints, p)
def project_to_image_plane(self, point3d):
r'''Project point from 3D world coordinates to 2D camera image location
Args:
point3d (Point): 3D location of a point in the world
Returns:
x (int): x coordinate of target point in image
y (int): y coordinate of target point in image
'''
stamp = self.camera_image.header.stamp
p_world = PointStamped()
p_world.header.seq = self.camera_image.header.seq
p_world.header.stamp = stamp
p_world.header.frame_id = '/world'
p_world.point.x = point3d[0]
p_world.point.y = point3d[1]
p_world.point.z = point3d[2]
# Transform point from world to camera frame.
self.listener.waitForTransform('/base_link', '/world', stamp,
rospy.Duration(1.0))
p_camera = self.listener.transformPoint('/base_link', p_world)
# The navigation frame has X pointing forward, Y left and Z up, whereas the
# vision frame has X pointing right, Y down and Z forward; hence the need to
# reassign axes here.
x = -p_camera.point.y
y = -p_camera.point.z
z = p_camera.point.x
return self.camera.project3dToPixel((x, y, z))
def get_light_state(self, light_index):
r'''Determines the current color of the traffic light
Args:
light (TrafficLight): light to classify
Returns:
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
'''
light = self.traffic_lights[light_index]
# state = self.traffic_lights_state[light_index]
if self.camera_image is None:
self.prev_light_loc = None
return TrafficLight.UNKNOWN
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
resized = cv2.resize(cv_image, (400, 300))
# save_training_data(resized, self.traffic_lights_state[light_index])
state = self.light_classifier.get_classification(resized)
return state
def process_traffic_lights(self):
r'''Finds closest visible traffic light, if one exists, and determines its
location and color
Returns:
int: index of waypoint closes to the upcoming stop line for a traffic light (-1 if none exists)
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
'''
# Entire function has changed? what exacty is going on here?
# is it failing to classify all topics, or is it not listening at all?
# Don't process if there are no waypoints or current position.
if any(v is None
for v in [self.waypoints, self.stop_indexes, self.pose]):
return (-1, TrafficLight.UNKNOWN)
# Get car's position.
i_car = self.get_closest_waypoint(self.pose.pose)
p_car = self.waypoints[i_car]
# Get the closest stop line's index.
j_stop = closest(self.stop_lines, p_car)
i_stop = self.stop_indexes[j_stop]
# If the car is ahead of the closest stop line, get the next one.
if i_car > i_stop:
j_stop = (j_stop + 1) % len(self.stop_indexes)
i_stop = self.stop_indexes[j_stop]
# Don't process if the closest stop line is too far.
if distance(p_car, self.stop_lines[j_stop]) > 70.0:
if self.previous_light_state != TrafficLight.UNKNOWN:
rospy.logwarn("light state is %s", TrafficLight.UNKNOWN)
self.previous_light_state = TrafficLight.UNKNOWN
return (-1, TrafficLight.UNKNOWN)
# Return the index and state of the traffic light.
state = self.get_light_state(j_stop)
# self.save_classifier_training_data(j_stop)
if state != self.previous_light_state:
rospy.logwarn("light state is %s", state)
self.previous_light_state = state
return (i_stop, state)
class PixelConversion():
def __init__(self):
self.image_topic = "wall_camera/image_raw"
self.image_info_topic = "wall_camera/camera_info"
self.point_topic = "/clicked_point"
self.frame_id = "/map"
self.pixel_x = 350
self.pixel_y = 215
# What we do during shutdown
rospy.on_shutdown(self.cleanup)
# Create the OpenCV display window for the RGB image
self.cv_window_name = self.image_topic
cv.NamedWindow(self.cv_window_name, cv.CV_WINDOW_NORMAL)
cv.MoveWindow(self.cv_window_name, 25, 75)
# Create the cv_bridge object
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber(self.image_topic, Image, self.image_cb, queue_size = 1)
self.image_info_sub = rospy.Subscriber(self.image_info_topic, CameraInfo, self.image_info_cb, queue_size = 1)
self.point_sub = rospy.Subscriber(self.point_topic, PointStamped, self.point_cb, queue_size = 4)
self.line_pub = rospy.Publisher("line", Marker, queue_size = 10)
self.inter_pub = rospy.Publisher("inter", Marker, queue_size = 10)
self.plane_pub = rospy.Publisher("plane", Marker, queue_size = 10)
self.points = []
self.ready_for_image = False
self.has_pic = False
self.camera_info_msg = None
self.tf_listener = tf.TransformListener()
# self.display_picture()
self.camera_model = PinholeCameraModel()
def point_cb(self, point_msg):
if len(self.points) < 4:
self.points.append(point_msg.point)
if len(self.points) == 1:
self.ready_for_image = True
print "Point stored from click: ", point_msg.point
def image_cb(self, img_msg):
if self.ready_for_image:
# Use cv_bridge() to convert the ROS image to OpenCV format
try:
frame = self.bridge.imgmsg_to_cv2(img_msg, "bgr8")
except CvBridgeError, e:
print e
# Convert the image to a Numpy array since most cv2 functions
# require Numpy arrays.
frame = np.array(frame, dtype=np.uint8)
print "Got array"
# Process the frame using the process_image() function
#display_image = self.process_image(frame)
# Display the image.
self.img = frame
# self.has_pic = True
if self.camera_info_msg is not None:
print "cx ", self.camera_model.cx()
print "cy ", self.camera_model.cy()
print "fx ", self.camera_model.fx()
print "fy ", self.camera_model.fy()
# Get the point from the clicked message and transform it into the camera frame
self.tf_listener.waitForTransform(img_msg.header.frame_id, self.frame_id, rospy.Time(0), rospy.Duration(0.5))
(trans,rot) = self.tf_listener.lookupTransform(img_msg.header.frame_id, self.frame_id, rospy.Time(0))
self.tfros = tf.TransformerROS()
tf_mat = self.tfros.fromTranslationRotation(trans, rot)
point_in_camera_frame = tuple(np.dot(tf_mat, np.array([self.points[0].x, self.points[0].y, self.points[0].z, 1.0])))[:3]
# Get the pixels related to the clicked point (the point must be in the camera frame)
pixel_coords = self.camera_model.project3dToPixel(point_in_camera_frame)
print "Pixel coords ", pixel_coords
# Get the unit vector related to the pixel coords
unit_vector = self.camera_model.projectPixelTo3dRay((int(pixel_coords[0]), int(pixel_coords[1])))
print "Unit vector ", unit_vector
# TF the unit vector of the pixel to the frame of the clicked point from the map frame
# self.tf_listener.waitForTransform(self.frame_id, img_msg.header.frame_id, rospy.Time(0), rospy.Duration(0.5))
# (trans,rot) = self.tf_listener.lookupTransform(self.frame_id, img_msg.header.frame_id, rospy.Time(0))
# self.tfros = tf.TransformerROS()
# tf_mat = self.tfros.fromTranslationRotation(trans, rot)
# xyz = tuple(np.dot(tf_mat, np.array([unit_vector[0], unit_vector[1], unit_vector[2], 1.0])))[:3]
# print "World xyz ", xyz
# intersect the unit vector with the ground plane
red = (0,125,255)
cv2.circle(self.img, (int(pixel_coords[0]), int(pixel_coords[1])), 30, red)
cv2.imshow(self.image_topic, self.img)
''' trying better calculations for going from pixel to world '''
# need plane of the map in camera frame points
# (0, 0, 0), (1, 1, 0), (-1, -1, 0) = map plane in map frame coordinates
p1 = [-94.0, -98.0, 0.0]
p2 = [-92.0, -88.0, 0.0]
p3 = [-84.0, -88.0, 0.0]
# p2 = [1.0, -1.0, 0.0]
# p3 = [-1.0, -1.0, 0.0]
# point_marker = self.create_points_marker([p1, p2, p3], "/map")
# self.plane_pub.publish(point_marker)
self.tf_listener.waitForTransform(img_msg.header.frame_id, self.frame_id, rospy.Time(0), rospy.Duration(0.5))
(trans,rot) = self.tf_listener.lookupTransform(img_msg.header.frame_id, self.frame_id, rospy.Time(0))
self.tfros = tf.TransformerROS()
tf_mat = self.tfros.fromTranslationRotation(trans, rot)
# pts in camera frame coodrds
p1 = list(np.dot(tf_mat, np.array([p1[0], p1[1], p1[2], 1.0])))[:3]
p2 = list(np.dot(tf_mat, np.array([p2[0], p2[1], p2[2], 1.0])))[:3]
p3 = list(np.dot(tf_mat, np.array([p3[0], p3[1], p3[2], 1.0])))[:3]
plane_norm = self.get_plane_normal(p1, p2, p3) ## maybe viz
print "P1 ", p1
print "P2 ", p2
print "P3 ", p3
print "Plane norm: ", plane_norm
point_marker = self.create_points_marker([p1, p2, p3], img_msg.header.frame_id)
self.plane_pub.publish(point_marker)
# need unit vector to define ray to cast onto plane
line_pt = list(unit_vector)
line_norm = self.get_line_normal([0.0,0.0,0.0], np.asarray(line_pt) * 10) # just scale the unit vector for another point, maybe just use both unit vector for the point and the line
inter = self.line_plane_intersection(line_pt, line_norm, p1, plane_norm)
## intersection is in the camera frame...
### tf this into map frame
self.tf_listener.waitForTransform(self.frame_id, img_msg.header.frame_id, rospy.Time(0), rospy.Duration(0.5))
(trans,rot) = self.tf_listener.lookupTransform(self.frame_id, img_msg.header.frame_id, rospy.Time(0))
self.tfros = tf.TransformerROS()
tf_mat = self.tfros.fromTranslationRotation(trans, rot)
xyz = tuple(np.dot(tf_mat,np.array([inter[0], inter[1], inter[2], 1.0])))[:3]
print "intersection pt: ", xyz
point_marker = self.create_point_marker(xyz, "/map")
self.inter_pub.publish(point_marker)
line_marker = self.create_line_marker([0.0,0.0,0.0], np.asarray(unit_vector) * 30, img_msg.header.frame_id)
self.line_pub.publish(line_marker)
self.ready_for_image = False
self.points = []
class InitialSampler(object):
def __init__(self,cam):
#topic_name = '/hsrb/head_rgbd_sensor/depth_registered/image_raw'
not_read = True
while not_read:
try:
cam_info = cam.read_info_data()
if(not cam_info == None):
not_read = False
except:
rospy.logerr('info not recieved')
self.pcm = PCM()
self.cam = cam
self.pcm.fromCameraInfo(cam_info)
self.br = tf.TransformBroadcaster()
self.tl = tf.TransformListener()
self.xbox = XboxController()
def debug_images(self,p_1,p_2):
c_img = self.cam.read_color_data()
p_1i = (int(p_1[0]),int(p_1[1]))
p_2i = (int(p_2[0]),int(p_2[1]))
cv2.line(c_img,p_1i,p_2i,(0,0,255),thickness = 10)
cv2.imshow('debug',c_img)
cv2.waitKey(300)
#IPython.embed()
def project_to_rgbd(self,trans):
M_t = tf.transformations.translation_matrix(trans)
M_R = self.get_map_to_rgbd()
M_cam_trans = np.matmul(LA.inv(M_R),M_t)
return M_cam_trans[0:3,3]
def make_projection(self,t_1,t_2):
###GO FROM MAP TO RGBD###
t_1 = self.project_to_rgbd(t_1)
t_2 = self.project_to_rgbd(t_2)
p_1 = self.pcm.project3dToPixel(t_1)
p_2 = self.pcm.project3dToPixel(t_2)
self.debug_images(p_1,p_2)
def debug_broadcast(self,pose,name):
while True:
self.br.sendTransform((pose[0], pose[1], pose[2]),
tf.transformations.quaternion_from_euler(ai=0.0,aj=0.0,ak=0.0),
rospy.Time.now(),
name,
#'head_rgbd_sensor_link')
'rgbd_sensor_rgb_frame_map')
def get_map_to_rgbd(self):
not_found = True
while not_found:
try:
pose = self.tl.lookupTransform('map','rgbd_sensor_rgb_frame_map', rospy.Time(0))
not_found = False
except:
rospy.logerr("waiting for pose")
M = tf.transformations.quaternion_matrix(pose[1])
M_t = tf.transformations.translation_matrix(pose[0])
M[:,3] = M_t[:,3]
return M
def get_postion(self,name):
not_found = True
while not_found:
try:
pose = self.tl.lookupTransform('map',name, rospy.Time(0))
not_found = False
except:
rospy.logerr("waiting for pose")
M = tf.transformations.quaternion_matrix(pose[1])
M_t = tf.transformations.translation_matrix(pose[0])
M[:,3] = M_t[:,3]
return pose[0]
def look_up_transform(self,count):
transforms = self.tl.getFrameStrings()
for transform in transforms:
current_grasp = 'bed_i_'+str(count)
if current_grasp in transform:
print 'got here'
pose = self.tl.lookupTransform('rgbd_sensor_rgb_frame_map',transform, rospy.Time(0))
M = tf.transformations.quaternion_matrix(pose[1])
M_t = tf.transformations.translation_matrix(pose[0])
M[:,3] = M_t[:,3]
return M
def sample_corners(self):
head_up = self.get_postion("head_up")
head_down = self.get_postion("head_down")
bottom_up = self.get_postion("bottom_up")
bottom_down = self.get_postion("bottom_down")
# Get true midpoints of table edges
# (i.e. don't assume bottom_up and head_up have the same y value
# in case sensor image is tilted)
middle_up = np.array([(bottom_up[0] + head_up[0])/2,
(bottom_up[1] + head_up[1])/2,
bottom_down[2]])
middle_down = np.array([(bottom_down[0] + head_down[0])/2,
(bottom_down[1] + head_down[1])/2,
bottom_down[2]] )
bottom_middle = np.array([(bottom_down[0] + bottom_up[0])/2,
(bottom_down[1] + bottom_up[1])/2,
bottom_down[2]] )
head_middle = np.array([(head_down[0] + head_up[0])/2,
(head_down[1] + head_up[1])/2,
bottom_down[2]])
center = np.array([(bottom_middle[0] + head_middle[0])/2,
(middle_down[1] + middle_up[1])/2,
bottom_down[2]])
# Generate random point in sensor frame for the closer corner
# Draws from gaussian distribution
u_down = np.random.uniform(low=0.0,high=0.5)
v_down = np.random.uniform(low=0.3,high=1.0)
x_down = center[0] + u_down*LA.norm(center - bottom_middle)
y_down = center[1] + v_down*LA.norm(center - middle_down)
down_corner = (x_down, y_down, center[2])
# Generate random point in sensor frame for the further corner
# Draws from gaussian distribution
u_up = np.random.uniform(low=0.0,high=0.5)
v_up = np.random.uniform(low=0.3,high=1.0)
x_up = center[0] - u_up*LA.norm(center - bottom_middle)
y_up = center[1] - v_up*LA.norm(center - middle_up)
up_corner = (x_up, y_up, center[2])
print "CENTER ",center
if center[1] < 0.0 or center[2] < 0.0:
raise "ROBOT TRANSFROM INCORRECT"
print "UP CORNER ", up_corner
print "DOWN CORNER ", down_corner
return down_corner, up_corner
def sample_initial_state(self):
down_corner, up_corner = self.sample_corners()
button = 1.0
while button > -0.1:
control_state = self.xbox.getControllerState()
d_pad = control_state['d_pad']
button = d_pad[1]
self.make_projection(down_corner,up_corner)
return down_corner, up_corner
def handle_img_msg(self, img_msg):
now = rospy.get_rostime()
for m in self.markerArray.markers:
m.action = Marker.DELETE
img = None
bridge = cv_bridge.CvBridge()
try:
img = bridge.imgmsg_to_cv2(img_msg, 'bgr8')
except cv_bridge.CvBridgeError as e:
rospy.logerr( 'image message to cv conversion failed :' )
rospy.logerr( e )
print( e )
return
timestamp = img_msg.header.stamp.to_nsec()
self.frame_index = self.timestamp_map[timestamp]
if self.offset :
self.frame_index -= self.offset
out_img = np.copy(img)
for i, f in enumerate(self.frame_map[self.frame_index]):
#f = self.frame_map[self.frame_index][0]
#color = (255,255,0)
color = kelly_colors_list[i % len(kelly_colors_list)]
marker = self.create_marker()
marker.color.r = color[0]/255
marker.color.g = color[1]/255
marker.color.b = color[2]/255
marker.color.a = 0.5
marker.header.stamp = now
marker.pose.position = Point(*f.trans)
marker.pose.orientation = Quaternion(*f.rotq)
marker.id = i
self.markerArray.markers.append(marker)
#self.markOutput.publish(marker)
md = self.metadata
dims = np.array([md['l'], md['w'], md['h']])
obs_centroid = np.array(f.trans)
orient = list(f.rotq)
#orient[2] -= 0.035
#R = tf.transformations.quaternion_matrix((0,0,-0.0065,1))
#obs_centroid = R.dot(list(obs_centroid)+[1])[:3]
if obs_centroid is None:
rospy.loginfo("Couldn't find obstacle centroid")
continue
#self.imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
#return
# print centroid info
rospy.loginfo(str(obs_centroid))
# case when obstacle is not in camera frame
if obs_centroid[0]<3 :
continue
#self.imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
#return
# get bbox
R = tf.transformations.quaternion_matrix(orient)
#R = tf.transformations.quaternion_matrix([0,0,0,1])
corners = [0.5*np.array([i,j,k])*dims for i in [-1,1]
for j in [-1,1] for k in [-1,1]]
corners = np.array([obs_centroid + R.dot(list(c)+[1])[:3] for c in corners])
#if self.ground_corr is not None:
# z_min, x_min, y_min = self.ground_corr.loc[timestamp]
# z_offset = z_min - min(corners[:,2])
# x_offset = x_min - min(corners[:,0])
# y_offset = y_min - min(corners[:,1])
# corr = np.array([0, 0, z_offset])
# #corr = np.array([0, 0,0])
# #corr = np.array([x_offset, y_offset, z_offset])
# corr[np.isnan(corr)]=0
# corners+=corr
#print(corners)
cameraModel = PinholeCameraModel()
cam_info = load_cam_info(self.calib_file)
cameraModel.fromCameraInfo(cam_info)
projected_pts = [cameraModel.project3dToPixel(list(pt)+[1]) for pt in corners]
projected_pts = np.array(projected_pts)
center = np.mean(projected_pts, axis=0)
out_img = drawBbox(out_img, projected_pts, color=color[::-1])
for i, f in enumerate(self.frame_map_corrected[self.frame_index]):
f = self.frame_map[self.frame_index][0]
color = (255,255,0)
color = kelly_colors_list[i % len(kelly_colors_list)]
marker = self.create_marker()
marker.color.r = 128
marker.color.g = 0
marker.color.b = 0
marker.color.a = 0.5
marker.header.stamp = now
marker.pose.position = Point(*f.trans)
marker.pose.orientation = Quaternion(*f.rotq)
marker.id = i
self.markerArray.markers.append(marker)
# self.markOutput.publish(marker)
md = self.metadata
dims = np.array([md['l'], md['w'], md['h']])
obs_centroid = np.array(f.trans)
orient = list(f.rotq)
# orient[2] -= 0.035
# R = tf.transformations.quaternion_matrix((0,0,-0.0065,1))
# obs_centroid = R.dot(list(obs_centroid)+[1])[:3]
if obs_centroid is None:
rospy.loginfo("Couldn't find obstacle centroid")
continue
# self.imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
# return
# print centroid info
rospy.loginfo(str(obs_centroid))
# case when obstacle is not in camera frame
if obs_centroid[0] < 3:
continue
# self.imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
# return
# get bbox
R = tf.transformations.quaternion_matrix(orient)
# R = tf.transformations.quaternion_matrix([0,0,0,1])
corners = [0.5 * np.array([i, j, k]) * dims for i in [-1, 1]
for j in [-1, 1] for k in [-1, 1]]
corners = np.array([obs_centroid + R.dot(list(c) + [1])[:3] for c in corners])
# if self.ground_corr is not None:
# z_min, x_min, y_min = self.ground_corr.loc[timestamp]
# z_offset = z_min - min(corners[:,2])
# x_offset = x_min - min(corners[:,0])
# y_offset = y_min - min(corners[:,1])
# corr = np.array([0, 0, z_offset])
# #corr = np.array([0, 0,0])
# #corr = np.array([x_offset, y_offset, z_offset])
# corr[np.isnan(corr)]=0
# corners+=corr
# print(corners)
cameraModel = PinholeCameraModel()
cam_info = load_cam_info(self.calib_file)
cameraModel.fromCameraInfo(cam_info)
projected_pts = [cameraModel.project3dToPixel(list(pt) + [1]) for pt in corners]
projected_pts = np.array(projected_pts)
center = np.mean(projected_pts, axis=0)
out_img = drawBbox(out_img, projected_pts, color=color[::-1])
self.markOutput.publish(self.markerArray)
self.imgOutput.publish(bridge.cv2_to_imgmsg(out_img, 'bgr8'))
class GridToImageConverter():
def __init__(self):
self.image_topic = "wall_camera/image_raw"
self.image_info_topic = "wall_camera/camera_info"
self.point_topic = "/clicked_point"
self.frame_id = "/map"
self.occupancy
self.pixel_x = 350
self.pixel_y = 215
# What we do during shutdown
rospy.on_shutdown(self.cleanup)
# Create the OpenCV display window for the RGB image
self.cv_window_name = self.image_topic
cv.NamedWindow(self.cv_window_name, cv.CV_WINDOW_NORMAL)
cv.MoveWindow(self.cv_window_name, 25, 75)
# Create the cv_bridge object
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber(self.image_topic, Image, self.image_cb, queue_size = 1)
self.image_info_sub = rospy.Subscriber(self.image_info_topic, CameraInfo, self.image_info_cb, queue_size = 1)
self.point_sub = rospy.Subscriber(self.point_topic, PointStamped, self.point_cb, queue_size = 4)
self.points = []
self.ready_for_image = False
self.has_pic = False
self.camera_info_msg = None
self.tf_listener = tf.TransformListener()
# self.display_picture()
self.camera_model = PinholeCameraModel()
def point_cb(self, point_msg):
if len(self.points) < 4:
self.points.append(point_msg.point)
if len(self.points) == 1:
self.ready_for_image = True
print "Point stored from click: ", point_msg.point
def image_cb(self, img_msg):
if self.ready_for_image:
# Use cv_bridge() to convert the ROS image to OpenCV format
try:
frame = self.bridge.imgmsg_to_cv2(img_msg, "bgr8")
except CvBridgeError, e:
print e
# Convert the image to a Numpy array since most cv2 functions
# require Numpy arrays.
frame = np.array(frame, dtype=np.uint8)
print "Got array"
# Process the frame using the process_image() function
#display_image = self.process_image(frame)
# Display the image.
self.img = frame
# self.has_pic = True
if self.camera_info_msg is not None:
print "cx ", self.camera_model.cx()
print "cy ", self.camera_model.cy()
print "fx ", self.camera_model.fx()
print "fy ", self.camera_model.fy()
# Get the point from the clicked message and transform it into the camera frame
self.tf_listener.waitForTransform(img_msg.header.frame_id, self.frame_id, rospy.Time(0), rospy.Duration(0.5))
(trans,rot) = self.tf_listener.lookupTransform(img_msg.header.frame_id, self.frame_id, rospy.Time(0))
self.tfros = tf.TransformerROS()
tf_mat = self.tfros.fromTranslationRotation(trans, rot)
point_in_camera_frame = tuple(np.dot(tf_mat, np.array([self.points[0].x, self.points[0].y, self.points[0].z, 1.0])))[:3]
# Get the pixels related to the clicked point (the point must be in the camera frame)
pixel_coords = self.camera_model.project3dToPixel(point_in_camera_frame)
print "Pixel coords ", pixel_coords
# Get the unit vector related to the pixel coords
unit_vector = self.camera_model.projectPixelTo3dRay((int(pixel_coords[0]), int(pixel_coords[1])))
print "Unit vector ", unit_vector
# TF the unit vector of the pixel to the frame of the clicked point from the map frame
self.tf_listener.waitForTransform(self.frame_id, img_msg.header.frame_id, rospy.Time(0), rospy.Duration(0.5))
(trans,rot) = self.tf_listener.lookupTransform(self.frame_id, img_msg.header.frame_id, rospy.Time(0))
self.tfros = tf.TransformerROS()
tf_mat = self.tfros.fromTranslationRotation(trans, rot)
xyz = tuple(np.dot(tf_mat, np.array([unit_vector[0], unit_vector[1], unit_vector[2], 1.0])))[:3]
print "World xyz ", xyz
red = (0,125,255)
cv2.circle(self.img, (int(pixel_coords[0]), int(pixel_coords[1])), 30, red)
cv2.imshow(self.image_topic, self.img)
self.ready_for_image = False
self.points = []
# Process any keyboard commands
self.keystroke = cv.WaitKey(5)
if 32 <= self.keystroke and self.keystroke < 128:
cc = chr(self.keystroke).lower()
if cc == 'q':
# The user has press the q key, so exit
rospy.signal_shutdown("User hit q key to quit.")
class ARTagCornerGrabber():
def __init__(self):
self.file = os.environ['HOME'] + '/ar_tag_corners_' + str(rospy.Time.now().to_nsec()) + '.pickle'
self.lis = tf.TransformListener()
self.bridge = CvBridge()
self.model = PinholeCameraModel()
self.need_info = True
rospy.Subscriber('/camera/rgb/camera_info', CameraInfo, self.info_callback)
self.need_bounds = True
rospy.Subscriber('/object_bounds', PolygonStamped, self.bounds_callback)
while self.need_info and self.need_bounds:
rospy.sleep(0.01) # block until have CameraInfo and object bounds
self.corners = []
rospy.Subscriber('/camera/rgb/image_color', Image, self.image_callback, queue_size=1)
#while not rospy.is_shutdown():
# self.get_tag_frame()
def image_callback(self, image_msg):
print 'Causing an image to be!'
image = self.bridge.imgmsg_to_cv2(image_msg)
self.get_tag_frame(image_msg.header.stamp)
if len(self.corners) > 0:
for corner in self.corners:
cv2.circle(image, corner, 3, (255,0,255), -1)
self.corners = [] # clear the corners
cv2.imshow('Image with AR tag corners shown', image)
cv2.waitKey(1)
def info_callback(self, info):
if self.need_info:
self.model.fromCameraInfo(info)
self.need_info = False
rospy.loginfo('AR_TAG_PROJECTOR> Got CameraInfo!')
def bounds_callback(self, bounds):
if self.need_bounds:
self.bounds = bounds
self.need_bounds = False
rospy.loginfo('AR_TAG_PROJECTOR> Got object bounds!')
def get_tag_frame(self, time):
print 'Updating tf frame!'
corners = []
source_frame = self.bounds.header.frame_id
target_frame = '/camera_rgb_optical_frame'
if True: #self.lis.frameExists(source_frame):
try:
self.lis.waitForTransform(target_frame, source_frame, time, rospy.Duration(3.0))
for point in self.bounds.polygon.points:
ps = PointStamped()
ps.header.stamp = time
ps.header.frame_id = source_frame
ps.point.x = point.x
ps.point.y = point.y
ps.point.z = 0.0
corner_cam_frame = self.lis.transformPoint(target_frame, ps)
uv = self.model.project3dToPixel((corner_cam_frame.point.x,
corner_cam_frame.point.y,
corner_cam_frame.point.z))
corners.append(tuple(int(el) for el in uv))
self.corners = corners
print corners
with open(self.file, 'a') as f:
pickle.dump((time.to_time(), corners), f)
except tf.Exception:
rospy.loginfo('Could not do coordinate transformation.')
else:
rospy.loginfo('Frame {0} does not exist.'.format(source_frame))
class Seeker():
def __init__(self,classifier,cvbridge):
self.classifier = classifier
self.cvbridge = cvbridge
cam_info=rospy.wait_for_message("/camera/rgb/camera_info", CameraInfo, timeout=None)
# print "Waiting for image data"
# while not (len(self.classifier.subscriber.depth_img)>0):
# if not rospy.is_shutdown():
# rospy.sleep(1)
# print self.classifier.subscriber.depth_img
# else: break
# data = self.classifier.subscriber.depth_img #depth image should now match up with whatever rgb_image was last used in the classifier
# img = self.cvbridge.imgmsg_to_cv2(data) #, "32FC1"
# img = np.array(img, dtype=np.float32)
self.img_shape = (480,640) #np.shape(img)
self.imgproc=PC()
self.imgproc.fromCameraInfo(cam_info)
self.rect = np.array([np.empty((0,4))]) #Array of most recent valid rectangles
self.xyz = np.array([np.empty((0,3))]) #Array of most recent valid xyz position
self.vel = np.array([np.zeros(3)])
def seek(self):
t = 0.25
scale = 1.5
# rospy.sleep(1)
rect = self.track(t,scale)
if not np.size(rect)>0:
print "Trying again with larger rectangle"
for i in range(3): #let's try a couple more times...
t += 1
scale = scale * 1.5
rect = self.track(t,scale)
if np.size(rect)>0:
break
if not np.size(rect)>0:
print "seek: Object lost. Re-initializing search"
self.rect = np.array([np.empty((0,4))])
self.xyz = np.array([np.empty((0,3))])
self.vel = np.array([np.zeros(3)])
return
xyz = self.get_xyz(rect)
if len(xyz) > 0:
self.append_history(rect,xyz)
else:
print "Couldn't retrieve depth info. Make sure object is at least 3 ft from camera and unobstructed."
print "Trying again with larger rectangle"
for i in range(3): #let's try a couple more times...
t += 1
scale = scale * 1.5
rect = self.track(t,scale)
if len(rect)>0:
break
if not len(rect)>0:
print "\n ===== seek: Object lost. Re-initializing search \n"
self.rect = np.array([np.empty((0,4))])
self.xyz = np.array([np.empty((0,3))])
self.vel = np.array([np.zeros(3)])
#displaying feed
disp_img = self.classifier.subscriber.rgb_img
disp_img = self.cvbridge.imgmsg_to_cv2(disp_img, "bgr8")
disp_img = np.array(disp_img, dtype=np.uint8)
#overlaying result if available
if len(rect)>0:
# print "rect: ",rect
(x1,y1,x2,y2)=np.int32(rect[0])
cv2.rectangle(disp_img, (x1, y1), (x1+x2, y1+y2), (0, 255, 0), 2)
text = "X: %.1f Y: %.1f Z: %.1f" %(xyz[0],xyz[1],xyz[2])
cv2.putText(disp_img,text, (x1-10,y1-10),cv2.FONT_HERSHEY_PLAIN,1,(0, 255, 0),1)
cv2.imshow("Seek",disp_img)
cv2.waitKey(1)
def calc_avg(self):
avg_rect = np.zeros(4)
avg_xyz = np.zeros(3)
avg_vel = np.zeros(3)
n = len(self.rect)
for i in range(n):
avg_rect += self.rect[i]*(i+1) #greater weight given to more recent values
avg_xyz += self.xyz[i]*(i+1)
if i>0:
vel = (self.xyz[i]-self.xyz[i-1])*i
avg_vel+=vel
avg_rect = np.int32(avg_rect / (n*(n+1)/2))
avg_xyz = avg_xyz / (n*(n+1)/2)
if n>1:
avg_vel = avg_vel / (n*(n+1)/2)
return (avg_rect, avg_xyz, avg_vel)
def track(self,t,scale):
# t represents estimated time elapsed between the last image obtained and the coming one... one might guess 1 second... this can be made adaptive with time history data... [?]
if not len(self.xyz[0])>0:
# print "Seeker: track() called before position history exists"
self.initial_seek() #might as well do so here
return self.rect
(avg_rect, avg_xyz, avg_vel) = self.calc_avg()
guess_xyz = avg_xyz + t * avg_vel
gx,gy,gz = guess_xyz
(g_centroid_x, g_centroid_y) = np.array(self.imgproc.project3dToPixel((gx,gy,gz)))
ax1,ay1,ax2,ay2 = avg_rect
ax2 = ax2 * scale
ay2 = ay2 * scale
a_centroid_x = ax1 + (ax2/2)
a_centroid_y = ay1 + (ay2/2)
new_centroid_x = np.int((a_centroid_x + g_centroid_x)/2) #centroid of new rectangle encompassing both current rectangle and guessed rectangle
new_centroid_y = np.int((a_centroid_y + g_centroid_y)/2)
new_x2 = np.abs(a_centroid_x - g_centroid_x) + ax2 #proposed rectangle width
new_y2 = np.abs(a_centroid_y - g_centroid_y) + ay2 #proposed rectangle height
new_x1 = max(0,new_centroid_x - np.int(new_x2/2)) #make sure new rectangle's top & left edges are within image
new_y1 = max(0,new_centroid_y - np.int(new_y2/2))
new_x2 = min(new_x2, self.img_shape[1] - new_x1) #make sure new rectangle's bottom & right edges are within image
new_y2 = min(new_y2, self.img_shape[0] - new_y1)
new_rect = np.array([new_x1,new_y1,new_x2,new_y2])
# print "track: new_rect: ", new_rect
#######REPLACE THIS WITH SOMETHING FASTER TT__TT
# rect = self.classifier.seg_search(new_rect)
rect = self.seg_track(new_rect)
# print "track: rect: ",rect
return rect
def append_history(self,rect,xyz):
# newest valid values are last in the list
# print "Appending: ",rect, xyz
# print "Current Rect, Current xyz: ", self.rect,self.xyz
rect = np.array(rect).reshape(4)
xyz = np.array(xyz).reshape(3)
self.rect = np.append(self.rect,[rect],0)
self.xyz = np.append(self.xyz,[xyz],0)
# print "Stored Rect, Stored xyz: ", self.rect,self.xyz
if len(self.rect)>5: #both history arrays should be of the same length, so no need to check both
#remove values beyond the 5th
self.rect = self.rect[-5:]
self.xyz = self.xyz[-5:]
def initial_seek(self):
print "Searching for object. Don't move!"
rect = self.classifier.initial_search_timed(2)
if len(rect) > 0:
# print "Rect: ", rect
xyz = self.get_xyz(rect)
if len(xyz) > 0:
# print xyz
#initializes history
self.rect = np.array([rect])
self.xyz = np.array([xyz])
self.vel = np.array([np.zeros(3)])
else:
print "Couldn't retrieve depth info. Make sure object is at least 3 ft from camera and unobstructed."
def seg_track(self,seg_rect):
# A much faster but potentially less accurate tracking function using depth data instead of trained classifier
# Given rect coord potentially containing object, returns rect around object if found
(rgb_frame,depth_frame) = self.classifier.get_feed() #retrieves updated rgb and depth images
# disp_frame = copy(rgb_frame)
# cv2.rectangle(disp_frame, (x1, y1), (x1+x2, y1+y2), (0, 255, 0), 2)
# cv2.imshow('Searching',disp_frame)
# cv2.waitKey(0)
seg_rect = np.array(seg_rect).reshape(4)
x1,y1,x2,y2 = seg_rect
depth_seg = depth_frame[y1:(y1+y2),x1:(x1+x2)]
(avg_rect, avg_xyz, avg_vel) = self.calc_avg()
if (avg_vel[2]>0):
depth_min = avg_xyz[2]
depth_max = depth_min + avg_vel[2] * 0.5 #predicts how far the object may have travelled in... 0.5 sec [?]
else:
depth_max = avg_xyz[2]
depth_min = depth_max + avg_vel[2] * 0.5
mask = np.nan_to_num(depth_seg)
ind = np.nonzero(( (mask>(depth_min-0.05)) & (mask<(depth_max+0.05)) ))
mask = np.zeros(np.shape(mask))
mask[ind] = 1
kernel = np.ones((10,10),np.uint8)
# mask = cv2.erode(mask,kernel,iterations = 1)
# mask = cv2.dilate(mask,kernel,iterations = 1)
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel) #clean noise
# cv2.imshow('Searching',mask)
# cv2.waitKey(0)
ind = np.nonzero(mask)
if (len(ind[0])==0):
return np.empty((0,4))
# calculation accuracy rely on there being no other objects of similar depth in the segment
dx2 = np.int32(0.5 * ((max(ind[1]) - min(ind[1])) + avg_rect[2]))
dy2 = np.int32(0.5 * ((max(ind[0]) - min(ind[0])) + avg_rect[3]))
dcentroid_x = np.int32(np.mean(ind[1]))
dcentroid_y = np.int32(np.mean(ind[0]))
# dx2 = dy2 = (dx2+dy2)/2
dx1 = max(dcentroid_x - (dx2/2),0)
dy1 = max(dcentroid_y - (dy2/2),0)
dx2 = min(dx2,self.img_shape[1]-dx1)
dy2 = min(dy2,self.img_shape[0]-dy1)
sdx1 = x1+dx1
sdy1 = y1+dy1
dseg = rgb_frame[sdy1:(sdy1+dy2),sdx1:(sdx1+dx2)]
# cv2.imshow('Searching',dseg)
# cv2.waitKey(0)
#if the image segment doesn't pass the color histogram test, object isn't in the segment
if self.classifier.hist_test.compare(dseg):
rect = np.array([[sdx1,sdy1,dx2,dy2]])
else:
rect = np.empty((0,4))
return rect
def get_xyz(self,rect):
# data = rospy.wait_for_message("/camera/depth_registered/hw_registered/image_rect", Image, timeout=None)
data = self.classifier.subscriber.depth_img #depth image should now match up with whatever rgb_image was last used in the classifier
img = self.cvbridge.imgmsg_to_cv2(data) #, "32FC1"
img = np.array(img, dtype=np.float32)
rect = np.array(rect).reshape(4,)
x1,y1,x2,y2 = rect
frame = img[y1:(y1+y2),x1:(x1+x2)]
mask = np.nan_to_num(frame)
#takes mean of center pixel values, assuming object exists there and there's no hole in the object's depth map
seg = mask[int(0.45*y2):int(0.55*y2),int(0.45*x2):int(0.55*x2)]
seg_mean = np.mean(seg) #approximate depth value of the object. Take mean of remaining object pixels
if (seg_mean > 0.25): #if there's no hole in the map. Minimum operating distance is about 0.5 meters
seg_std = np.std(seg)
ind = np.nonzero(np.abs(mask-seg_mean) < ((1.5 * seg_std) + 0.025)) # Valid region should be within one standard deviation of the approximate mean[?]
if np.size(ind) == 0:
return np.empty((0,3))
mask_depth = mask[ind]
obj_depth = np.mean(mask_depth)
else: #backup method in case hole exists
ind_depth_calc = np.nonzero((mask>0.25) & (mask<5)) #
mask_depth = mask[ind_depth_calc]
mask_mean = np.mean(mask_depth)
mask_std = np.std(mask_depth)
ind = np.nonzero((np.abs(mask-mask_mean) < (max(0.25,0.5*mask_std))))
if np.size(ind) < np.int(0.5 *(np.size(mask))):
return np.empty((0,3))
obj_depth = np.mean(mask[ind]) # mean depth value of pixels representing object
# mask_depth = np.uint8(mask_depth*(255/np.max(mask_depth))) #normalize to greyscale
# mask = np.uint8(mask*(255/np.max(mask_depth)))
# retval,mask = cv2.threshold(mask,0,255,cv2.THRESH_BINARY+cv2.THRESH_OTSU) #threshold was designed for greyscale, but this works too.
# kernel = np.ones((5,5),np.uint8)
# mask = cv2.erode(mask,kernel,iterations = 1) #[?]
# ind = np.nonzero(mask)
# print "Object Depth: ", obj_depth
row = ind[0]+y1
column = ind[1]+x1
# blah = np.zeros(np.shape(img))
# blah[(row,column)] = 255
# cv2.imshow("Blah",blah)
# cv2.waitKey(1)
centroid_y = np.mean(row)
centroid_x = np.mean(column)
obj_coord = np.array(self.imgproc.projectPixelTo3dRay((centroid_x,centroid_y)))
scale=obj_depth/obj_coord[2] #scale the unit vector according to measured depth
# print "Centroid y,x,ray,depth: ", centroid_y,centroid_x,obj_coord,obj_depth
obj_coord_sc=obj_coord*scale
return obj_coord_sc
class GroundProjection():
def __init__(self, robot_name="birdbot0"):
# defaults overwritten by param
self.robot_name = robot_name
self.rectified_input = False
# Load homography
self.H = self.load_homography()
self.Hinv = np.linalg.inv(self.H)
self.pcm_ = PinholeCameraModel()
# Load checkerboard information
#self.board_ = self.load_board_info()
# wait until we have received the camera info message through ROS and then initialize
def initialize_pinhole_camera_model(self,camera_info):
self.ci_=camera_info
self.pcm_.fromCameraInfo(camera_info)
print("pinhole camera model initialized")
def vector2pixel(self, vec):
pixel = Pixel()
cw = self.ci_.width
ch = self.ci_.height
pixel.u = int(cw * vec.x)
pixel.v = int(ch * vec.y)
if (pixel.u < 0): pixel.u = 0
if (pixel.u > cw -1): pixel.u = cw - 1
if (pixel.v < 0): pixel.v = 0
if (pixel.v > ch - 1): pixel.v = 0
return pixel
def pixel2vector(self, pixel):
vec = Vector2D()
vec.x = pixel.u / self.ci_.width
vec.y = pixel.v / self.ci_.height
return vec
def vector2ground(self, vec):
pixel = self.vector2pixel(vec)
return self.pixel2ground(pixel)
def ground2vector(self, point):
pixel = self.ground2pixel(point)
return self.pixel2vector(pixel)
def pixel2ground(self,pixel):
uv_raw = np.array([pixel.u, pixel.v])
if not self.rectified_input:
uv_raw = self.pcm_.rectifyPoint(uv_raw)
#uv_raw = [uv_raw, 1]
uv_raw = np.append(uv_raw, np.array([1]))
ground_point = np.dot(self.H, uv_raw)
point = Point()
x = ground_point[0]
y = ground_point[1]
z = ground_point[2]
point.x = x/z
point.y = y/z
point.z = 0.0
return point
def ground2pixel(self, point):
ground_point = np.array([point.x, point.y, 1.0])
image_point = np.dot(self.Hinv, ground_point)
image_point = image_point / image_point[2]
pixel = Pixel()
if not self.rectified_input:
distorted_pixel = self.pcm_.project3dToPixel(image_point)
pixel.u = distorted_pixel[0]
pixel.v = distorted_pixel[1]
else:
pixel.u = image_point[0]
pixel.v = image_point[1]
def rectify(self, cv_image_raw):
'''Undistort image'''
cv_image_rectified = np.zeros(np.shape(cv_image_raw))
mapx = np.ndarray(shape=(self.pcm_.height, self.pcm_.width, 1), dtype='float32')
mapy = np.ndarray(shape=(self.pcm_.height, self.pcm_.width, 1), dtype='float32')
mapx, mapy = cv2.initUndistortRectifyMap(self.pcm_.K, self.pcm_.D, self.pcm_.R, self.pcm_.P, (self.pcm_.width, self.pcm_.height), cv2.CV_32FC1, mapx, mapy)
return cv2.remap(cv_image_raw, mapx, mapy, cv2.INTER_CUBIC, cv_image_rectified)
def load_homography(self):
'''Load homography (extrinsic parameters)'''
"""
filename = (get_duckiefleet_root() + "/calibrations/camera_extrinsic/" + self.robot_name + ".yaml")
if not os.path.isfile(filename):
#logger.warn("no extrinsic calibration parameters for {}, trying default".format(self.robot_name))
filename = (get_duckiefleet_root() + "/calibrations/camera_extrinsic/default.yaml")
if not os.path.isfile(filename):
#logger.error("can't find default either, something's wrong")
else:
data = yaml_load_file(filename)
else:
#rospy.loginfo("Using extrinsic calibration of " + self.robot_name)
data = yaml_load_file(filename)
#logger.info("Loaded homography for {}".format(os.path.basename(filename)))
return np.array(data['homography']).reshape((3,3))
"""
# hardcorded birdbot0 homography
#return np.array([[-1.3138222289537473e-05,-0.00016799247320246641, -0.18508764249409215],
# [0.0008702503150508597, -1.314730233433855e-06, -0.2779744199471874],
# [-7.940529271577331e-05,-0.006045110837995103,1.0]])
return np.array([[-5.987554218305854e-05,-0.00012542467699075597, -0.183339048091576],
[0.0008439191581241052, -3.098547600170272e-05, -0.28159137198032724],
[-0.00015406069103711482,-0.006343978853492832, 0.9999999999999999]])
class Colorama(object):
def __init__(self):
info_topic = camera_root + "/camera_info"
image_topic = camera_root + "/image_raw" # Change this to rect on boat
self.tf_listener = tf.TransformListener()
# Map id => [{color, error}, ...] for determining when a color is good
self.colored = {}
db_request = rospy.ServiceProxy("/database/requests", ObjectDBQuery)
self.make_request = lambda **kwargs: db_request(ObjectDBQueryRequest(**kwargs))
rospy.Service("/vision/buoy_colorizer", VisionRequest, self.got_request)
self.image_history = ImageHistory(image_topic)
# Wait for camera info, and exit if not found
try:
fprint("Waiting for camera info on: '{}'".format(info_topic))
camera_info_msg = rospy.wait_for_message(info_topic, CameraInfo, timeout=3)
except rospy.exceptions.ROSException:
fprint("Camera info not found! Terminating.", msg_color="red")
rospy.signal_shutdown("Camera not found!")
return
fprint("Camera info found!", msg_color="green")
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(camera_info_msg)
self.debug = DebugImage("/colorama/debug", prd=.5)
# These are color mappings from Hue values [0, 360]
self.color_map = {'red': np.radians(0), 'yellow': np.radians(60),
'green': np.radians(120), 'blue': np.radians(240)}
# Some tunable parameters
self.update_time = .5 # s
self.saturation_reject = 50
self.value_reject = 50
self.hue_error_reject = .4 # rads
# To decide when to accept color observations
self.error_tolerance = .4 # rads
self.spottings_req = 4
self.similarity_reject = .1 # If two colors are within this amount of error, reject
rospy.Timer(ros_t(self.update_time), self.do_update)
def valid_color(self, _id):
"""
Either returns valid color or None depending if the color is valid or not
"""
if _id not in self.colored:
return None
object_data = self.colored[_id]
print "object_data", object_data
# Maps color => [err1, err2, ..]
potential_colors = {}
for data in object_data:
color, err = data['color'], data['err']
if color not in potential_colors:
potential_colors[color] = []
potential_colors[color].append(err)
potential_colors_avg = {}
for color, errs in potential_colors.iteritems():
if len(errs) > self.spottings_req:
potential_colors_avg[color] = np.average(errs)
print "potential_colors_avg", potential_colors_avg
if len(potential_colors_avg) == 1:
# No debate about the color
color = potential_colors_avg.keys()[0]
err = potential_colors_avg[color]
fprint("Only one color found, error: {} (/ {})".format(err, self.error_tolerance))
if len(potential_colors[color]) > self.spottings_req and err <= self.error_tolerance:
return color
elif len(potential_colors_avg) > 1:
# More than one color, see if one of them is still valid
fprint("More than one color detected. Removing all.")
self.colored[_id] = []
return None
def got_request(self, req):
# Threading blah blah something unsafe
color = self.valid_color(req.target_id)
if color is None:
fprint("ID {} is NOT a valid color".format(req.target_id), msg_color='red')
return VisionRequestResponse(found=False)
return VisionRequestResponse(found=True, color=color)
def do_update(self, *args):
resp = self.make_request(name='all')
if resp.found:
# temp
time_of_marker = rospy.Time.now() - ros_t(.2) # header.stamp not filled out
image_holder = self.image_history.get_around_time(time_of_marker)
if not image_holder.contains_valid_image:
return
header = navigator_tools.make_header(frame="/enu", stamp=image_holder.time) #resp.objects[0].header
image = image_holder.image
self.debug.image = np.copy(image)
cam_tf = self.camera_model.tfFrame()
fprint("Getting transform between /enu and {}...".format(cam_tf))
self.tf_listener.waitForTransform("/enu", cam_tf, time_of_marker, ros_t(1))
t_mat44 = self.tf_listener.asMatrix(cam_tf, header) # homogenous 4x4 transformation matrix
for obj in resp.objects:
if len(obj.points) > 0 and obj.name == "totem":
fprint("{} {}".format(obj.id, "=" * 50))
print obj.position
# Get objects position in camera frame and make sure it's in view
object_cam = t_mat44.dot(np.append(navigator_tools.point_to_numpy(obj.position), 1))
object_px = map(int, self.camera_model.project3dToPixel(object_cam[:3]))
if not self._object_in_frame(object_cam):
continue
#print object_px
points_np = np.array(map(navigator_tools.point_to_numpy, obj.points))
# We dont want points below a certain level
points_np = points_np[points_np[:, 2] > -2.5]
# Shove ones in there to make homogenous points
points_np_homo = np.hstack((points_np, np.ones((points_np.shape[0], 1)))).T
points_cam = t_mat44.dot(points_np_homo).T
points_px = map(self.camera_model.project3dToPixel, points_cam[:, :3])
#print points_px
roi = self._get_ROI_from_points(points_px)
color_info = self._get_color_from_ROI(roi, image) # hue, string_color, error
bgr = (0, 0, 0)
if color_info is not None:
hue, color, error = color_info
c = (int(hue), 255, 255)
hsv = np.array([[c]])[:, :3].astype(np.uint8)
bgr = cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR)[0, 0]
bgr = tuple(bgr.astype(np.uint8).tolist())
if hue is not None:
if obj.id not in self.colored:
self.colored[obj.id] = []
self.colored[obj.id].append({'color':color, 'err':error})
if self.valid_color(obj.id):
fprint("COLOR IS VALID", msg_color='green')
self.make_request(cmd='{name}={bgr[2]},{bgr[1]},{bgr[0]},{_id}'.format(name=obj.name,_id= obj.id, bgr=bgr))
[cv2.circle(self.debug.image, tuple(map(int, p)), 2, bgr, -1) for p in points_px]
cv2.circle(self.debug.image, tuple(object_px), 10, bgr, -1)
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(self.debug.image, str(obj.id), tuple(object_px), font, 1, bgr, 2)
print '\n' * 2
def _get_ROI_from_points(self, image_points):
# Probably will return a set of contours
cnt = np.array(image_points).astype(np.float32)
rect = cv2.minAreaRect(cnt)
box = cv2.cv.BoxPoints(rect)
box = np.int0(box)
fprint("Drawing rectangle")
cv2.drawContours(self.debug.image, [box], 0, (0, 0, 255), 2)
return box
def _get_color_from_ROI(self, roi, img):
mask = np.zeros(img.shape[:2], np.uint8)
cv2.drawContours(mask, [roi], 0, 255, -1)
bgr = cv2.mean(img, mask)
# We have to treat that bgr value as a 3d array to get cvtColor to work
bgr = np.array([[bgr]])[:, :3].astype(np.uint8)
h, s, v = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)[0, 0]
# Now check that s and v are in a good range
if s < self.saturation_reject or v < self.value_reject:
fprint("The colors aren't expressive enough. Rejecting.", msg_color='red')
return None
# Compute hue error in SO2
hue_angle = np.radians(h * 2)
c = np.cos(hue_angle)
s = np.sin(hue_angle)
error = np.inf
likely_color = 'bazinga'
for color, h_val in self.color_map.iteritems():
cg = np.cos(h_val)
sg = np.sin(h_val)
this_error = np.abs(np.arctan2(sg*c - cg*s, cg*c + sg*s))
if this_error < error:
error = this_error
likely_color = color
if error > self.hue_error_reject:
fprint("Closest color was {} with an error of {} rads (> {}) Rejecting.".format(likely_color, np.round(error, 3),
self.hue_error_reject), msg_color='red')
return None
fprint("Likely color: {} with an hue error of {} rads.".format(likely_color, np.round(error, 3)))
return [np.degrees(self.color_map[likely_color]) / 2.0, likely_color, error]
def _object_in_frame(self, object_point):
"""
Returns if the object is in frame given the centroid of the object.
`object_point` should be in the camera_model's frame.
"""
print object_point
if object_point[2] < 0:
return False
px = np.array(self.camera_model.project3dToPixel(object_point))
resoultion = self.camera_model.fullResolution()
if np.any([0,0] > px) or np.any(px > resoultion):
return False
return True
def labelData(self):
# Detected and idxs values to False and [], to make sure we are not using information from a previous labelling
self.labels['detected'] = False
self.labels['idxs'] = []
# Labelling process dependent of the sensor type
if self.msg_type_str == 'LaserScan': # 2D LIDARS -------------------------------------
# For 2D LIDARS the process is the following: First cluster all the range data into clusters. Then,
# associate one of the clusters with the calibration pattern by selecting the cluster which is closest to
# the rviz interactive marker.
clusters = [] # initialize cluster list to empty
cluster_counter = 0 # init counter
points = [] # init points
# Compute cartesian coordinates
xs, ys = interactive_calibration.utilities.laser_scan_msg_to_xy(self.msg)
# Clustering:
first_iteration = True
for idx, r in enumerate(self.msg.ranges):
# Skip if either this point or the previous have range smaller than minimum_range_value
if r < self.minimum_range_value or self.msg.ranges[idx - 1] < self.minimum_range_value:
continue
if first_iteration: # if first iteration, create a new cluster
clusters.append(LaserScanCluster(cluster_counter, idx))
first_iteration = False
else: # check if new point belongs to current cluster, create new cluster if not
x = xs[clusters[-1].idxs[-1]] # x coordinate of last point of last cluster
y = ys[clusters[-1].idxs[-1]] # y coordinate of last point of last cluster
distance = math.sqrt((xs[idx] - x) ** 2 + (ys[idx] - y) ** 2)
if distance > self.threshold: # if distance larger than threshold, create new cluster
cluster_counter += 1
clusters.append(LaserScanCluster(cluster_counter, idx))
else: # same cluster, push this point into the same cluster
clusters[-1].pushIdx(idx)
# Association stage: find out which cluster is closer to the marker
x_marker, y_marker = self.marker.pose.position.x, self.marker.pose.position.y # interactive marker pose
idx_closest_cluster = 0
min_dist = sys.maxint
for cluster_idx, cluster in enumerate(clusters): # cycle all clusters
for idx in cluster.idxs: # cycle each point in the cluster
x, y = xs[idx], ys[idx]
dist = math.sqrt((x_marker - x) ** 2 + (y_marker - y) ** 2)
if dist < min_dist:
idx_closest_cluster = cluster_idx
min_dist = dist
closest_cluster = clusters[idx_closest_cluster]
# Find the coordinate of the middle point in the closest cluster and bring the marker to that point
x_sum, y_sum = 0, 0
for idx in closest_cluster.idxs:
x_sum += xs[idx]
y_sum += ys[idx]
self.marker.pose.position.x = x_sum / float(len(closest_cluster.idxs))
self.marker.pose.position.y = y_sum / float(len(closest_cluster.idxs))
self.marker.pose.position.z = 0
self.menu_handler.reApply(self.server)
self.server.applyChanges()
# Update the dictionary with the labels
self.labels['detected'] = True
percentage_points_to_remove = 0.0 # remove x% of data from each side
number_of_idxs = len(clusters[idx_closest_cluster].idxs)
idxs_to_remove = int(percentage_points_to_remove * float(number_of_idxs))
clusters[idx_closest_cluster].idxs_filtered = clusters[idx_closest_cluster].idxs[
idxs_to_remove:number_of_idxs - idxs_to_remove]
self.labels['idxs'] = clusters[idx_closest_cluster].idxs_filtered
# Create and publish point cloud message with the colored clusters (just for debugging)
cmap = cm.prism(np.linspace(0, 1, len(clusters)))
points = []
z, a = 0, 255
for cluster in clusters:
for idx in cluster.idxs:
x, y = xs[idx], ys[idx]
r, g, b = int(cmap[cluster.cluster_count, 0] * 255.0), \
int(cmap[cluster.cluster_count, 1] * 255.0), \
int(cmap[cluster.cluster_count, 2] * 255.0)
rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0]
pt = [x, y, z, rgb]
points.append(pt)
fields = [PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1), PointField('rgba', 12, PointField.UINT32, 1)]
header = Header()
header.frame_id = self.parent
header.stamp = self.msg.header.stamp
pc_msg = point_cloud2.create_cloud(header, fields, points)
self.publisher_clusters.publish(pc_msg)
# Create and publish point cloud message containing only the selected calibration pattern points
points = []
for idx in clusters[idx_closest_cluster].idxs_filtered:
x_marker, y_marker, z_marker = xs[idx], ys[idx], 0
r = int(0 * 255.0)
g = int(0 * 255.0)
b = int(1 * 255.0)
a = 255
rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0]
pt = [x_marker, y_marker, z_marker, rgb]
points.append(pt)
pc_msg = point_cloud2.create_cloud(header, fields, points)
self.publisher_selected_points.publish(pc_msg)
elif self.msg_type_str == 'Image': # Cameras -------------------------------------------
# Convert to opencv image and save image to disk
image = self.bridge.imgmsg_to_cv2(self.msg, "bgr8")
result = self.pattern.detect(image)
if result['detected']:
c = []
for corner in result['keypoints']:
c.append({'x': float(corner[0][0]), 'y': float(corner[0][1])})
x = int(round(c[0]['x']))
y = int(round(c[0]['y']))
cv2.line(image, (x, y), (x, y), (0, 255, 255), 20)
# Update the dictionary with the labels
self.labels['detected'] = True
self.labels['idxs'] = c
# For visual debugging
self.pattern.drawKeypoints(image, result)
msg_out = self.bridge.cv2_to_imgmsg(image, encoding="passthrough")
msg_out.header.stamp = self.msg.header.stamp
msg_out.header.frame_id = self.msg.header.frame_id
self.publisher_labelled_image.publish(msg_out)
elif self.msg_type_str == 'PointCloud2': # RGB-D pointcloud -------------------------------------------
# print("Found point cloud!")
# Get 3D coords
points = pc2.read_points_list(self.msg, skip_nans=False, field_names=("x", "y", "z"))
# Get the marker position
x_marker, y_marker, z_marker = self.marker.pose.position.x, self.marker.pose.position.y, self.marker.pose.position.z # interactive marker pose
cam_model = PinholeCameraModel()
# Wait for camera info message
camera_info = rospy.wait_for_message('/top_center_rgbd_camera/depth/camera_info', CameraInfo)
cam_model.fromCameraInfo(camera_info)
# Project points
seed_point = cam_model.project3dToPixel((x_marker, y_marker, z_marker))
seed_point = (int(math.floor(seed_point[0])), int(math.floor(seed_point[1])))
# Wait for depth image message
imgmsg = rospy.wait_for_message('/top_center_rgbd_camera/depth/image_rect', Image)
# img = self.bridge.imgmsg_to_cv2(imgmsg, desired_encoding="8UC1")
img = self.bridge.imgmsg_to_cv2(imgmsg, desired_encoding="passthrough")
img_float = img.astype(np.float32)
img_float = img_float / 1000
# print('img type = ' + str(img.dtype))
# print('img_float type = ' + str(img_float.dtype))
# print('img_float shape = ' + str(img_float.shape))
h, w = img.shape
mask = np.zeros((h + 2, w + 2, 1), np.uint8)
# mask[seed_point[1] - 2:seed_point[1] + 2, seed_point[0] - 2:seed_point[0] + 2] = 255
img_float2 = deepcopy(img_float)
cv2.floodFill(img_float2, mask, seed_point, 128, 0.1, 0.1,
8 | (128 << 8) | cv2.FLOODFILL_MASK_ONLY) # | cv2.FLOODFILL_FIXED_RANGE)
# Switch coords of seed point
# mask[seed_point[1]-2:seed_point[1]+2, seed_point[0]-2:seed_point[0]+2] = 255
tmpmask = mask[1:h + 1, 1:w + 1]
# calculate moments of binary image
M = cv2.moments(tmpmask)
if M["m00"] != 0:
# calculate x,y coordinate of center
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
mask[cY-2:cY+2, cX-2:cX+2] = 255
cv2.imshow("mask", mask)
cv2.waitKey(20)
# msg_out = self.bridge.cv2_to_imgmsg(showcenter, encoding="passthrough")
# msg_out.header.stamp = self.msg.header.stamp
# msg_out.header.frame_id = self.msg.header.frame_id
# self.publisher_labelled_depth_image.publish(msg_out)
coords = points[cY * 640 + cX]
if not math.isnan(coords[0]):
self.marker.pose.position.x = coords[0]
self.marker.pose.position.y = coords[1]
self.marker.pose.position.z = coords[2]
self.menu_handler.reApply(self.server)
self.server.applyChanges()
class LidarCameraCalibration():
def __init__(
self,
settingsFile,
cameraInfoFile,
):
""" Init the cameraModel as PinholeCameraModel with camera intrinsics and params
=============
settingsFile : json file storing 3D-2D point pairs and params for optimization
cameraInfoFile : yaml file storing camera intrinsics and params
"""
self.settings = json.load(settingsFile)
self.cameraParams = yaml.load(cameraInfoFile)
self.cameraModel = PinholeCameraModel()
self.cameraInfo = CameraInfo()
self._fillCameraInfo()
self.cameraModel.fromCameraInfo(self.cameraInfo)
print('Camera Model is initialized, ready to calibrate ...\n')
def calibrateTranformation(self, ):
""" Calculate the transformation/extrinsics between lidar and camera by minimizing the reprojection errors, providing camera model and params and 2d-3d correspondence points.
"""
print('Calibrating the transformation ... ')
result = minimize(self.costFuction,
self.settings['initialTransform'],
args=(1.0),
bounds=self.settings['bounds'],
method='SLSQP',
options={
'disp': True,
'maxiter': 500
})
while not result.success or result.fun > 25:
for i in range(0, len(self.settings['initialTransform'])):
self.settings['initialTransform'][i] = random.uniform(
self.settings['bounds'][i][0],
self.settings['bounds'][i][1])
print('\nRestart with new initialTransform ...\n')
result = minimize(self.costFuction,
self.settings['initialTransform'],
args=(1.0),
bounds=self.settings['bounds'],
method='SLSQP',
options={
'disp': True,
'maxiter': 500
})
print('Calibration done ... ')
print('Final transformation:')
print(result.x)
print('Error: ' + str(result.fun))
def costFuction(self, x, sign=1):
""" Define the cost function with reprojection errors. Reprojection error is defined as the difference between ground-truth image points and reprojected image points from the corresponding lidar 3d points.
=============
x : the initial guess of the transformation (x, y, z, yaw, pitch, roll)
sign : ditermines minimization (+1) or maximization (-1) since the 'from scipy.optimize import minimize' is used.
"""
params = x
translation = [params[0], params[1], params[2], 1.0]
rotationMatrix = tf.transformations.euler_matrix(
params[5], params[4], params[3])
rotationMatrix[:, 3] = translation
error = 0
for point, uv in zip(self.settings['points'], self.settings['uvs']):
rotatedPoint = rotationMatrix.dot(point)
uv_new = self.cameraModel.project3dToPixel(rotatedPoint)
diff = np.array(uv_new) - np.array(uv)
error += math.sqrt(np.sum(diff**2))
return error * sign
def _fillCameraInfo(self, ):
""" Fill the camera params from yaml file to the sensor_msgs/CameraInfo
"""
self.cameraInfo.width = self.cameraParams['image_width']
self.cameraInfo.height = self.cameraParams['image_height']
self.cameraInfo.distortion_model = self.cameraParams[
'distortion_model']
self.cameraInfo.D = self.cameraParams['distortion_coefficients'][
"data"]
self.cameraInfo.R = self.cameraParams['rectification_matrix']["data"]
self.cameraInfo.P = self.cameraParams['projection_matrix']["data"]
self.cameraInfo.K = self.cameraParams['camera_matrix']["data"]
class InteractiveDataLabeler:
"""
Handles data labelling for a generic sensor:
Cameras: Fully automated labelling. Periodically runs a chessboard detection on the newly received image.
LaserScans: Semi-automated labelling. An rviz interactive marker is placed on the laser cluster which contains
the calibration pattern, and the pattern is tracked from there onward.
PointCloud2: #TODO Tiago Madeira can you complete?
"""
def __init__(self, server, menu_handler, sensor_dict, marker_scale,
calib_pattern):
"""
Class constructor. Initializes several variables and ros stuff.
:param server: an interactive marker server
:param menu_handler: an interactive MenuHandler
:param sensor_dict: A dictionary that describes the sensor
:param marker_scale: scale of the markers to be drawn
:param chess_numx: chessboard size in x
:param chess_numy: chessboard size in y
"""
print('Creating an InteractiveDataLabeler for sensor ' +
str(sensor_dict['_name']))
# Store variables to class attributes
self.server = server
self.menu_handler = menu_handler
self.name = sensor_dict['_name']
self.parent = sensor_dict['parent']
self.topic = sensor_dict['topic']
self.marker_scale = marker_scale
self.received_first_msg = False
self.labels = {'detected': False, 'idxs': []}
self.lock = threading.Lock()
# self.calib_pattern = calib_pattern
if calib_pattern['pattern_type'] == 'chessboard':
self.pattern = patterns.ChessboardPattern(
calib_pattern['dimension'], calib_pattern['size'])
elif calib_pattern['pattern_type'] == 'charuco':
self.pattern = patterns.CharucoPattern(calib_pattern['dimension'],
calib_pattern['size'],
calib_pattern['inner_size'],
calib_pattern['dictionary'])
print(calib_pattern['dictionary'])
else:
print("Unknown pattern type '{}'".format(
calib_pattern['pattern_type']))
sys.exit(1)
# Get the type of message from the message topic of the sensor data, which is given as input. The message
# type is used to define which labelling technique is used.
self.msg_type_str, self.msg_type = atom_core.utilities.getMessageTypeFromTopic(
self.topic)
print('msg_type_str is = ' + str(self.msg_type_str))
# Handle the interactive labelling of data differently according to the sensor message types.
if self.msg_type_str in ['LaserScan']:
# TODO parameters given from a command line input?
self.threshold = 0.2 # pt to pt distance to create new cluster (param only for 2D LIDAR labelling)
self.minimum_range_value = 0.3 # distance to assume range value valid (param only for 2D LIDAR labelling)
self.publisher_selected_points = rospy.Publisher(
self.topic + '/labeled',
sensor_msgs.msg.PointCloud2,
queue_size=0) # publish a point cloud with the points
# in the selected cluster
self.publisher_clusters = rospy.Publisher(
self.topic + '/clusters',
sensor_msgs.msg.PointCloud2,
queue_size=0) # publish a point cloud with coloured clusters
self.createInteractiveMarker(
) # interactive marker to label the calibration pattern cluster (one time)
print('Created interactive marker for laser scans.')
elif self.msg_type_str in ['Image']:
self.bridge = CvBridge(
) # a CvBridge structure is needed to convert opencv images to ros messages.
self.publisher_labelled_image = rospy.Publisher(
self.topic + '/labeled', sensor_msgs.msg.Image,
queue_size=0) # publish
# images with the detected chessboard overlaid onto the image.
elif self.msg_type_str in [
'PointCloud2TIAGO'
]: # TODO, this will have to be revised later on Check #44
self.publisher_selected_points = rospy.Publisher(
self.topic + '/labeled',
sensor_msgs.msg.PointCloud2,
queue_size=0) # publish a point cloud with the points
self.createInteractiveMarkerRGBD(
) # interactive marker to label the calibration pattern cluster (one time)
self.bridge = CvBridge()
self.publisher_labelled_depth_image = rospy.Publisher(
self.topic + '/depth_image_labelled',
sensor_msgs.msg.Image,
queue_size=0) # publish
self.cam_model = PinholeCameraModel()
topic_name = os.path.dirname(self.topic) + '/camera_info'
rospy.loginfo('Waiting for for camera info message on topic' +
str(topic_name))
camera_info = rospy.wait_for_message(
'/top_center_rgbd_camera/depth/camera_info', CameraInfo)
print('... received!')
self.cam_model.fromCameraInfo(camera_info)
print('Created interactive marker for point clouds.')
elif self.msg_type_str in ['PointCloud2'
]: # Velodyne data (Andre Aguiar)
self.publisher_selected_points = rospy.Publisher(
self.topic + '/labeled',
sensor_msgs.msg.PointCloud2,
queue_size=0) # publish a point cloud with the points
self.createInteractiveMarkerRGBD(
x=0.804, y=0.298, z=-0.109
) # interactive marker to label the calibration pattern
# cluster (one time)
# Labeler definitions
# Hessian plane coefficients
self.A = 0
self.B = 0
self.C = 0
self.D = 0
self.n_inliers = 0 # RANSAC number of inliers initialization
self.number_iterations = 30 # RANSAC number of iterations
self.ransac_threshold = 0.02 # RANSAC point-to-plane distance threshold to consider inliers
# Chessboard point tracker distance threshold
self.tracker_threshold = math.sqrt(((calib_pattern['dimension']['x'] - 1) * calib_pattern['size']) ** 2 + \
((calib_pattern['dimension']['y'] - 1) * calib_pattern['size']) ** 2)
print('Created interactive marker for point clouds.')
else:
# We handle only know message types
raise ValueError('Message type ' + self.msg_type_str +
' for topic ' + self.topic +
'is of an unknown type.')
# self.publisher = rospy.Publisher(self.topic + '/labeled', self.msg_type, queue_size=0)
# Subscribe to the message topic containing sensor data
print(self.msg_type)
self.subscriber = rospy.Subscriber(self.topic,
self.msg_type,
self.sensorDataReceivedCallback,
queue_size=None)
def sensorDataReceivedCallback(self, msg):
self.lock.acquire(
) # use semaphores to make sure the data is not being written on two sides simultaneously
self.msg = msg # make a local copy of sensor data
# now = rospy.Time.now()
self.labelData # label the data
# rospy.loginfo('Labelling data for ' + self.name + ' took ' + str((rospy.Time.now() - now).to_sec()) + ' secs.')
self.lock.release() # release lock
@property
def labelData(self):
# Detected and idxs values to False and [], to make sure we are not using information from a previous labelling
self.labels['detected'] = False
self.labels['idxs'] = []
# Labelling process dependent of the sensor type
if self.msg_type_str == 'LaserScan': # 2D LIDARS -------------------------------------
# For 2D LIDARS the process is the following: First cluster all the range data into clusters. Then,
# associate one of the clusters with the calibration pattern by selecting the cluster which is closest to
# the rviz interactive marker.
clusters = [] # initialize cluster list to empty
cluster_counter = 0 # init counter
points = [] # init points
# Compute cartesian coordinates
xs, ys = atom_core.utilities.laser_scan_msg_to_xy(self.msg)
# Clustering:
first_iteration = True
for idx, r in enumerate(self.msg.ranges):
# Skip if either this point or the previous have range smaller than minimum_range_value
if r < self.minimum_range_value or self.msg.ranges[
idx - 1] < self.minimum_range_value:
continue
if first_iteration: # if first iteration, create a new cluster
clusters.append(LaserScanCluster(cluster_counter, idx))
first_iteration = False
else: # check if new point belongs to current cluster, create new cluster if not
x = xs[clusters[-1].idxs[
-1]] # x coordinate of last point of last cluster
y = ys[clusters[-1].idxs[
-1]] # y coordinate of last point of last cluster
distance = math.sqrt((xs[idx] - x)**2 + (ys[idx] - y)**2)
if distance > self.threshold: # if distance larger than threshold, create new cluster
cluster_counter += 1
clusters.append(LaserScanCluster(cluster_counter, idx))
else: # same cluster, push this point into the same cluster
clusters[-1].pushIdx(idx)
# Association stage: find out which cluster is closer to the marker
x_marker, y_marker = self.marker.pose.position.x, self.marker.pose.position.y # interactive marker pose
idx_closest_cluster = 0
min_dist = sys.maxint
for cluster_idx, cluster in enumerate(
clusters): # cycle all clusters
for idx in cluster.idxs: # cycle each point in the cluster
x, y = xs[idx], ys[idx]
dist = math.sqrt((x_marker - x)**2 + (y_marker - y)**2)
if dist < min_dist:
idx_closest_cluster = cluster_idx
min_dist = dist
closest_cluster = clusters[idx_closest_cluster]
# Find the coordinate of the middle point in the closest cluster and bring the marker to that point
x_sum, y_sum = 0, 0
for idx in closest_cluster.idxs:
x_sum += xs[idx]
y_sum += ys[idx]
self.marker.pose.position.x = x_sum / float(
len(closest_cluster.idxs))
self.marker.pose.position.y = y_sum / float(
len(closest_cluster.idxs))
self.marker.pose.position.z = 0
self.menu_handler.reApply(self.server)
self.server.applyChanges()
# Update the dictionary with the labels
self.labels['detected'] = True
percentage_points_to_remove = 0.0 # remove x% of data from each side
number_of_idxs = len(clusters[idx_closest_cluster].idxs)
idxs_to_remove = int(percentage_points_to_remove *
float(number_of_idxs))
clusters[idx_closest_cluster].idxs_filtered = clusters[
idx_closest_cluster].idxs[idxs_to_remove:number_of_idxs -
idxs_to_remove]
self.labels['idxs'] = clusters[idx_closest_cluster].idxs_filtered
# Create and publish point cloud message with the colored clusters (just for debugging)
cmap = cm.prism(np.linspace(0, 1, len(clusters)))
points = []
z, a = 0, 255
for cluster in clusters:
for idx in cluster.idxs:
x, y = xs[idx], ys[idx]
r, g, b = int(cmap[cluster.cluster_count, 0] * 255.0), \
int(cmap[cluster.cluster_count, 1] * 255.0), \
int(cmap[cluster.cluster_count, 2] * 255.0)
rgb = struct.unpack('I', struct.pack('BBBB', b, g, r,
a))[0]
pt = [x, y, z, rgb]
points.append(pt)
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('rgba', 12, PointField.UINT32, 1)
]
header = Header()
header.frame_id = self.parent
header.stamp = self.msg.header.stamp
pc_msg = point_cloud2.create_cloud(header, fields, points)
self.publisher_clusters.publish(pc_msg)
# Create and publish point cloud message containing only the selected calibration pattern points
points = []
for idx in clusters[idx_closest_cluster].idxs_filtered:
x_marker, y_marker, z_marker = xs[idx], ys[idx], 0
r = int(0 * 255.0)
g = int(0 * 255.0)
b = int(1 * 255.0)
a = 255
rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0]
pt = [x_marker, y_marker, z_marker, rgb]
points.append(pt)
pc_msg = point_cloud2.create_cloud(header, fields, points)
self.publisher_selected_points.publish(pc_msg)
elif self.msg_type_str == 'Image': # Cameras -------------------------------------------
# Convert to opencv image and save image to disk
image = self.bridge.imgmsg_to_cv2(self.msg, "bgr8")
result = self.pattern.detect(image, equalize_histogram=True)
if result['detected']:
c = []
if result.has_key('ids'):
# The charuco pattern also return an ID for each keypoint.
# We can use this information for partial detections.
for idx, corner in enumerate(result['keypoints']):
c.append({
'x': float(corner[0][0]),
'y': float(corner[0][1]),
'id': result['ids'][idx]
})
else:
for corner in result['keypoints']:
c.append({
'x': float(corner[0][0]),
'y': float(corner[0][1])
})
x = int(round(c[0]['x']))
y = int(round(c[0]['y']))
cv2.line(image, (x, y), (x, y), (0, 255, 255), 20)
# Update the dictionary with the labels
self.labels['detected'] = True
self.labels['idxs'] = c
# For visual debugging
self.pattern.drawKeypoints(image, result)
msg_out = self.bridge.cv2_to_imgmsg(image, encoding="passthrough")
msg_out.header.stamp = self.msg.header.stamp
msg_out.header.frame_id = self.msg.header.frame_id
self.publisher_labelled_image.publish(msg_out)
elif self.msg_type_str == 'PointCloud2TIAGO': # RGB-D pointcloud -------------------------------------------
# TODO, this will have to be revised later on Check #44
# print("Found point cloud!")
tall = rospy.Time.now()
# Get 3D coords
t = rospy.Time.now()
# points = pc2.read_points_list(self.msg, skip_nans=False, field_names=("x", "y", "z"))
print('0. took ' + str((rospy.Time.now() - t).to_sec()))
# Get the marker position
x_marker, y_marker, z_marker = self.marker.pose.position.x, self.marker.pose.position.y, self.marker.pose.position.z # interactive marker pose
t = rospy.Time.now()
# Project points
print('x_marker=' + str(x_marker))
print('y_marker=' + str(y_marker))
print('z_marker=' + str(z_marker))
seed_point = self.cam_model.project3dToPixel(
(x_marker, y_marker, z_marker))
print('seed_point = ' + str(seed_point))
if np.isnan(
seed_point[0]
): # something went wrong, reposition marker on initial position and return
self.marker.pose.position.x = 0
self.marker.pose.position.y = 0
self.marker.pose.position.z = 4
self.menu_handler.reApply(self.server)
self.server.applyChanges()
rospy.logwarn(
'Could not project pixel, putting marker in home position.'
)
return
seed_point = (int(round(seed_point[0])), int(round(seed_point[1])))
# Check if projection is inside the image
x = seed_point[0]
y = seed_point[1]
if x < 0 or x >= self.cam_model.width or y < 0 or y >= self.cam_model.height:
rospy.logwarn(
'Projection of point is outside of image. Not labelling point cloud.'
)
return
print('1. took ' + str((rospy.Time.now() - t).to_sec()))
t = rospy.Time.now()
# Wait for depth image message
imgmsg = rospy.wait_for_message(
'/top_center_rgbd_camera/depth/image_rect', Image)
print('2. took ' + str((rospy.Time.now() - t).to_sec()))
t = rospy.Time.now()
# img = self.bridge.imgmsg_to_cv2(imgmsg, desired_encoding="8UC1")
img_raw = self.bridge.imgmsg_to_cv2(imgmsg,
desired_encoding="passthrough")
img = deepcopy(img_raw)
img_float = img.astype(np.float32)
img_float = img_float
h, w = img.shape
# print('img type = ' + str(img.dtype))
# print('img_float type = ' + str(img_float.dtype))
# print('img_float shape = ' + str(img_float.shape))
mask = np.zeros((h + 2, w + 2, 1), np.uint8)
# mask[seed_point[1] - 2:seed_point[1] + 2, seed_point[0] - 2:seed_point[0] + 2] = 255
# PCA + Consensus + FloodFill ------------------
# get 10 points around the seed
# seed = {'x': seed_point[0], 'y': seed_point[1]}
# pts = []
# pts.append({'x': seed['x'], 'y': seed['y'] - 10}) # top neighbor
# pts.append({'x': seed['x'], 'y': seed['y'] + 10}) # bottom neighbor
# pts.append({'x': seed['x'] - 1, 'y': seed['y']}) # left neighbor
# pts.append({'x': seed['x'] + 1, 'y': seed['y']}) # right neighbor
#
# def fitPlaneLTSQ(XYZ):
# (rows, cols) = XYZ.shape
# G = np.ones((rows, 3))
# G[:, 0] = XYZ[:, 0] # X
# G[:, 1] = XYZ[:, 1] # Y
# Z = XYZ[:, 2]
# (a, b, c), resid, rank, s = np.linalg.lstsq(G, Z)
# normal = (a, b, -1)
# nn = np.linalg.norm(normal)
# normal = normal / nn
# return (c, normal)
#
# data = np.random.randn(100, 3) / 3
# data[:, 2] /= 10
# c, normal = fitPlaneLTSQ(data)
# out flood fill ------------------
# to_visit = [{'x': seed_point[0], 'y': seed_point[1]}]
# # filled = []
# threshold = 0.05
# filled_img = np.zeros((h, w), dtype=np.bool)
# visited_img = np.zeros((h, w), dtype=np.bool)
#
# def isInsideBox(p, min_x, max_x, min_y, max_y):
# if min_x <= p['x'] < max_x and min_y <= p['y'] < max_y:
# return True
# else:
# return False
#
# def getNotVisitedNeighbors(p, min_x, max_x, min_y, max_y, img):
# neighbors = []
# tmp_neighbors = []
# tmp_neighbors.append({'x': p['x'], 'y': p['y'] - 1}) # top neighbor
# tmp_neighbors.append({'x': p['x'], 'y': p['y'] + 1}) # bottom neighbor
# tmp_neighbors.append({'x': p['x'] - 1, 'y': p['y']}) # left neighbor
# tmp_neighbors.append({'x': p['x'] + 1, 'y': p['y']}) # right neighbor
#
# for idx, n in enumerate(tmp_neighbors):
# if isInsideBox(n, min_x, max_x, min_y, max_y) and not img[n['y'], n['x']] == True:
# neighbors.append(n)
#
# return neighbors
#
# cv2.namedWindow('Filled', cv2.WINDOW_NORMAL)
# cv2.namedWindow('Visited', cv2.WINDOW_NORMAL)
# cv2.namedWindow('To Visit', cv2.WINDOW_NORMAL)
# while to_visit != []:
# p = to_visit[0]
# # print('Visiting ' + str(p))
# range_p = img_float[p['y'], p['x']]
# to_visit.pop(0) # remove p from to_visit
# # filled.append(p) # append p to filled
# filled_img[p['y'], p['x']] = True
# # print(filled)
#
# # compute neighbors of this point
# neighbors = getNotVisitedNeighbors(p, 0, w, 0, h, visited_img)
#
# # print('neighbors ' + str(neighbors))
#
# for n in neighbors: # test if should propagate to neighbors
# range_n = img_float[n['y'], n['x']]
# visited_img[n['y'], n['x']] = True
#
# if abs(range_n - range_p) <= threshold:
# # if not n in to_visit:
# to_visit.append(n)
#
# # Create the mask image
# to_visit_img = np.zeros((h, w), dtype=np.bool)
# for p in to_visit:
# to_visit_img[p['y'], p['x']] = True
#
#
# # print('To_visit ' + str(to_visit))
#
# cv2.imshow('Filled', filled_img.astype(np.uint8) * 255)
# cv2.imshow('Visited', visited_img.astype(np.uint8) * 255)
# cv2.imshow('To Visit', to_visit_img.astype(np.uint8) * 255)
# key = cv2.waitKey(5)
# --------------------------------
img_float2 = deepcopy(img_float)
cv2.floodFill(
img_float2, mask, seed_point, 128, 80, 80, 8 | (128 << 8)
| cv2.FLOODFILL_MASK_ONLY | cv2.FLOODFILL_FIXED_RANGE)
# Switch coords of seed point
# mask[seed_point[1]-2:seed_point[1]+2, seed_point[0]-2:seed_point[0]+2] = 255
tmpmask = mask[1:h + 1, 1:w + 1]
cv2.namedWindow('tmpmask', cv2.WINDOW_NORMAL)
cv2.imshow('tmpmask', tmpmask)
def onMouse(event, x, y, flags, param):
print("x = " + str(x) + ' y = ' + str(y) + ' value = ' +
str(img_float2[y, x]))
cv2.namedWindow('float', cv2.WINDOW_GUI_EXPANDED)
cv2.setMouseCallback('float', onMouse, param=None)
cv2.imshow('float', img_raw)
key = cv2.waitKey(0)
print('3. took ' + str((rospy.Time.now() - t).to_sec()))
t = rospy.Time.now()
# calculate moments of binary image
M = cv2.moments(tmpmask)
self.labels['detected'] = True
print('4. took ' + str((rospy.Time.now() - t).to_sec()))
t = rospy.Time.now()
if M["m00"] != 0:
# calculate x,y coordinate of center
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
red = deepcopy(img)
# bmask = tmpmask.astype(np.bool)
print(tmpmask.shape)
tmpmask = np.reshape(tmpmask, (480, 640))
print(img.shape)
red[tmpmask != 0] = red[tmpmask != 0] + 10000
img = cv2.merge((img, img, red))
img[cY - 2:cY + 2, cX - 2:cX + 2, 1] = 30000
img[seed_point[1] - 2:seed_point[1] + 2,
seed_point[0] - 2:seed_point[0] + 2, 0] = 30000
# img[100:400, 20:150] = 255
cv2.imshow("mask", img)
cv2.waitKey(5)
# msg_out = self.bridge.cv2_to_imgmsg(showcenter, encoding="passthrough")
# msg_out.header.stamp = self.msg.header.stamp
# msg_out.header.frame_id = self.msg.header.frame_id
# self.publisher_labelled_depth_image.publish(msg_out)
# coords = points[cY * 640 + cX]
# print('coords' + str(coords))
ray = self.cam_model.projectPixelTo3dRay((cX, cY))
print('ray' + str(ray))
print('img' + str(img_float.shape))
print(type(cX))
print(type(cY))
print(type(ray))
dist = float(img_float[cX, cY])
print('dist = ' + str(dist))
x = ray[0] * dist
y = ray[1] * dist
z = ray[2] * dist
print('xyz = ' + str(x) + ' ' + str(y) + ' ' + str(z))
# if not math.isnan(coords[0]):
# self.marker.pose.position.x = coords[0]
# self.marker.pose.position.y = coords[1]
# self.marker.pose.position.z = coords[2]
# self.menu_handler.reApply(self.server)
# self.server.applyChanges()
if dist > 0.1:
# self.marker.pose.position.x = x
# self.marker.pose.position.y = y
# self.marker.pose.position.z = z
# self.menu_handler.reApply(self.server)
# self.server.applyChanges()
pass
print('5. took ' + str((rospy.Time.now() - t).to_sec()))
# idx = np.where(tmpmask == 100)
# # Create tuple with (l, c)
# pointcoords = list(zip(idx[0], idx[1]))
#
# points = pc2.read_points_list(self.msg, skip_nans=False, field_names=("x", "y", "z"))
# tmppoints = []
#
# for coord in pointcoords:
# pointidx = (coord[0]) * 640 + (coord[1])
# tmppoints.append(points[pointidx])
#
# msg_out = createRosCloud(tmppoints, self.msg.header.stamp, self.msg.header.frame_id)
#
# self.publisher_selected_points.publish(msg_out)
print('all. took ' + str((rospy.Time.now() - tall).to_sec()))
elif self.msg_type_str == 'PointCloud2': # 3D scan pointcloud (Andre Aguiar) ---------------------------------
# Get the marker position (this comes from the shpere in rviz)
x_marker, y_marker, z_marker = self.marker.pose.position.x, self.marker.pose.position.y, \
self.marker.pose.position.z # interactive marker pose
# Extract 3D point from the LiDAR
pc = ros_numpy.numpify(self.msg)
points = np.zeros((pc.shape[0], 3))
points[:, 0] = pc['x']
points[:, 1] = pc['y']
points[:, 2] = pc['z']
# Extract the points close to the seed point from the entire PCL
marker_point = np.array([[x_marker, y_marker, z_marker]])
dist = scipy.spatial.distance.cdist(marker_point,
points,
metric='euclidean')
pts = points[np.transpose(dist < self.tracker_threshold)[:, 0], :]
idx = np.where(np.transpose(dist < self.tracker_threshold)[:,
0])[0]
# Tracker - update seed point with the average of cluster to use in the next
# iteration
seed_point = []
if 0 < len(pts):
x_sum, y_sum, z_sum = 0, 0, 0
for i in range(0, len(pts)):
x_sum += pts[i, 0]
y_sum += pts[i, 1]
z_sum += pts[i, 2]
seed_point.append(x_sum / len(pts))
seed_point.append(y_sum / len(pts))
seed_point.append(z_sum / len(pts))
# RANSAC - eliminate the tracker outliers
number_points = pts.shape[0]
if number_points == 0:
return []
# RANSAC iterations
for i in range(0, self.number_iterations):
# Randomly select three points that cannot be coincident
# nor collinear
while True:
idx1 = random.randint(0, number_points - 1)
idx2 = random.randint(0, number_points - 1)
idx3 = random.randint(0, number_points - 1)
pt1, pt2, pt3 = pts[[idx1, idx2, idx3], :]
# Compute the norm of position vectors
ab = np.linalg.norm(pt2 - pt1)
bc = np.linalg.norm(pt3 - pt2)
ac = np.linalg.norm(pt3 - pt1)
# Check if points are colinear
if (ab + bc) == ac:
continue
# Check if are coincident
if idx2 == idx1:
continue
if idx3 == idx1 or idx3 == idx2:
continue
# If all the conditions are satisfied, we can end the loop
break
# ABC Hessian coefficients and given by the external product between two vectors lying on hte plane
A, B, C = np.cross(pt2 - pt1, pt3 - pt1)
# Hessian parameter D is computed using one point that lies on the plane
D = -(A * pt1[0] + B * pt1[1] + C * pt1[2])
# Compute the distance from all points to the plane
# from https://www.geeksforgeeks.org/distance-between-a-point-and-a-plane-in-3-d/
distances = abs(
(A * pts[:, 0] + B * pts[:, 1] + C * pts[:, 2] +
D)) / (math.sqrt(A * A + B * B + C * C))
# Compute number of inliers for this plane hypothesis.
# Inliers are points which have distance to the plane less than a tracker_threshold
num_inliers = (distances < self.ransac_threshold).sum()
# Store this as the best hypothesis if the number of inliers is larger than the previous max
if num_inliers > self.n_inliers:
self.n_inliers = num_inliers
self.A = A
self.B = B
self.C = C
self.D = D
# Extract the inliers
distances = abs((self.A * pts[:, 0] + self.B * pts[:, 1] + self.C * pts[:, 2] + self.D)) / \
(math.sqrt(self.A * self.A + self.B * self.B + self.C * self.C))
inliers = pts[np.where(distances < self.ransac_threshold)]
# Create dictionary [pcl point index, distance to plane] to select the pcl indexes of the inliers
idx_map = dict(zip(idx, distances))
final_idx = []
for key in idx_map:
if idx_map[key] < self.ransac_threshold:
final_idx.append(key)
# -------------------------------------- End of RANSAC ----------------------------------------- #
# publish the points that belong to the cluster
points = []
for i in range(len(inliers)):
r = int(1 * 255.0)
g = int(1 * 255.0)
b = int(1 * 255.0)
a = 150
rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0]
pt = [inliers[i, 0], inliers[i, 1], inliers[i, 2], rgb]
points.append(pt)
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('rgba', 12, PointField.UINT32, 1)
]
header = Header()
header.frame_id = self.parent
header.stamp = self.msg.header.stamp
pc_msg = point_cloud2.create_cloud(header, fields, points)
self.publisher_selected_points.publish(pc_msg)
# Reset the number of inliers to have a fresh start on the next interation
self.n_inliers = 0
# Update the dictionary with the labels (to be saved if the user selects the option)
self.labels['detected'] = True
self.labels['idxs'] = final_idx
# Update the interactive marker pose
self.marker.pose.position.x = seed_point[0]
self.marker.pose.position.y = seed_point[1]
self.marker.pose.position.z = seed_point[2]
self.menu_handler.reApply(self.server)
self.server.applyChanges()
def markerFeedback(self, feedback):
# print(' sensor ' + self.name + ' received feedback')
# pass
# self.optT.setTranslationFromPosePosition(feedback.pose.position)
# self.optT.setQuaternionFromPoseQuaternion(feedback.pose.orientation)
# self.menu_handler.reApply(self.server)
self.server.applyChanges()
def createInteractiveMarker(self):
self.marker = InteractiveMarker()
self.marker.header.frame_id = self.parent
self.marker.pose.position.x = 0
self.marker.pose.position.y = 0
self.marker.pose.position.z = 0
self.marker.pose.orientation.x = 0
self.marker.pose.orientation.y = 0
self.marker.pose.orientation.z = 0
self.marker.pose.orientation.w = 1
self.marker.scale = self.marker_scale
self.marker.name = self.name
self.marker.description = self.name + '_labeler'
# insert a box
control = InteractiveMarkerControl()
control.always_visible = True
marker_box = Marker()
marker_box.type = Marker.SPHERE
marker_box.scale.x = self.marker.scale * 0.3
marker_box.scale.y = self.marker.scale * 0.3
marker_box.scale.z = self.marker.scale * 0.3
marker_box.color.r = 0
marker_box.color.g = 1
marker_box.color.b = 0
marker_box.color.a = 0.2
control.markers.append(marker_box)
self.marker.controls.append(control)
self.marker.controls[
0].interaction_mode = InteractiveMarkerControl.NONE
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
control.orientation_mode = InteractiveMarkerControl.FIXED
self.marker.controls.append(control)
# control = InteractiveMarkerControl()
# control.orientation.w = 1
# control.orientation.x = 0
# control.orientation.y = 1
# control.orientation.z = 0
# control.name = "move_z"
# control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
# control.orientation_mode = InteractiveMarkerControl.FIXED
# self.marker.controls.append(control)
# #
# control = InteractiveMarkerControl()
# control.orientation.w = 1
# control.orientation.x = 0
# control.orientation.y = 0
# control.orientation.z = 1
# control.name = "move_y"
# control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
# control.orientation_mode = InteractiveMarkerControl.FIXED
# self.marker.controls.append(control)
self.server.insert(self.marker, self.markerFeedback)
self.menu_handler.apply(self.server, self.marker.name)
def createInteractiveMarkerRGBD(self, x=0, y=0, z=0):
self.marker = InteractiveMarker()
self.marker.header.frame_id = self.parent
self.marker.pose.position.x = x
self.marker.pose.position.y = y
self.marker.pose.position.z = z
self.marker.pose.orientation.x = 0
self.marker.pose.orientation.y = 0
self.marker.pose.orientation.z = 0
self.marker.pose.orientation.w = 1
self.marker.scale = self.marker_scale
self.marker.name = self.name
self.marker.description = self.name + '_labeler'
# insert a box
control = InteractiveMarkerControl()
control.always_visible = True
marker_box = Marker()
marker_box.type = Marker.SPHERE
marker_box.scale.x = self.marker.scale * 0.3
marker_box.scale.y = self.marker.scale * 0.3
marker_box.scale.z = self.marker.scale * 0.3
marker_box.color.r = 1
marker_box.color.g = 0
marker_box.color.b = 0
marker_box.color.a = 0.2
control.markers.append(marker_box)
self.marker.controls.append(control)
self.marker.controls[
0].interaction_mode = InteractiveMarkerControl.MOVE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
self.marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
self.marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
self.marker.controls.append(control)
self.server.insert(self.marker, self.markerFeedback)
self.menu_handler.apply(self.server, self.marker.name)
def main(args):
global image
rospy.init_node('skeleton_viewer', anonymous=True)
image_info_sub = rospy.Subscriber("/camera/rgb/camera_info", CameraInfo,
image_info_cb)
# Init the Kinect object
kin = Kinect()
ic = image_converter()
cam_model = PinholeCameraModel()
while not camera_info:
time.sleep(1)
print("waiting for camera info")
cam_model.fromCameraInfo(camera_info)
print(cam_model.cx(), cam_model.cy())
# font
font = cv2.FONT_HERSHEY_SIMPLEX
# fontScale
fontScale = 1
# Blue color in BGR
color = (255, 0, 0)
# Line thickness of 2 px
thickness = 2
rate = rospy.Rate(1) # 1hz
while not rospy.is_shutdown():
try:
frames, trans = kin.get_posture()
disp_image = image
i = 0
for frame in frames:
coords = cam_model.project3dToPixel(frame)
disp_image = cv2.circle(disp_image,
(int(coords[0]), int(coords[1])),
radius=10,
color=(0, 0, 255),
thickness=-1)
disp_image = cv2.putText(disp_image, FRAMES[i],
(int(coords[0]), int(coords[1])),
font, fontScale, color, thickness,
cv2.LINE_AA)
i += 1
#coords = cam_model.project3dToPixel(frame)
disp_image = cv2.circle(disp_image,
(int(cam_model.cx()), int(cam_model.cy())),
radius=5,
color=(0, 255, 0),
thickness=-1)
cv2.imshow("Image window", disp_image)
cv2.waitKey(1)
#rate.sleep()
except tf2_ros.TransformException:
print("user not found error")
def handle_img_msg(self, img_msg):
now = rospy.get_rostime()
for m in self.markerArray.markers:
m.action = Marker.DELETE
img = None
bridge = cv_bridge.CvBridge()
try:
img = bridge.imgmsg_to_cv2(img_msg, 'bgr8')
except cv_bridge.CvBridgeError as e:
rospy.logerr('image message to cv conversion failed :')
rospy.logerr(e)
print(e)
return
timestamp = img_msg.header.stamp.to_nsec()
self.frame_index = self.timestamp_map[timestamp]
if self.offset:
self.frame_index -= self.offset
out_img = np.copy(img)
for i, f in enumerate(self.frame_map[self.frame_index]):
#f = self.frame_map[self.frame_index][0]
#color = (255,255,0)
color = kelly_colors_list[i % len(kelly_colors_list)]
marker = self.create_marker()
marker.color.r = color[0] / 255
marker.color.g = color[1] / 255
marker.color.b = color[2] / 255
marker.color.a = 0.5
marker.header.stamp = now
marker.pose.position = Point(*f.trans)
marker.pose.orientation = Quaternion(*f.rotq)
marker.id = i
self.markerArray.markers.append(marker)
#self.markOutput.publish(marker)
md = self.metadata
dims = np.array([md['l'], md['w'], md['h']])
obs_centroid = np.array(f.trans)
orient = list(f.rotq)
#orient[2] -= 0.035
#R = tf.transformations.quaternion_matrix((0,0,-0.0065,1))
#obs_centroid = R.dot(list(obs_centroid)+[1])[:3]
if obs_centroid is None:
rospy.loginfo("Couldn't find obstacle centroid")
continue
#self.imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
#return
# print centroid info
rospy.loginfo(str(obs_centroid))
# case when obstacle is not in camera frame
if obs_centroid[0] < 3:
continue
#self.imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
#return
# get bbox
R = tf.transformations.quaternion_matrix(orient)
#R = tf.transformations.quaternion_matrix([0,0,0,1])
corners = [
0.5 * np.array([i, j, k]) * dims for i in [-1, 1]
for j in [-1, 1] for k in [-1, 1]
]
corners = np.array(
[obs_centroid + R.dot(list(c) + [1])[:3] for c in corners])
#if self.ground_corr is not None:
# z_min, x_min, y_min = self.ground_corr.loc[timestamp]
# z_offset = z_min - min(corners[:,2])
# x_offset = x_min - min(corners[:,0])
# y_offset = y_min - min(corners[:,1])
# corr = np.array([0, 0, z_offset])
# #corr = np.array([0, 0,0])
# #corr = np.array([x_offset, y_offset, z_offset])
# corr[np.isnan(corr)]=0
# corners+=corr
#print(corners)
cameraModel = PinholeCameraModel()
cam_info = load_cam_info(self.calib_file)
cameraModel.fromCameraInfo(cam_info)
projected_pts = [
cameraModel.project3dToPixel(list(pt) + [1]) for pt in corners
]
projected_pts = np.array(projected_pts)
center = np.mean(projected_pts, axis=0)
out_img = drawBbox(out_img, projected_pts, color=color[::-1])
self.markOutput.publish(self.markerArray)
self.imgOutput.publish(bridge.cv2_to_imgmsg(out_img, 'bgr8'))
class LidarImage:
def __init__(self):
self._imageInput = rospy.Subscriber('/sensors/camera/image_color',
Image,
callback=self.imageCallback,
queue_size=1)
self._camera = rospy.Subscriber('/sensors/camera/camera_info',
CameraInfo,
callback=self.cameraCallback,
queue_size=1)
self._velodyne = rospy.Subscriber('/sensors/velodyne_points',
PointCloud2,
callback=self.velodyneCallback,
queue_size=20)
self._imageOutput = rospy.Publisher('image_overlay',
Image,
queue_size=10)
self._bridge = cv_bridge.CvBridge()
self._cameraModel = PinholeCameraModel()
self._tf = tf.TransformListener()
self.point_list = []
self.image_coordinates = Vector3()
def imageCallback(self, data):
print('Received an image!')
cvImage = {}
try:
cvImage = self._bridge.imgmsg_to_cv2(data, 'bgr8')
except cv_bridge.CvBridgeError as e:
rospy.logerr('[lidar_image] Failed to convert image')
rospy.logerr('[lidar_image] ' + e)
print(e)
return
(translation,
rotation) = self._tf.lookupTransform('/world', '/velodyne',
rospy.Time(0))
translation = tuple(translation) + (1, )
Rq = tf.transformations.quaternion_matrix(rotation)
Rq[:, 3] = translation
if self._velodyneData:
for i in range(0, len(self._velodyneData) - 1):
try:
point = self._velodyneData[i][:3] + (1, )
if math.sqrt(np.sum(np.array(point[:3])**2)) > 2.5:
continue
except IndexError:
break
self.point_list.append([
self._velodyneData[i][0], self._velodyneData[i][1],
self._velodyneData[i][2]
])
rotatedPoint = Rq.dot(point)
uv = self._cameraModel.project3dToPixel(rotatedPoint)
if uv[0] >= 0 and uv[0] <= 1920 and uv[1] >= 0 and uv[
1] <= 1200:
cv2.circle(cvImage, (int(uv[0]), int(uv[1])), 2,
(255, 0, 0))
try:
self._imageOutput.publish(
self._bridge.cv2_to_imgmsg(cvImage, 'bgr8'))
except cv_bridge.CvBridgeError as e:
rospy.logerr('[lidar_image] Failed to convert image to message')
rospy.logerr('[lidar_image] ' + e)
print(e)
return
def cameraCallback(self, data):
print('Received camera info')
self._cameraModel.fromCameraInfo(data)
def velodyneCallback(self, data):
print('Received velodyne point cloud')
formatString = 'ffff'
if data.is_bigendian:
formatString = '>' + formatString
else:
formatString = '<' + formatString
points = []
for index in range(0, len(data.data), 32):
points.append(
struct.unpack(formatString, data.data[index:index + 16]))
print(len(points))
self._velodyneData = points
class GroundProjection():
def __init__(self, robot_name="neo"):
# defaults overwritten by param
self.robot_name = robot_name
self.rectified_input = False
# Load homography
self.H = self.load_homography()
self.Hinv = np.linalg.inv(self.H)
self.pcm_ = PinholeCameraModel()
# Load checkerboard information
self.board_ = self.load_board_info()
# wait until we have recieved the camera info message through ROS and then initialize
def initialize_pinhole_camera_model(self, camera_info):
self.ci_ = camera_info
self.pcm_.fromCameraInfo(camera_info)
print("pinhole camera model initialized")
def vector2pixel(self, vec):
pixel = Pixel()
cw = self.ci_.width
ch = self.ci_.height
pixel.u = cw * vec.x
pixel.v = ch * vec.y
if (pixel.u < 0): pixel.u = 0
if (pixel.u > cw - 1): pixel.u = cw - 1
if (pixel.v < 0): pixel.v = 0
if (pixel.v > ch - 1): pixel.v = 0
return pixel
def pixel2vector(self, pixel):
vec = Vector2D()
vec.x = pixel.u / self.ci_.width
vec.y = pixel.v / self.ci_.height
return vec
def vector2ground(self, vec):
pixel = self.vector2pixel(vec)
return self.pixel2ground(pixel)
def ground2vector(self, point):
pixel = self.ground2pixel(point)
return self.pixel2vector(pixel)
def pixel2ground(self, pixel):
uv_raw = np.array([pixel.u, pixel.v])
if not self.rectified_input:
uv_raw = self.pcm_.rectifyPoint(uv_raw)
#uv_raw = [uv_raw, 1]
uv_raw = np.append(uv_raw, np.array([1]))
ground_point = np.dot(self.H, uv_raw)
point = Point()
x = ground_point[0]
y = ground_point[1]
z = ground_point[2]
point.x = x / z
point.y = y / z
point.z = 0.0
return point
def ground2pixel(self, point):
ground_point = np.array([point.x, point.y, 1.0])
image_point = np.dot(self.Hinv, ground_point)
image_point = image_point / image_point[2]
pixel = Pixel()
if not self.rectified_input:
distorted_pixel = self.pcm_.project3dToPixel(image_point)
pixel.u = distorted_pixel[0]
pixel.v = distorted_pixel[1]
else:
pixel.u = image_point[0]
pixel.v = image_point[1]
def rectify(self, cv_image_raw):
'''Undistort image'''
cv_image_rectified = np.zeros(np.shape(cv_image_raw))
mapx = np.ndarray(shape=(self.pcm_.height, self.pcm_.width, 1),
dtype='float32')
mapy = np.ndarray(shape=(self.pcm_.height, self.pcm_.width, 1),
dtype='float32')
mapx, mapy = cv2.initUndistortRectifyMap(
self.pcm_.K, self.pcm_.D, self.pcm_.R, self.pcm_.P,
(self.pcm_.width, self.pcm_.height), cv2.CV_32FC1, mapx, mapy)
return cv2.remap(cv_image_raw, mapx, mapy, cv2.INTER_CUBIC,
cv_image_rectified)
def estimate_homography(self, cv_image):
'''Estimate ground projection using instrinsic camera calibration parameters'''
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
cv_image_rectified = self.rectify(cv_image)
logger.info("image rectified")
ret, corners = cv2.findChessboardCorners(
cv_image_rectified, (self.board_['width'], self.board_['height']))
if ret == False:
logger.error("No corners found in image")
exit(1)
if len(corners) != self.board_['width'] * self.board_['height']:
logger.error("Not all corners found in image")
exit(2)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30,
0.01)
corners2 = cv2.cornerSubPix(cv_image_rectified, corners, (11, 11),
(-1, -1), criteria)
#TODO flip checks
src_pts = []
for r in range(self.board_['height']):
for c in range(self.board_['width']):
src_pts.append(
np.array([
r * self.board_['square_size'], c *
self.board_['square_size']
],
dtype='float32') + self.board_['offset'])
# OpenCV labels corners left-to-right, top-to-bottom
# We're having a problem with our pattern since it's not rotation-invariant
# only reverse order if first point is at bottom right corner
if ((corners[0])[0][0] <
(corners[self.board_['width'] * self.board_['height'] - 1])[0][0]
and (corners[0])[0][0] <
(corners[self.board_['width'] * self.board_['height'] - 1])[0][1]):
rospy.loginfo("Reversing order of points.")
src_pts.reverse()
# Compute homography from image to ground
self.H, mask = cv2.findHomography(corners2.reshape(len(corners2), 2),
np.array(src_pts), cv2.RANSAC)
extrinsics_filename = sys.path[
0] + "/calibrations/camera_extrinsic/" + self.robot_name + ".yaml"
self.write_homography(extrinsics_filename)
logger.info(
"Wrote ground projection to {}".format(extrinsics_filename))
# Check if specific point in matrix is larger than zero (this would definitly mean we're having a corrupted rotation matrix)
if (self.H[1][2] > 0):
rospy.logerr("WARNING: Homography could be corrupt!")
def load_homography(self):
'''Load homography (extrinsic parameters)'''
filename = (sys.path[0] + "/calibrations/camera_extrinsic/" +
self.robot_name + ".yaml")
if not os.path.isfile(filename):
logger.warn(
"no extrinsic calibration parameters for {}, trying default".
format(self.robot_name))
filename = (sys.path[0] +
"/calibrations/camera_extrinsic/default.yaml")
if not os.path.isfile(filename):
logger.error("can't find default either, something's wrong")
else:
data = yaml_load_file(filename)
else:
rospy.loginfo("Using extrinsic calibration of " + self.robot_name)
data = yaml_load_file(filename)
logger.info("Loaded homography for {}".format(
os.path.basename(filename)))
return np.array(data['homography']).reshape((3, 3))
def write_homography(self, filename):
ob = {'homography': sum(self.H.reshape(9, 1).tolist(), [])}
print ob
yaml_write_to_file(ob, filename)
def load_board_info(self, filename=''):
'''Load calibration checkerboard info'''
if not os.path.isfile(filename):
filename = get_ros_package_path(
'duckietown'
) + '/config/baseline/ground_projection/ground_projection/default.yaml'
target_data = yaml_load_file(filename)
target_info = {
'width': target_data['board_w'],
'height': target_data['board_h'],
'square_size': target_data['square_size'],
'x_offset': target_data['x_offset'],
'y_offset': target_data['y_offset'],
'offset':
np.array([target_data['x_offset'], -target_data['y_offset']]),
'size': (target_data['board_w'], target_data['board_h']),
}
logger.info("Loaded checkerboard parameters")
return target_info
#######################################################
# OLD STUFF #
#######################################################
def load_camera_info(self):
'''Load camera intrinsics'''
filename = (sys.path[0] + "/calibrations/camera_intrinsic/" +
self.robot_name + ".yaml")
if not os.path.isfile(filename):
logger.warn(
"no intrinsic calibration parameters for {}, trying default".
format(self.robot_name))
filename = (sys.path[0] +
"/calibrations/camera_intrinsic/default.yaml")
if not os.path.isfile(filename):
logger.error("can't find default either, something's wrong")
calib_data = yaml_load_file(filename)
# logger.info(yaml_dump(calib_data))
cam_info = CameraInfo()
cam_info.width = calib_data['image_width']
cam_info.height = calib_data['image_height']
cam_info.K = np.array(calib_data['camera_matrix']['data']).reshape(
(3, 3))
cam_info.D = np.array(
calib_data['distortion_coefficients']['data']).reshape((1, 5))
cam_info.R = np.array(
calib_data['rectification_matrix']['data']).reshape((3, 3))
cam_info.P = np.array(calib_data['projection_matrix']['data']).reshape(
(3, 4))
cam_info.distortion_model = calib_data['distortion_model']
logger.info(
"Loaded camera calibration parameters for {} from {}".format(
self.robot_name, os.path.basename(filename)))
return cam_info
def _load_homography(self, filename):
data = yaml_load_file(filename)
return np.array(data['homography']).reshape((3, 3))
def _load_camera_info(self, filename):
calib_data = yaml_load_file(filename)
# logger.info(yaml_dump(calib_data))
cam_info = CameraInfo()
cam_info.width = calib_data['image_width']
cam_info.height = calib_data['image_height']
cam_info.K = np.array(calib_data['camera_matrix']['data']).reshape(
(3, 3))
cam_info.D = np.array(
calib_data['distortion_coefficients']['data']).reshape((1, 5))
cam_info.R = np.array(
calib_data['rectification_matrix']['data']).reshape((3, 3))
cam_info.P = np.array(calib_data['projection_matrix']['data']).reshape(
(3, 4))
cam_info.distortion_model = calib_data['distortion_model']
return cam_info
def _rectify(self, cv_image_raw):
'''old'''
#cv_image_rectified = cvMat()
cv_image_rectified = np.zeros(np.shape(cv_image_raw))
# TODO: debug PinholeCameraModel()
self.pcm_.rectifyImage(cv_image_raw, cv_image_rectified)
return cv_image_rectified
path_root + 'matt-00.pickle',
path_root + 'jasper-00.pickle']
for path, color, name in zip(paths, 'kbg', ['Cameron', 'Matthew', 'Jasper']):
with open(path, 'r') as f:
data = pickle.load(f)
depths = [d[0] for d in data]
sizes_pixel = [d[1][-1] for d in data]
top_lefts = [model.projectPixelTo3dRay((u,v))
for (d, (u,v,w,h)) in [(d[0], d[1]) for d in data]]
top_rights = [model.projectPixelTo3dRay((u+w,v))
for (d, (u,v,w,h)) in [(d[0], d[1]) for d in data]]
sizes_meter = [tr[0] / tr[2] * d - tl[0] / tl[2] * d
for tl, tr, d in zip(top_lefts, top_rights, depths)]
size_avg = sum(sizes_meter) / len(sizes_meter)
print size_avg
depths_true = [i*0.01 for i in range(50,501)]
sizes_pixel_true = [model.project3dToPixel((size_avg, 0, d))[0]-
model.project3dToPixel((0, 0, d))[0]
for d in depths_true]
pyplot.plot(sizes_pixel, depths, color+'o', label=name+' Actual')
pyplot.plot(sizes_pixel_true, depths_true, color, label=name+' Calculated, Size = '+str(int(size_avg*1000))+'mm')
pyplot.title('Face Sizes at different Depths -- Average NAW Male Face Size = 187mm')
pyplot.xlabel('Face Size (pixels)')
pyplot.ylabel('Depth (meters)')
pyplot.legend(loc='right')
pyplot.show()
def handle_radar_msg(self, radar_msg):
md = self.metadata
now = radar_msg.header.stamp
poseArray = PoseArray()
for track in radar_msg.tracks:
r = track.range
# ignore obstacles farther than 55 m
if (track.status != 3) or (r > 45) or (r < 6):
continue
pose = Pose()
theta = track.angle / 180. * pi
pose.position.x = (r + md['l'] / 2.) * math.cos(theta) + 1.5
pose.position.y = -(r + md['l'] / 2.) * math.sin(theta)
pose.position.z = -0.8
poseArray.poses.append(pose)
self.detected_poses = poseArray
self.radarPub.publish(poseArray)
img = None
bridge = cv_bridge.CvBridge()
if self.raw_image is None:
return
try:
img = bridge.imgmsg_to_cv2(self.raw_image, 'bgr8')
except cv_bridge.CvBridgeError as e:
rospy.logerr('image message to cv conversion failed :')
rospy.logerr(e)
print(e)
return
out_img = np.copy(img)
if self.detected_poses is None:
return
for i, pose in enumerate(self.detected_poses.poses):
color = kelly_colors_list[i % len(kelly_colors_list)]
marker = self.create_marker()
marker.color.r = color[0] / 255
marker.color.g = color[1] / 255
marker.color.b = color[2] / 255
marker.color.a = 0.5
marker.header.stamp = now
marker.pose = pose
marker.id = i
self.markerArray.markers.append(marker)
dims = np.array([md['l'], md['l'], md['h']])
obs_centroid = np.array(
[pose.position.x, pose.position.y, pose.position.z])
orient = list([
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
])
# case when obstacle is not in camera frame
if obs_centroid[0] < 4:
continue
# get bbox
R = tf.transformations.quaternion_matrix(orient)
corners = [
0.5 * np.array([i, j, k]) * dims for i in [-1, 1]
for j in [-1, 1] for k in [-1, 1]
]
corners = np.array(
[obs_centroid + R.dot(list(c) + [1])[:3] for c in corners])
cameraModel = PinholeCameraModel()
cam_info = load_cam_info(self.calib_file)
cameraModel.fromCameraInfo(cam_info)
projected_pts = [
cameraModel.project3dToPixel(list(pt) + [1]) for pt in corners
]
projected_pts = np.array(projected_pts)
center = np.mean(projected_pts, axis=0)
out_img = drawBbox(out_img, projected_pts, color=color[::-1])
self.markOutput.publish(self.markerArray)
self.imgOutput.publish(bridge.cv2_to_imgmsg(out_img, 'bgr8'))
class Imbox2odom:
def __init__(self):
# self.image_pub = rospy.Publisher("/carina/sensor/camera/left/tsign_image_detection",Image, queue_size=1)
self.odom_objs_tfsign_pub = rospy.Publisher("/carina/sensor/tfsigns_odom", ObstacleArray, queue_size = 1)
self.bbox_sub = rospy.Subscriber("/carina/sensor/tfsigns_box",BoundingBoxArray,self.boundingbox_callback, queue_size = 1)
self.info_sub_l = rospy.Subscriber("/carina/sensor/camera/left/camera_info", CameraInfo, self.callback_info_left, queue_size=1)
self.objs_sub = rospy.Subscriber("/carina/perception/lidar/obstacles_array", ObstacleArray, self.obstacles_callback, queue_size=1)
self.camInfo = CameraInfo()
self.tf_l = tf.TransformListener()
self.obstacles_loc = ObstacleArray()
self.model = PinholeCameraModel()
self.odom_objs_tfsign = ObstacleArray()
self.odom_objs_tfsign.obstacle = []
self.n_tfsigns = 0
self.rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
# tfs = "Publish traffic signs %s" % rospy.get_time()
# rospy.loginfo(tfs)
self.odom_objs_tfsign_pub.publish(self.odom_objs_tfsign)
self.rate.sleep()
def obstacles_callback(self,a_obstacles):
self.obstacles_loc=a_obstacles
def callback_info_left(self,camerainfo):
self.model.fromCameraInfo(camerainfo)
def boundingbox_callback(self,bbox_loc):
for obstacle in self.obstacles_loc.obstacle:
if( obstacle.pose.position.x>0.0 and obstacle.pose.position.x < 30 and obstacle.scale.x<0.80 and obstacle.scale.y<0.80 and obstacle.scale.x*obstacle.scale.y<0.50):
try:
# (trans,rot) = self.tf_l.lookupTransform('stereo', 'velodyne', rospy.Time(0))
(trans,rot) = self.tf_l.lookupTransform(bbox_loc.header.frame_id, obstacle.header.frame_id, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print('tf error')
pc_point=PointStamped()
pc_point.header.frame_id = obstacle.header.frame_id#"velodyne"
# pc_point.header.frame_id = "velodyne"
pc_point.header.stamp =rospy.Time(0)
pc_point.point=obstacle.pose.position
# pt=self.tf_l.transformPoint("stereo",pc_point)
pt=self.tf_l.transformPoint(bbox_loc.header.frame_id,pc_point)
p_2d = self.model.project3dToPixel((pt.point.x,pt.point.y,pt.point.z))
for bbox in bbox_loc.objects:
if( p_2d[0]>bbox.p1.x-30 and p_2d[0]<bbox.p3.x+30 ):
try:
# (trans_odom,rot_odom) = self.tf_l.lookupTransform('odom', 'velodyne', rospy.Time(0))
(trans_odom,rot_odom) = self.tf_l.lookupTransform('odom', obstacle.header.frame_id, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print('tf error')
pt_odom = self.tf_l.transformPoint("odom",pc_point)
obj_odom = Obstacle()
obj_odom = obstacle
obj_odom.header.frame_id = "odom"
obj_odom.header.stamp = rospy.Time.now()
obj_odom.ns = "odom_namespace";
obj_odom.pose.position = pt_odom.point
obj_odom.scale.x = 0.5
obj_odom.scale.y = 0.5
obj_odom.scale.z = 0.5
classe = {
"30":(1,0.5,0),
"60":(1,0,1),
"90":(0,1,1),
"r":(1,0,0),
"g":(0,1,0),
"y":(1,1,0)
}
color = classe[bbox.classe.data]
print(color[0])
# print(type(obj_odom.color))
# obj_odom.color= color
obj_odom.color.r = color[0]
obj_odom.color.g = color[1]
obj_odom.color.b = color[2]
obj_odom.color.a = 1
obj_odom.lifetime= rospy.Duration(3)
self.n_tfsigns+=1
print("n traffic signs: ", self.n_tfsigns)
obj_odom.id = self.n_tfsigns
self.odom_objs_tfsign.obstacle.append(obj_odom)
class ParseOpenFace:
def __init__(self):
print "in init"
self.image_geo = None
rospy.Subscriber("faces", Faces, self.openface_callback)
rospy.Subscriber("beliefs/features", PcFeatureArray,
self.hlpr_callback)
self.camera_info_sub = rospy.Subscriber("kinect/qhd/camera_info",
CameraInfo,
self.camera_info_callback)
self.yolo_sub = rospy.Subscriber("detector_node/detections",
Detections, self.yolo_callback)
self.line_pub = rospy.Publisher("gaze_gui_line", Lines)
self.obj_bridge_pub = rospy.Publisher("object_detector_bridge",
GazeTopic)
self.camera_init_done = False
self.gaze_x, self.gaze_y, self.gaze_z = None, None, None
self.hlpr_objects, self.yolo_objects = None, None
self.mutual = False
print "finished init"
def camera_info_callback(self, msg):
self.image_geo = PinholeCameraModel()
self.image_geo.fromCameraInfo(msg)
self.camera_init_done = True
self.camera_info_sub.unregister()
def hlpr_callback(self, msg):
self.hlpr_objects = msg.objects
def yolo_callback(self, msg):
self.yolo_objects = msg.objects
def openface_callback(self, msg):
if len(msg.faces) > 0:
left, right = msg.faces[0].left_gaze, msg.faces[0].right_gaze
sum_gaze = [(left.x + right.x), (left.y + right.y),
(left.z + right.z)]
norm_factor = math.sqrt(sum(map(lambda x: x * x, sum_gaze)))
self.avg_gaze = map(lambda x: x / norm_factor, sum_gaze)
self.avg_gaze = map(lambda x: x * 1000,
self.avg_gaze) #####SCALING FACTOR
# print self.avg_gaze
if (self.avg_gaze[0] > 0.065):
self.mutual = True
else:
self.mutual = False
# print self.mutual
head_msg = msg.faces[0].head_pose.position
self.head_pose = [head_msg.x, head_msg.y, head_msg.z]
self.head_gaze_pose = np.add(self.head_pose,
self.avg_gaze).tolist()
# print self.head_pose
self.find_nearest_object()
self.publish_nearest_obj()
def find_nearest_object(self):
if not (self.camera_init_done or self.hlpr_objects
or self.yolo_objects):
return
# scaling_factor = 1500
# self.avg_gaze = map(lambda x: x*scaling_factor, self.avg_gaze)
def publish_nearest_obj(self):
if not (self.hlpr_objects or self.yolo_objects
or len(self.hlpr_objects) > 0):
return
# min_center = self.find_closest_hlpr_cluster().bb_center
# print min_center # DEBUG
# obj_x, obj_y = self.image_geo.project3dToPixel((min_center.x, min_center.y, min_center.z))
yolo_object = self.find_closest_yolo_obj()
gaze_x, gaze_y = self.image_geo.project3dToPixel(self.head_gaze_pose)
head_x, head_y = self.image_geo.project3dToPixel(self.head_pose)
prediction = [gaze_x, gaze_y, head_x, head_y]
self.publish_object_bridge(yolo_object, prediction)
def publish_object_bridge(self, yolo_obj, prediction):
mutual_bool = Bool(data=self.mutual)
nearest_object_msg = String(data=yolo_obj.label)
coordinate_array = Float32MultiArray(data=prediction)
msg_topic = GazeTopic(nearest_object=nearest_object_msg,
mutual=mutual_bool,
coordinates=coordinate_array)
self.obj_bridge_pub.publish(msg_topic)
# print yolo_obj.label # DEBUG
# def find_closest_hlpr_cluster(self):
# min_diff = float('inf')
# min_item = None
# for item in self.hlpr_objects: # Find closest hlpr object to gaze vector
# center = np.array([item.bb_center.x,item.bb_center.y,item.bb_center.z])
# l1 = np.array(self.head_pose)
# l2 = center
# p = np.array(self.avg_gaze)
# # diff = np.linalg.norm(np.cross(l2-l1, p))/np.linalg.norm(p)
# diff = np.linalg.norm(np.cross(l2-l1, p-l1))/np.linalg.norm(l2-l1)
# # diff = (self.avg_gaze[0] - center[0])**2 + (self.avg_gaze[1] - center[1])**2 + (self.avg_gaze[2] - center[2])**2
# if diff<min_diff:
# min_diff = diff
# min_item = item
# min_l1, min_l2, min_p = l1, l2, p
# head_x, head_y = self.image_geo.project3dToPixel(self.head_pose)
# print "min l2 {0}".format(min_l2)
# print "min l1 {0}".format(min_l1)
# print "min diff {0}".format(min_l2-min_l1)
# l1_x, l1_y = self.image_geo.project3dToPixel(min_l2-min_l1)
# line1 = Float32MultiArray(data=[head_x,head_y,l1_x+head_x, l1_y+head_y])
# l2_x, l2_y = self.image_geo.project3dToPixel(min_l1 - 1000*min_p)
# line2 = Float32MultiArray(data=[head_x,head_y,l2_x, l2_y])
# self.line_pub.publish([line1, line2])
# return min_item
def find_closest_yolo_obj(self):
min_diff = float('inf')
yolo_obj = None
head_x, head_y = self.image_geo.project3dToPixel(self.head_pose)
head_gaze_x, head_gaze_y = self.image_geo.project3dToPixel(
self.head_gaze_pose)
for yolo_object in self.yolo_objects:
if yolo_object.label != "spoon":
head = np.array([head_x, head_y])
head_gaze = np.array([head_gaze_x, head_gaze_y])
obj = np.array(
[yolo_object.centroid_x, yolo_object.centroid_y])
# diff = np.linalg.norm(np.cross(l2-l1, p))/np.linalg.norm(p)
# diff = np.linalg.norm(np.cross(l2-l1, l1-p))/np.linalg.norm(l2-l1)
diff = np.linalg.norm(
np.cross(head_gaze - head,
head - obj)) / np.linalg.norm(head_gaze - head)
# diff = (self.avg_gaze[0] - center[0])**2 + (self.avg_gaze[1] - center[1])**2 + (self.avg_gaze[2] - center[2])**2
if diff < min_diff:
min_diff = diff
yolo_obj = yolo_object
# min_l1, min_l2, min_p = l1, l2, p
# print "min l2 {0}".format(min_l2)
# print "min l1 {0}".format(min_l1)
# print "min diff {0}".format(min_l2-min_l1)
line1 = Float32MultiArray(data=[0, 0, 0, 0])
line2 = Float32MultiArray(data=[
head_x, head_y, head_x + 100 * (head_gaze_x - head_x), head_y +
100 * (head_gaze_y - head_y)
])
self.line_pub.publish([line1, line2])
return yolo_obj
class FaceDetector():
""" Measures face location, compiles important info. """
def __init__(self, stillness_radius, mode_topic, torso_topic, info_topic, image_topic):
self.stillness_radius = stillness_radius
# Subscribe to mode info and torso location
self.need_mode = True
rospy.Subscriber(mode_topic, Bool, self.mode_callback)
self.need_torso = True
rospy.Subscriber(torso_topic, PointStamped, self.torso_callback, queue_size=1)
# Camera model stuff
self.model = PinholeCameraModel()
self.need_camera_info = True
rospy.Subscriber(info_topic, CameraInfo, self.info_callback, queue_size=1)
# Ready the face detector
self.classifier = cv2.CascadeClassifier(os.environ['ROS_ROOT'] + '/../OpenCV/haarcascades/haarcascade_frontalface_alt.xml')
# Wait for all those messages to begin arriving
while self.need_mode or self.need_torso or self.need_camera_info:
rospy.sleep(0.01)
# Subscribe to images
self.have_image = False
self.bridge = CvBridge()
rospy.Subscriber(image_topic, Image, self.handle_image, queue_size=1)
def mode_callback(self, mode):
self.is_standing_still = mode.data
if self.need_mode:
rospy.loginfo('Got first mode message!')
self.need_mode = False
def torso_callback(self, torso_location):
self.torso_location = torso_location
if self.need_torso:
rospy.loginfo('Got first torso message!')
self.need_torso = False
def info_callback(self, info):
if self.need_camera_info:
rospy.loginfo('Got camera info message!')
self.info = info
self.model.fromCameraInfo(info)
self.need_camera_info = False
def handle_image(self, imgmsg):
image = self.bridge.imgmsg_to_cv2(imgmsg)
if not self.have_image:
self.have_image = True
success = self.filter_face(image)
def filter_face(self, image):
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
faces = self.classifier.detectMultiScale(gray, 2, 1)
# Mark torso location
torso = [self.torso_location.point.x,
self.torso_location.point.y,
self.torso_location.point.z]
#torso = [el * -1 for el in torso] # mirror about the origin
torso_uv = self.model.project3dToPixel(torso)
torso_uv = tuple(int(el) for el in torso_uv)
cv2.circle(image, torso_uv, 5, (0,255,0), -1)
if len(faces) > 0: # filter the face
for face in faces:
(x,y,w,h) = face
cv2.rectangle(image, (x,y), (x+w,y+h), (0,255,0), 3) # plot detected faces
else: # filter according to the contingency plan
x,y,w,h = self.expand_filter(torso)
cv2.rectangle(image, (x,y), (x+w,y+h), (0,255,0), 5) # plot expanded filter area
# Use text to indicate mode -- standing or walking
if self.is_standing_still:
text = 'Standing'
else:
text = 'Walking'
cv2.putText(image, text, (50, 480-50), cv2.FONT_HERSHEY_COMPLEX, 2, (0, 0, 255), 2)
cv2.imshow('Filtered Image', image)
cv2.waitKey(1)
def expand_filter(self, torso):
""" If the person is walking, filter the whole screen.
If the person is standing, filter within a certain distance of their torso. """
# Max image dimensions
W = self.info.width
H = self.info.height
if self.is_standing_still:
# Do torso +/- radius in x-direction
left_xyz = list(torso) # copy
left_xyz[0] -= self.stillness_radius
right_xyz = list(torso) # copy
right_xyz[0] += self.stillness_radius
# Project to image plane
left_uv = self.model.project3dToPixel(left_xyz)
right_uv = self.model.project3dToPixel(right_xyz)
# Filter floor to ceiling, -y to +y
x = max(left_uv[0], 0)
y = 0
w = right_uv[0] - left_uv[0]
w = min(w, W - x)
h = H
else:
x, y, w, h = [0, 0, W, H] # filter everything
return [int(el) for el in [x,y,w,h]]
