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))
class ConverterToUV():
def __init__(self, info_topic):
self.camera_model = PinholeCameraModel()
rospy.Subscriber(info_topic, CameraInfo, self.update_model)
def update_model(self, info):
self.camera_model.fromCameraInfo(info)
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 FindDist(color):
cont = checkIfColorIsShowed(color)
if cont == []:
rospy.sleep(0.5)
print "None"
return None
cnt = cont[0]
M = cv2.moments(cnt)
center_x = int(M['m10'] / M['m00'])
center_y = 400
imgCntr = (400, 400)
camInfo = rospy.wait_for_message("camera/camera_info", CameraInfo)
camera = PinholeCameraModel()
camera.fromCameraInfo(camInfo)
cenRay = camera.projectPixelTo3dRay(imgCntr)
pixRay = camera.projectPixelTo3dRay((center_x, center_y))
angle = math.acos(
prod(pixRay, cenRay) / (math.sqrt(prod(pixRay, pixRay))) *
(math.sqrt(prod(cenRay, cenRay))))
dg = int(math.degrees(angle))
if center_x >= imgCntr[0]:
dg = 360 - dg
LaserMessage = rospy.wait_for_message("scan", LaserScan)
return retrunResult(LaserMessage, dg)
def random_point_inside_fov(camera_info,
maxdist,
mindist=0,
Tcamera=np.eye(4)):
"""
Generates a random XYZ point inside the camera field of view
@type camera_info: sensor_msgs.CameraInfo
@param camera_info: Message with the meta information for a camera
@type maxdist: float
@param maxdist: distance from the camera ref frame in the z direction
@type mindist: float
@param mindist: distance from the camera ref frame in the z direction
@type Tcamera: np.array
@param Tcamera: homogeneous transformation for the camera ref frame
@rtype: array
@return: The random XYZ point
"""
cam_model = PinholeCameraModel()
cam_model.fromCameraInfo(camera_info)
z = np.random.uniform(mindist, maxdist)
delta_x = cam_model.getDeltaX(cam_model.width / 2, z)
delta_y = cam_model.getDeltaY(cam_model.height / 2, z)
point = np.array([0, 0, z, 1])
point[:2] = np.array([delta_x, delta_y
]) * (2 * np.random.random_sample(2) - 1.)
return np.dot(Tcamera, point)[:3]
def from_camera_info(CameraConfig, message):
''' Load camera intrinsics from ROS camera info topic, initialize
camera extrinsics with None value. '''
model = PinholeCameraModel()
model.fromCameraInfo(message)
return CameraConfig(model.K, model.D, None,
(model.width, model.height), model.P, model.R)
class Helpers:
def __init__(self, camera_info):
self.ci = camera_info
self.pcm = PinholeCameraModel()
self.pcm.fromCameraInfo(self.ci)
self._rectify_inited = False
self._distort_inited = False
def process_image(self, cv_image_raw, interpolation=cv2.INTER_NEAREST):
''' Undistort an image.
To be more precise, pass interpolation= cv2.INTER_CUBIC
'''
if not self._rectify_inited:
self._init_rectify_maps()
cv_image_rectified = np.empty_like(cv_image_raw)
res = cv2.remap(cv_image_raw, self.mapx, self.mapy, interpolation,
cv_image_rectified)
return res
def _init_rectify_maps(self):
W = self.pcm.width
H = self.pcm.height
mapx = np.ndarray(shape=(H, W, 1), dtype='float32')
mapy = np.ndarray(shape=(H, W, 1), dtype='float32')
mapx, mapy = cv2.initUndistortRectifyMap(self.pcm.K, self.pcm.D,
self.pcm.R, self.pcm.P,
(W, H), cv2.CV_32FC1, mapx,
mapy)
self.mapx = mapx
self.mapy = mapy
self._rectify_inited = True
def dpx_to_distance(dx, dy, camera_name, current_ps, offset):
print("The dx is " + str(dx) + " the dy is " + str(dy) + " and the camera name is " + camera_name)
big_z_mappings = {'camera1': transform_broadcaster_mapping['camera1'][0][2] - current_ps.pose.position.z, # x-y plane
'camera2': transform_broadcaster_mapping['camera2'][0][1] - current_ps.pose.position.y, # x-z plane
'camera3': transform_broadcaster_mapping['camera3'][0][0] - current_ps.pose.position.x} # y-z plane
#print("The frame_id for the current pose is " + current_ps.header.frame_id)
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(camera_info_mapping[camera_name])
x, y, z = camera_model.projectPixelTo3dRay((dx, dy))
#print " x : {} , y : {} , z : {}".format(x, y, z)
x_center, y_center, z_center = camera_model.projectPixelTo3dRay((0, 0))
big_z = abs(big_z_mappings[camera_name])
#print("The big_z for " + camera_name + " is " + str(big_z))
big_x = (x / z) * big_z # Use law of similar trianges to solve
big_y = (y / z) * big_z
big_x_center = (x_center / z_center) * big_z
big_y_center = (y_center / z_center) * big_z
#print("The x_center is " + str(x_center) + " the y_center is " + str(y_center) + " and the z_center is " + str(
# z_center))
#print(
#"The x distance is " + str(big_x - big_x_center) + " the y distance is " + str(big_y - big_y_center) + " and the camera name is " + camera_name + "\n")
if offset:
return big_x - big_x_center, big_y - big_y_center
else:
return big_x, big_y
def broadcast_poses():
robot = robot_interface.Robot_Interface()
cam = camera.RGBD()
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')
pcm = PCM()
pcm.fromCameraInfo(cam_info)
point = (208 + (323 - 208) / 2, 247 + (424 - 247) / 2)
print(point)
#(208.076538,247.264099) (323.411957,242.667325) (204.806457,424.053619) (324.232857,434.011618)
dep_data = robot.get_img_data()[1]
print(dep_data)
dep = robot.get_depth(point, dep_data)
print(dep)
br = tf.TransformBroadcaster()
td_points = pcm.projectPixelTo3dRay(point)
# pose = np.array([td_points[0], td_points[1], 0.001 * dep])
norm_pose = np.array(td_points)
norm_pose = norm_pose / norm_pose[2]
norm_pose = norm_pose * (dep)
a = tf.transformations.quaternion_from_euler(ai=-2.355, aj=-3.14, ak=0.0)
b = tf.transformations.quaternion_from_euler(ai=0.0, aj=0.0, ak=1.57)
base_rot = tf.transformations.quaternion_multiply(a, b)
br.sendTransform(norm_pose, base_rot, rospy.Time.now(), 'tree',
'head_camera_rgb_optical_frame')
def get_camera_model(bag, camera_name):
camera_infos = [msg.message for msg in bag.read_messages(camera_name)]
camera_info = camera_infos[0]
from image_geometry import PinholeCameraModel
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(camera_info)
return camera_model
def process_msgs(msgs, cinfo, shift):
ret = len(msgs) * [None]
cam_model = PinholeCameraModel()
if cinfo is not None:
cam_model.fromCameraInfo(cinfo)
offset = None
for it in range(0, len(msgs)):
cur_msg = msgs[it]
out_msg = msg(cur_msg.header.stamp.to_sec(), list())
if cinfo is not None:
# rospy.loginfo("using cinfo")
for det in cur_msg.points:
xyz = np.array([det.x, det.y, det.z])
if offset is None:
offset = xyz
xyz = xyz - offset + shift
# xyz = np.dot(xyz, R)
out_msg.positions.append(xyz)
else:
# rospy.loginfo("not using cinfo")
x = cur_msg.pose.pose.position.x
y = cur_msg.pose.pose.position.y
z = cur_msg.pose.pose.position.z
out_msg.positions.append(np.array([x, y, z]))
ret[it] = out_msg
return ret
class CamPixelToPointServer:
def __init__(self):
self.camera_model = PinholeCameraModel()
self.bridge = CvBridge()
self.model_set = False
self.tf_listener = TransformListener()
def pixel_to_point(self,
cam_pos,
out_frame='map'): # type: (np.ndarray) -> PointStamped
if not self.model_set:
self.camera_model.fromCameraInfo(
rospy.wait_for_message('/camera/rgb/camera_info', CameraInfo))
self.model_set = True
x, y = int(cam_pos[0]), int(cam_pos[1])
d = self.read_depth_simple(x, y)
pos = np.array(self.camera_model.projectPixelTo3dRay((x, y))) * d
point = PointStamped()
point.header.frame_id = self.camera_model.tfFrame()
point.point.x, point.point.y, point.point.z = pos[0], pos[1], pos[2]
point = convert_frame(self.tf_listener, point, out_frame)
return point
def read_depth_simple(self, x, y): # (int, int) -> float
image = rospy.wait_for_message('/camera/depth_registered/image_raw',
Image)
image = self.bridge.imgmsg_to_cv2(image)
return image[y, x]
def centroid_to_pose(self, rect, moment):
"""
Publish the region of interest as a geometry_msgs/Pose message from
the ROI or the center of mass coordinates
@param rect - sensor_msgs/RegionOfInterest
@param moment - object's center of mass
"""
if not moment:
moment = (int(rect[0] + rect[2] / 2), int(rect[1] + rect[3] / 2))
u, v = int(moment[0]), int(moment[1])
ps = Pose()
ps.orientation.x = 0.0
ps.orientation.y = 0.0
ps.orientation.z = 0.0
ps.orientation.w = 1.0
cam = PinholeCameraModel()
cam.fromCameraInfo(self.cam_info)
(x, y, z) = cam.projectPixelTo3dRay((u, v))
ps.position.x = x
ps.position.y = y
ps.position.z = z
return ps
class RGBD_Project(object):
def __init__(self):
self.cam = RGBD()
not_read = True
while not_read:
try:
cam_info = self.cam.read_info_data()
if (not cam_info == None):
not_read = False
except:
rospy.logerr('info not recieved')
self.pcm = PCM()
self.pcm.fromCameraInfo(cam_info)
def deproject_pose(self, pose):
""""
pose = (u_x,u_y,z)
u_x,u_y correspond to pixel value in image
x corresponds to depth
"""
td_points = self.pcm.projectPixelTo3dRay((pose[0], pose[1]))
norm_pose = np.array(td_points)
norm_pose = norm_pose / norm_pose[2]
norm_pose = norm_pose * (cfg.MM_TO_M * pose[2])
return norm_pose
class CamPixelToPointServer:
def __init__(self):
self.camera_model = PinholeCameraModel()
self.bridge = CvBridge()
self.camera_model.fromCameraInfo(SubscriberValue('camera_info', CameraInfo).value)
self.depth_image = SubscriberValue('camera_depth_image', Image, transform=self.bridge.imgmsg_to_cv2)
self.service = rospy.Service('cam_pixel_to_point', CamPixelToPoint, self.handle_service)
self.tf_listener = TransformListener()
print('Service is ready.')
def handle_service(self, req): # type: (CamPixelToPoint) -> CamPixelToPointResponse
x, y = int(req.cam_pixel.x), int(req.cam_pixel.y)
methods = [self.read_depth_simple,
# self.read_depth_average,
self.read_depth_as_floor_depth]
for method in methods:
d = method(x, y)
if not np.isnan(d):
break
pos = np.array(self.camera_model.projectPixelTo3dRay((x, y))) * d
point = PointStamped()
point.header.frame_id = self.camera_model.tfFrame()
point.point.x, point.point.y, point.point.z = pos[0], pos[1], pos[2]
return CamPixelToPointResponse(point)
def read_depth_simple(self, x, y): # (int, int) -> float
return self.depth_image.value[y, x]
def read_depth_average(self, x, y): # (int, int) -> float
print('Fallback to average')
s = 5
return np.nanmean(self.depth_image.value[y-s:y+s, x-s:x+s])
def read_depth_as_floor_depth(self, x, y): # (int, int) -> float
print('Fallback to floor model')
min_distance = 10.0
# Extend the camera ray until it passes through where the floor should be. Use its length as the depth.
camera_origin = PointStamped()
camera_origin.header.frame_id = self.camera_model.tfFrame()
camera_origin.point.x, camera_origin.point.y, camera_origin.point.z = 0.0, 0.0, 0.0
point_along_ray = PointStamped()
point_along_ray.header.frame_id = self.camera_model.tfFrame()
point_along_ray.point.x, point_along_ray.point.y, point_along_ray.point.z = self.camera_model.projectPixelTo3dRay((x, y))
self.tf_listener.waitForTransform('base_footprint', self.camera_model.tfFrame(), rospy.Time(rospy.get_time()), rospy.Duration(1))
camera_origin = self.tf_listener.transformPoint('base_footprint', camera_origin)
point_along_ray = self.tf_listener.transformPoint('base_footprint', point_along_ray)
camera_origin = np_from_poinstamped(camera_origin)
point_along_ray = np_from_poinstamped(point_along_ray)
ray_dir = point_along_ray - camera_origin
# Assuming this transformation was orthogonal, |ray_dir| = 1, at least approximately
d = camera_origin[1]/max(-ray_dir[1], camera_origin[1]/min_distance)
if d <= 0.01:
d = np.nan
return d
class DrawRect(): def __init__(self, topic_image, topic_blob, topic_info, topic_out): self.sync1_callback = False #synchronize the image self.sync2_callback = False #synchronize the camera info self.bridge = CvBridge() rospy.Subscriber(topic_image, Image, self.image_callback) rospy.Subscriber(topic_blob, Blobs, self.blob_callback) rospy.Subscriber(topic_info, CameraInfo, self.info_callback) self.pub = rospy.Publisher(topic_out, PointStamped) def image_callback(self, image): self.sync1_callback = True self.image = self.bridge.imgmsg_to_cv(image) self.image = np.asarray(self.image) def info_callback (self, info): #get the camera information self.sync2_callback = True self.info = info def blob_callback(self, blob): if self.sync1_callback and self.sync2_callback and (blob.blob_count != 0): bleb = self.FindBiggestBlob (blob) z = self.image[bleb.y][bleb.x] # depth in mm (INT!) z /= 1000.0 # now in meters (FLOAT!) self.UVtoXYZ = PinholeCameraModel() self.UVtoXYZ.fromCameraInfo(self.info) vec = self.UVtoXYZ.projectPixelTo3dRay ((bleb.x, bleb.y)) vec = [x * (z/vec[2]) for x in vec] self.P = PointStamped() self.P.header.frame_id = self.info.header.frame_id self.P.point.x = vec[0] #the np make the downward direction positive direction, #so we need to multiply to -1 to make upward direction #positive direction self.P.point.y = vec[1] self.P.point.z = vec[2] print self.P self.pub.publish(self.P) def FindBiggestBlob(self, blob): #Assume the hat is the only green blob in front of the camera x = 0 bleb = None while x < blob.blob_count: if (x == 0): bleb = blob.blobs[x] else: if (blob.blobs[x].area > bleb.area): bleb = blob.blobs[x] x = x + 1 return bleb
class WorldProjector(object):
def __init__(self, camera_info, h_matrix, distorted_input=True):
self.distorted_input = distorted_input
self.camera_info = camera_info
self.fisheye = False
if camera_info.distortion_model == "fisheye":
self.camera = FisheyeCameraModel()
self.camera.from_camera_info(camera_info)
self.fisheye = True
else:
self.camera = PinholeCameraModel()
self.camera.fromCameraInfo(camera_info)
self.h_matrix = h_matrix
def pixel2ground(self, pixel_coord, distorted_input=None):
"""Returns the ground projection of a pixel."""
# use default value if 'distorted_input' is not set
if distorted_input is None:
distorted_input = self.distorted_input
if distorted_input:
pixel_coord = self.rectify_pixel(pixel_coord)
pixel_coord = np.array(pixel_coord)
# normalize coordinates
pixel_normalized = normalize_image_coordinates(
pixel_coord,
self.camera.width,
self.camera.height)
point = self._project_normalized_pixel(pixel_normalized)
return point
def _project_normalized_pixel(self, pixel_normalized):
# create homogeneous coordinate
pixel_coord = np.append(pixel_normalized, 1.0)
# project to world
ground_coord = np.dot(self.h_matrix, pixel_coord)
# scale the coordinates appropriately -> transform to eucledian space
ground_coord = ground_coord / ground_coord[2]
return ground_coord
def rectify_pixel(self, pixel_coord):
"""Calculates the rectified undistorted image coordinate of a pixel.
Args:
pixel_coord: Distorted pixel coordinates
Returns: Undistorted pixel coordinates
"""
if self.fisheye:
return self.camera.undistort_point(pixel_coord)
else:
return self.camera.rectifyPoint(pixel_coord)
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 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 get_rays(self):
model = PinholeCameraModel()
model.fromCameraInfo(self.depth_info)
self.array_rays = numpy.zeros((self.image_depth.height, self.image_depth.width, 3))
for u in range(self.image_depth.height):
for v in range(self.image_depth.width):
ray = model.projectPixelTo3dRay((u, v))
ray_z = [el / ray[2] for el in ray] # normalize the ray so its Z-component equals 1.0
self.array_rays[u, v, :] = ray_z
def create_cam_model(info_msg):
from image_geometry import PinholeCameraModel
import numpy as np
cam_model = PinholeCameraModel()
# <have an info message >
cam_model.fromCameraInfo(info_msg)
print 'cam_model = ', np.array(cam_model.intrinsicMatrix())
print 'cam tf = ', cam_model.tfFrame()
return cam_model
def make_pixel_to_3d(self):
cm = PinholeCameraModel()
cm.fromCameraInfo(self.camera_info)
self.p3d = np.zeros(
[self.camera_info.height, self.camera_info.width, 3])
for v in range(0, self.camera_info.height):
for u in range(0, self.camera_info.width):
uv_rect = cm.rectifyPoint((u, v))
p = cm.projectPixelTo3dRay(uv_rect)
self.p3d[v, u, :] = (p[0] / p[2], p[1] / p[2], 1.0
) # make homogeneous
def localize_detection(self, detection):
"""
2d image detection -> 3d point in [map frame]
"""
u = detection.x + detection.width / 2
v = detection.y + detection.height / 2
camera_info = None
best_time = 100
for ci in self.camera_infos:
time = abs(ci.header.stamp.to_sec() -
detection.header.stamp.to_sec())
if time < best_time:
camera_info = ci
best_time = time
if not camera_info or best_time > 1:
return False, None
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(camera_info)
point = Point(
((u - camera_model.cx()) - camera_model.Tx()) / camera_model.fx(),
((v - camera_model.cy()) - camera_model.Ty()) / camera_model.fy(),
1)
localization = self.localize(detection.header, point,
self.region_scope)
if not localization:
return False, None
point_trans = None
print "Face localized ", localization.pose.position.x, localization.pose.position.y
if localization.pose.position.x == 0.0:
print "Ignoring this one!"
return False, None
try:
#(pos_trans, rot) = self.tf_listener.lookupTransform('/map', detection.header.frame_id, rospy.Time(0))
point_trans = self.tf_listener.transformPoint(
'/map',
PointStamped(detection.header, localization.pose.position))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
pass
if point_trans == None: # transformation failed
return False, None
print "Face detected at %d,%d" % (point_trans.point.x,
point_trans.point.y)
return True, point_trans.point
def create_cam_model(info_msg):
from image_geometry import PinholeCameraModel
import numpy as np
cam_model = PinholeCameraModel()
# <have an info message >
cam_model.fromCameraInfo(info_msg)
print "cam_model = ", np.array(cam_model.intrinsicMatrix())
print "cam tf = ", cam_model.tfFrame()
return cam_model
def get_rays(self):
model = PinholeCameraModel()
model.fromCameraInfo(self.depth_info)
self.array_rays = numpy.zeros(
(self.image_depth.height, self.image_depth.width, 3))
for u in range(self.image_depth.height):
for v in range(self.image_depth.width):
ray = model.projectPixelTo3dRay((u, v))
ray_z = [el / ray[2] for el in ray
] # normalize the ray so its Z-component equals 1.0
self.array_rays[u, v, :] = ray_z
class AtLocalization:
"""
Handles image rectification and april tag detection.
"""
def __init__(self, camera_info, tag_size):
# init image rectification
self.pcm = PinholeCameraModel()
self.pcm.fromCameraInfo(camera_info)
self.K_rect, self.mapx, self.mapy = self._init_rectification()
# init apriltag detector
self.at_detector = Detector(searchpath=['apriltags'],
families='tag36h11',
nthreads=4,
quad_decimate=4.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
self.tag_size = tag_size
self.camera_params = (self.K_rect[0, 0], self.K_rect[1, 1],
self.K_rect[0, 2], self.K_rect[1, 2])
def _init_rectification(self):
"""
Establish rectification mapping.
"""
w = self.pcm.width
h = self.pcm.height
K_rect, roi = cv2.getOptimalNewCameraMatrix(self.pcm.K, self.pcm.D,
(w, h), 1.0)
mapx = np.ndarray(shape=(h, w, 1), dtype='float32')
mapy = np.ndarray(shape=(h, w, 1), dtype='float32')
mapx, mapy = cv2.initUndistortRectifyMap(self.pcm.K, self.pcm.D,
self.pcm.R, self.pcm.P,
(w, h), cv2.CV_32FC1, mapx,
mapy)
return K_rect, mapx, mapy
def rectify(self, img_raw, interpolation=cv2.INTER_NEAREST):
"""
Rectify image.
"""
return cv2.remap(img_raw, self.mapx, self.mapy, interpolation)
def detect(self, img):
img_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
tags = self.at_detector.detect(img_gray,
estimate_tag_pose=True,
camera_params=self.camera_params,
tag_size=self.tag_size)
return tags
def camera_model(self, msg):
pcm = PinholeCameraModel()
pcm.fromCameraInfo(msg)
(self.m1, self.m2) = cv2.fisheye.initUndistortRectifyMap(
pcm.K, pcm.D[:4], pcm.R, pcm.P, (msg.width, msg.height),
cv2.CV_32FC1)
print('K\n{}\nD\n{}\nR\n{}\nP\n{}'.format(pcm.K, pcm.D[:4], pcm.R,
pcm.P))
#PP = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(pcm.K, pcm.D[:4],
# (msg.width, msg.height), pcm.R, pcm.P)
#print('PP\n{}\nm1{}\nm2{}'.format(PP, self.m1, self.m2))
self.subscribe_camera()
class VisionNode(object):
'''
Base class to be used unify the interfacing for MIL's computer vision scripts.
Handles the bootstrap of image subscription, enable/disable, etc.
Provides a callback for new images which is expected to return
'''
__metaclass__ = abc.ABCMeta
def __init__(self):
self._objects_pub = rospy.Publisher("~identified_objects", ObjectsInImage, queue_size=3)
self._camera_info = None
self.camera_model = None
self._enabled = False
self._image_sub = Image_Subscriber("image", callback=self._img_cb)
if rospy.get_param("~autostart", default=False):
self._enable()
else:
self._disable()
self._enable_srv = rospy.Service("~enable", SetBool, self._enable_cb)
def _enable_cb(self, req):
if req.data and not self._enabled:
self._enable()
elif not req.data and self._enabled:
self._disable()
return {'success': True}
def _enable(self):
if self._camera_info is None:
self._camera_info = self._image_sub.wait_for_camera_info()
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(self._camera_info)
self._enabled = True
rospy.loginfo("Enabled.")
def _disable(self):
self._enabled = False
rospy.loginfo("Disabled.")
def _img_cb(self, img):
if not self._enabled:
return
msg = ObjectsInImage()
msg.header = self._image_sub.last_image_header
msg.objects = self.find_objects(img)
if not isinstance(msg.objects, list) or (len(msg.objects) and not isinstance(msg.objects[0], ObjectInImage)):
rospy.logwarn("find_objects did not return a list of mil_msgs/ObjectInImage message. Ignoring.")
self._objects_pub.publish(msg)
@abc.abstractmethod
def find_objects(self, img):
pass
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)
def remap(raw_img, camera_info: CameraInfo):
cam = PinholeCameraModel()
cam.fromCameraInfo(camera_info)
map_x, map_y = cv2.initUndistortRectifyMap(
cam.intrinsicMatrix(),
cam.distortionCoeffs(),
cam.R,
cam.projectionMatrix(),
(cam.width, cam.height),
cv2.CV_32FC1,
)
rectified_image = cv2.remap(raw_img, map_x, map_y, cv2.INTER_CUBIC)
return rectified_image
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 PointFromPixel():
""" Given a pixel location, find its 3D location in the world """
def __init__(self, topic_camera_info, topic_depth):
self.need_camera_info = True
self.need_depth_image = True
self.model = PinholeCameraModel()
rospy.Subscriber(topic_camera_info, CameraInfo, self.callback_camera_info)
rospy.Subscriber(topic_depth, Image, self.callback_depth_image)
def callback_camera_info(self, info):
""" Define Pinhole Camera Model parameters using camera info msg """
if self.need_camera_info:
rospy.loginfo('Got camera info!')
self.model.fromCameraInfo(info) # define model params
self.frame = info.header.frame_id
self.need_camera_info = False
def callback_depth_image(self, depth_image):
""" Get depth at chosen pixel using depth image """
if self.need_depth_image:
rospy.loginfo('Got depth image!')
self.depth = img.fromstring("F", (depth_image.width, depth_image.height), depth_image.data)
self.need_depth_image = False
def calculate_3d_point(self, pixel):
""" Project ray through chosen pixel, then use pixel depth to get 3d point """
lookup = self.depth.load()
depth = lookup[pixel[0], pixel[1]] # lookup pixel in depth image
ray = self.model.projectPixelTo3dRay(tuple(pixel)) # get 3d ray of unit length through desired pixel
ray_z = [el / ray[2] for el in ray] # normalize the ray so its Z-component equals 1.0
pt = [el * depth for el in ray_z] # multiply the ray by the depth; its Z-component should now equal the depth value
point = PointStamped()
point.header.frame_id = self.frame
point.point.x = pt[0]
point.point.y = pt[1]
point.point.z = pt[2]
return point
def ray_plane_intersection(self, pixel, plane):
""" Given plane parameters [a, b, c, d] as in ax+by+cz+d=0,
finds intersection of 3D ray with the plane. """
ray = self.model.projectPixelTo3dRay(tuple(pixel)) # get 3d ray of unit length through desired pixel
scale = -plane[3] / (plane[0]*ray[0] + plane[1]*ray[1] + plane[2]*ray[2])
point = PointStamped()
point.header.frame_id = self.frame
point.point.x = ray[0] * scale
point.point.y = ray[1] * scale
point.point.z = ray[2] * scale
return point
def img_callback(self, msg):
cv_image = self.bridge.imgmsg_to_cv2(msg,
desired_encoding='passthrough')
#Respond to attribute error if subscribers haven't ran yet
try:
objects, img = self.yolo.search_for_objects(cv_image)
self.img_pub.publish(
self.bridge.cv2_to_imgmsg(img, encoding='passthrough'))
model = PinholeCameraModel()
model.fromCameraInfo(self.cam_info)
except AttributeError:
print("waiting")
return
if (len(objects) != 0):
for obj in objects:
print(obj)
xy = obj["Point"]
dist = self.depth_finder.get_depth(int(xy[0]), int(xy[1]))
depth = dist[0]
vect = model.projectPixelTo3dRay((xy[0], xy[1]))
xyz = [el / vect[2] for el in vect]
stampedPoint = PointStamped()
stampedPoint.header.frame_id = "head_rgbd_sensor_rgb_frame"
stampedPoint.point.x = xyz[0] * depth
stampedPoint.point.y = xyz[1] * depth
stampedPoint.point.z = depth
rospy.wait_for_service('transform_point')
get_3d_points = rospy.ServiceProxy('transform_point',
LocalizePoint)
resp = get_3d_points(stampedPoint)
#print(resp.localizedPointMap)
threeDPoint = resp.localizedPointMap
dictMsg = {}
dictMsg["name"] = obj["Label"]
dictMsg["type"] = "object"
dictMsg["coords"] = [
threeDPoint.point.x, threeDPoint.point.y,
threeDPoint.point.z
]
dictMsg["others"] = {}
self.nav_pub.publish(json.dumps(dictMsg))
else:
print("No objects found.")
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)
if test_mode:
cap = cv2.VideoCapture(0)
size = (int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)),
int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)))
else:
ic = image_converter()
cam_model = PinholeCameraModel()
while (not camera_info) and (not rospy.is_shutdown()):
time.sleep(1)
print("waiting for camera info")
cam_model.fromCameraInfo(camera_info)
size = cam_model.fullResolution()
print(cam_model.cx(), cam_model.cy(), cam_model.fullResolution())
fps = 30
rate = rospy.Rate(fps)
fourcc = cv2.VideoWriter_fourcc(*'XVID')
writer = cv2.VideoWriter()
success = writer.open(file, fourcc, fps, size, True)
while not rospy.is_shutdown():
if test_mode:
ret, image = cap.read()
writer.write(image)
cv2.imshow("Image window", image)
if cv2.waitKey(1) & 0xFF == 27:
break
rate.sleep()
if test_mode:
cap.release()
writer.release()
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
def run(sub_singleton):
global marker_found
nh = sub_singleton._node_handle
#next_pose = yield nh.get_service_client('/next_search_pose', SearchPose)
cam = PinholeCameraModel()
# Go to max height to see as much as we can.
yield sub_singleton.move.depth(SEARCH_DEPTH).zero_roll_and_pitch().go()
yield nh.sleep(1.0)
# ---------------------------------------
# MOST OF THIS IS FOR THE SEARCH PATTERN.
# ---------------------------------------
# This is just to get camera info
resp = yield sub_singleton.channel_marker.get_2d()
cam.fromCameraInfo(resp.camera_info)
# intial_pose = sub_singleton.last_pose()
# intial_height = yield sub_singleton.get_dvl_range()
# radius = calculate_visual_radius(cam, intial_height)
# s = SearchPoseRequest()
# s.intial_position = (yield intial_pose).pose.pose
# s.search_radius = radius
# s.reset_search = True
# yield next_pose(s)
# s.reset_search = False
marker_found = False
goer = go_to_marker(nh, sub_singleton, cam)
# while not marker_found:
# # Move in search pattern until we find the marker
# resp = yield next_pose(s)
# print resp
# if resp.area > .8:
# continue
# print pose_to_numpy(resp.target_pose)[0]
# yield sub_singleton.move.set_position(pose_to_numpy(resp.target_pose)[0]).zero_roll_and_pitch().go()
# print "Searcher arrived"
yield goer
def run(sub_singleton):
global marker_found
nh = sub_singleton._node_handle
#next_pose = yield nh.get_service_client('/next_search_pose', SearchPose)
cam = PinholeCameraModel()
# Go to max height to see as much as we can.
yield sub_singleton.move.depth(SEARCH_DEPTH).zero_roll_and_pitch().go()
yield nh.sleep(1.0)
# ---------------------------------------
# MOST OF THIS IS FOR THE SEARCH PATTERN.
# ---------------------------------------
# This is just to get camera info
resp = yield sub_singleton.channel_marker.get_2d()
cam.fromCameraInfo(resp.camera_info)
# intial_pose = sub_singleton.last_pose()
# intial_height = yield sub_singleton.get_dvl_range()
# radius = calculate_visual_radius(cam, intial_height)
# s = SearchPoseRequest()
# s.intial_position = (yield intial_pose).pose.pose
# s.search_radius = radius
# s.reset_search = True
# yield next_pose(s)
# s.reset_search = False
marker_found = False
goer = go_to_marker(nh, sub_singleton, cam)
# while not marker_found:
# # Move in search pattern until we find the marker
# resp = yield next_pose(s)
# print resp
# if resp.area > .8:
# continue
# print pose_to_numpy(resp.target_pose)[0]
# yield sub_singleton.move.set_position(pose_to_numpy(resp.target_pose)[0]).zero_roll_and_pitch().go()
# print "Searcher arrived"
yield goer
def unit_vector_to_point(self, x, y):
'''
Creates a unit vector from the camera's frame, given a pixel point from the image
'''
height, width, depth = self.frame # Obtain the dimensions of the frame
cam_info = rospy.wait_for_message("/cameras/" + self.camera_viewer +
"/camera_info",
CameraInfo,
timeout=None)
img_proc = PinholeCameraModel()
img_proc.fromCameraInfo(cam_info)
# The origin of the camera is initally set to the top left corner
# However the projectPixelTo3dRay uses the origin at the center of the camera. Hence the coordinates have to be converted
x = x - width / 2
y = height / 2 - y
# Creates a unit vector to the given point. Unit vector passes through point from the center of the camera
n = img_proc.projectPixelTo3dRay((x, y))
return n
def run(sub_singleton):
global marker_found
print "Searching"
nh = sub_singleton._node_handle
next_pose = yield nh.get_service_client('/next_search_pose', SearchPose)
cam = PinholeCameraModel()
# Go to max height to see as much as we can.
yield sub_singleton.move.depth(0.6).zero_roll_and_pitch().go()
yield nh.sleep(1.0)
# This is just to get camera info
resp = yield sub_singleton.channel_marker.get_2d()
cam.fromCameraInfo(resp.camera_info)
intial_pose = sub_singleton.last_pose()
intial_height = yield sub_singleton.get_dvl_range()
raidus = calculate_visual_radius(cam, intial_height)
s = SearchPoseRequest()
s.intial_position = (yield intial_pose).pose.pose
s.search_radius = raidus
s.reset_search = True
yield next_pose(s)
s.reset_search = False
marker_found = False
goer = go_to_marker(nh, sub_singleton, cam)
while not marker_found:
# Move in search pattern until we find the marker
resp = yield next_pose(s)
print resp
if resp.area > .8:
continue
print pose_to_numpy(resp.target_pose)[0]
yield sub_singleton.move.set_position(pose_to_numpy(resp.target_pose)[0]).zero_roll_and_pitch().go()
print "Searcher arrived"
yield goer
def random_point_inside_fov(camera_info, maxdist, Tcamera=np.eye(4)): """ Generates a random XYZ point inside the camera field of view @type camera_info: sensor_msgs.CameraInfo @param camera_info: Message with the meta information for a camera @type maxdist: float @param maxdist: distance from the camera ref frame in the z direction @type Tcamera: np.array @param Tcamera: homogeneous transformation for the camera ref frame @rtype: array @return: The random XYZ point """ cam_model = PinholeCameraModel() cam_model.fromCameraInfo(camera_info) z = maxdist*np.random.random() delta_x = cam_model.getDeltaX(cam_model.width/2, z) delta_y = cam_model.getDeltaY(cam_model.height/2, z) point = np.array([0, 0, z, 1]) point[:2] = np.array([delta_x, delta_y]) * (2*np.random.random_sample(2) - 1.) return np.dot(Tcamera, point)[:3]
def camera_fov_corners(camera_info, zdist, Tcamera=np.eye(4)):
"""
Generates the 5 corners of the camera field of view
@type camera_info: sensor_msgs.CameraInfo
@param camera_info: Message with the meta information for a camera
@type zdist: float
@param zdist: distance from the camera ref frame in the z direction
@type Tcamera: np.array
@param Tcamera: homogeneous transformation for the camera ref frame
@rtype: list
@return: The 5 corners of the camera field of view
"""
cam_model = PinholeCameraModel()
cam_model.fromCameraInfo(camera_info)
delta_x = cam_model.getDeltaX(cam_model.width/2, zdist)
delta_y = cam_model.getDeltaY(cam_model.height/2, zdist)
corners = [Tcamera[:3,3]]
for k in itertools.product([-1,1],[-1,1]):
point = np.array([0, 0, zdist, 1])
point[:2] = np.array([delta_x, delta_y]) * np.array(k)
corners.append( np.dot(Tcamera, point)[:3] )
return np.array(corners)
def store_image_info_cb(msg, topic_name):
''' Stores the topic name, pinhole camera model object, and
matrices from the camera to the map in pickle fiels.
'''
import pickle
import os
import rospkg
print "Storing info"
pinhole_camera_model = PinholeCameraModel()
pinhole_camera_model.fromCameraInfo(msg)
matrices = PixelConversion.get_matrices(msg.header.frame_id, "/map")
filename = topic_name.replace("/", "_") + ".pickle"
rospack = rospkg.RosPack()
pkg_path = rospack.get_path('contamination_monitor')
directory = os.path.join(pkg_path, "eval", "camera_map_tfs")
if not os.path.exists(directory):
os.makedirs(directory)
filepath = os.path.join(directory, filename)
with open(filepath, 'wb+') as f:
pickle.dump(topic_name, f)
pickle.dump(pinhole_camera_model, f)
pickle.dump(matrices, f)
print "pickle dumped to ", filepath
def get_point_cloud(self):
# Scan the scene
col_associations = self.get_projector_line_associations()
# Project white light onto the scene to get the intensities of each pixel (for coloring our point cloud)
illumination_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
cv.Set(illumination_projection, 255)
intensities_image = self.get_picture_of_projection(illumination_projection)
# Turn projector off when done with it
off_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
cv.SetZero(off_projection)
off_image_message = self.bridge.cv_to_imgmsg(off_projection, encoding="mono8")
self.set_projection(off_image_message)
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(self.camera_info)
valid_points_mask = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
cv.CmpS(col_associations, -1, valid_points_mask, cv.CV_CMP_NE)
number_of_valid_points = cv.CountNonZero(valid_points_mask)
rectified_camera_coordinates = cv.CreateMat(1, number_of_valid_points, cv.CV_32FC2)
scanline_associations = cv.CreateMat(1, number_of_valid_points, cv.CV_32FC1)
intensities = cv.CreateMat(1, number_of_valid_points, cv.CV_8UC1)
i = 0;
for row in range(self.camera_info.height):
for col in range(self.camera_info.width):
if valid_points_mask[row, col] != 0:
rectified_camera_coordinates[0, i] = (col, row)
scanline_associations[0, i] = col_associations[row, col]
intensities[0, i] = intensities_image[row, col]
i += 1
cv.UndistortPoints(rectified_camera_coordinates, rectified_camera_coordinates, camera_model.intrinsicMatrix(), camera_model.distortionCoeffs())
# Convert scanline numbers to plane normal vectors
planes = self.scanline_numbers_to_planes(scanline_associations)
camera_rays = project_pixels_to_3d_rays(rectified_camera_coordinates, camera_model)
intersection_points = ray_plane_intersections(camera_rays, planes)
self.point_cloud_msg = sensor_msgs.msg.PointCloud()
self.point_cloud_msg.header.stamp = rospy.Time.now()
self.point_cloud_msg.header.frame_id = 'base_link'
channels = []
channel = sensor_msgs.msg.ChannelFloat32()
channel.name = "intensity"
points = []
intensities_list = []
for i in range(number_of_valid_points):
point = geometry_msgs.msg.Point32()
point.x = intersection_points[0, i][0]
point.y = intersection_points[0, i][1]
point.z = intersection_points[0, i][2]
if point.z < 0:
print scanline_associations[0, i]
print rectified_camera_coordinates[0, i]
points.append(point)
intensity = intensities[0, i]
intensities_list.append(intensity)
channel.values = intensities_list
channels.append(channel)
self.point_cloud_msg.channels = channels
self.point_cloud_msg.points = points
return self.point_cloud_msg
class Scanner:
def __init__(self):
self.pixels_per_scanline = rospy.get_param('~pixels_per_scanline')
self.scanner_info_file_name = rospy.get_param('~scanner_info_file_name')
self.threshold = rospy.get_param('~threshold')
self.mutex = threading.RLock()
self.image_update_flag = threading.Event()
self.bridge = CvBridge()
rospy.Subscriber("image_stream", sensor_msgs.msg.Image, self.update_image)
rospy.loginfo("Waiting for camera info...")
self.camera_info = rospy.wait_for_message('camera_info', sensor_msgs.msg.CameraInfo)
rospy.loginfo("Camera info received.")
rospy.loginfo("Waiting for projector info service...")
rospy.wait_for_service('get_projector_info')
rospy.loginfo("Projector info service found.")
get_projector_info = rospy.ServiceProxy('get_projector_info', projector.srv.GetProjectorInfo)
self.projector_info = get_projector_info().projector_info
self.projector_model = PinholeCameraModel()
self.projector_model.fromCameraInfo(self.projector_info)
self.read_scanner_info()
self.projector_to_camera_rotation_matrix = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.Rodrigues2(self.projector_to_camera_rotation_vector, self.projector_to_camera_rotation_matrix)
predistortmap_x = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_32FC1)
predistortmap_y = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_32FC1)
InitPredistortMap(self.projector_model.intrinsicMatrix(), self.projector_model.distortionCoeffs(), predistortmap_x, predistortmap_y)
minVal, maxVal, minLoc, maxLoc = cv.MinMaxLoc(predistortmap_x)
cv.AddS(predistortmap_x, -minVal, predistortmap_x)
uncropped_projection_width = int(math.ceil(maxVal - minVal))
self.center_pixel = self.projector_model.cx() - minVal
minVal, maxVal, minLoc, maxLoc = cv.MinMaxLoc(predistortmap_y)
cv.AddS(predistortmap_y, -minVal, predistortmap_y)
uncropped_projection_height = int(math.ceil(maxVal - minVal))
self.number_of_scanlines = int(math.ceil(float(uncropped_projection_width)/self.pixels_per_scanline))
rospy.loginfo("Generating projection patterns...")
graycodes = []
for i in range(self.number_of_scanlines):
graycodes.append(graycodemath.generate_gray_code(i))
self.number_of_projection_patterns = int(math.ceil(math.log(self.number_of_scanlines, 2)))
self.predistorted_positive_projections = []
self.predistorted_negative_projections = []
for i in range(self.number_of_projection_patterns):
cross_section = cv.CreateMat(1, uncropped_projection_width, cv.CV_8UC1)
#Fill in cross section with the associated bit of each gray code
for pixel in range(uncropped_projection_width):
scanline = pixel // self.pixels_per_scanline
scanline_value = graycodemath.get_bit(graycodes[scanline], i) * 255
cross_section[0, pixel] = scanline_value
#Repeat the cross section over the entire image
positive_projection = cv.CreateMat(uncropped_projection_height, uncropped_projection_width, cv.CV_8UC1)
cv.Repeat(cross_section, positive_projection)
#Predistort the projections to compensate for projector optics so that that the scanlines are approximately planar
predistorted_positive_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
cv.Remap(positive_projection, predistorted_positive_projection, predistortmap_x, predistortmap_y, flags=cv.CV_INTER_NN)
#Create a negative of the pattern for thresholding
predistorted_negative_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
cv.Not(predistorted_positive_projection, predistorted_negative_projection)
#Fade the borders of the patterns to avoid artifacts at the edges of projection
predistorted_positive_projection_faded = fade_edges(predistorted_positive_projection, 20)
predistorted_negative_projection_faded = fade_edges(predistorted_negative_projection, 20)
self.predistorted_positive_projections.append(predistorted_positive_projection_faded)
self.predistorted_negative_projections.append(predistorted_negative_projection_faded)
rospy.loginfo("Waiting for projection setting service...")
rospy.wait_for_service('set_projection')
rospy.loginfo("Projection setting service found.")
self.set_projection = rospy.ServiceProxy('set_projection', projector.srv.SetProjection)
self.pixel_associations_msg = None
self.point_cloud_msg = None
rospy.Service("~get_point_cloud", graycode_scanner.srv.GetPointCloud, self.handle_point_cloud_srv)
point_cloud_pub = rospy.Publisher('~point_cloud', sensor_msgs.msg.PointCloud)
rospy.loginfo("Ready.")
rate = rospy.Rate(1)
while not rospy.is_shutdown():
if self.point_cloud_msg is not None:
point_cloud_pub.publish(self.point_cloud_msg)
rate.sleep()
def read_scanner_info(self):
try:
input_stream = file(self.scanner_info_file_name, 'r')
except IOError:
rospy.loginfo('Specified scanner info file at ' + self.scanner_info_file_name + ' does not exist.')
return
info_dict = yaml.load(input_stream)
try:
self.projector_to_camera_translation_vector = tuple(info_dict['projector_to_camera_translation_vector'])
self.projector_to_camera_rotation_vector = list_to_matrix(info_dict['projector_to_camera_rotation_vector'], 3, 1, cv.CV_32FC1)
except (TypeError, KeyError):
rospy.loginfo('Scanner info file at ' + self.scanner_info_file_name + ' is in an unrecognized format.')
return
rospy.loginfo('Scanner info successfully read from ' + self.scanner_info_file_name)
def update_image(self, imagemsg):
with self.mutex:
if not self.image_update_flag.is_set():
self.latest_image = self.bridge.imgmsg_to_cv(imagemsg, "mono8")
self.image_update_flag.set()
def get_next_image(self):
with self.mutex:
self.image_update_flag.clear()
self.image_update_flag.wait()
with self.mutex:
return self.latest_image
def get_picture_of_projection(self, projection):
image_message = self.bridge.cv_to_imgmsg(projection, encoding="mono8")
self.set_projection(image_message)
return self.get_next_image()
def get_projector_line_associations(self):
rospy.loginfo("Scanning...")
positives = []
negatives = []
for i in range(self.number_of_projection_patterns):
positives.append(self.get_picture_of_projection(self.predistorted_positive_projections[i]))
negatives.append(self.get_picture_of_projection(self.predistorted_negative_projections[i]))
rospy.loginfo("Thresholding...")
strike_sum = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
cv.SetZero(strike_sum)
gray_codes = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
cv.SetZero(gray_codes)
for i in range(self.number_of_projection_patterns):
difference = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
cv.Sub(positives[i], negatives[i], difference)
absolute_difference = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
cv.AbsDiff(positives[i], negatives[i], absolute_difference)
#Mark all the pixels that were "too close to call" and add them to the running total
strike_mask = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
cv.CmpS(absolute_difference, self.threshold, strike_mask, cv.CV_CMP_LT)
strikes = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
cv.Set(strikes, 1, strike_mask)
cv.Add(strikes, strike_sum, strike_sum)
#Set the corresponding bit in the gray_code
bit_mask = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
cv.CmpS(difference, 0, bit_mask, cv.CV_CMP_GT)
bit_values = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
cv.Set(bit_values, 2 ** i, bit_mask)
cv.Or(bit_values, gray_codes, gray_codes)
rospy.loginfo("Decoding...")
# Decode every gray code into binary
projector_line_associations = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
cv.Copy(gray_codes, projector_line_associations)
for i in range(cv.CV_MAT_DEPTH(cv.GetElemType(projector_line_associations)), -1, -1):
projector_line_associations_bitshifted_right = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
#Using ConvertScale with a shift of -0.5 to do integer division for bitshifting right
cv.ConvertScale(projector_line_associations, projector_line_associations_bitshifted_right, (2 ** -(2 ** i)), -0.5)
cv.Xor(projector_line_associations, projector_line_associations_bitshifted_right, projector_line_associations)
rospy.loginfo("Post processing...")
# Remove all pixels that were "too close to call" more than once
strikeout_mask = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
cv.CmpS(strike_sum, 1, strikeout_mask, cv.CV_CMP_GT)
cv.Set(projector_line_associations, -1, strikeout_mask)
# Remove all pixels that don't decode to a valid scanline number
invalid_scanline_mask = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
cv.InRangeS(projector_line_associations, 0, self.number_of_scanlines, invalid_scanline_mask)
cv.Not(invalid_scanline_mask, invalid_scanline_mask)
cv.Set(projector_line_associations, -1, invalid_scanline_mask)
self.display_scanline_associations(projector_line_associations)
return projector_line_associations
def handle_point_cloud_srv(self, req):
response = graycode_scanner.srv.GetPointCloudResponse()
self.get_point_cloud()
response.point_cloud = self.point_cloud_msg
response.success = True
return response
def get_point_cloud(self):
# Scan the scene
col_associations = self.get_projector_line_associations()
# Project white light onto the scene to get the intensities of each pixel (for coloring our point cloud)
illumination_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
cv.Set(illumination_projection, 255)
intensities_image = self.get_picture_of_projection(illumination_projection)
# Turn projector off when done with it
off_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
cv.SetZero(off_projection)
off_image_message = self.bridge.cv_to_imgmsg(off_projection, encoding="mono8")
self.set_projection(off_image_message)
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(self.camera_info)
valid_points_mask = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
cv.CmpS(col_associations, -1, valid_points_mask, cv.CV_CMP_NE)
number_of_valid_points = cv.CountNonZero(valid_points_mask)
rectified_camera_coordinates = cv.CreateMat(1, number_of_valid_points, cv.CV_32FC2)
scanline_associations = cv.CreateMat(1, number_of_valid_points, cv.CV_32FC1)
intensities = cv.CreateMat(1, number_of_valid_points, cv.CV_8UC1)
i = 0;
for row in range(self.camera_info.height):
for col in range(self.camera_info.width):
if valid_points_mask[row, col] != 0:
rectified_camera_coordinates[0, i] = (col, row)
scanline_associations[0, i] = col_associations[row, col]
intensities[0, i] = intensities_image[row, col]
i += 1
cv.UndistortPoints(rectified_camera_coordinates, rectified_camera_coordinates, camera_model.intrinsicMatrix(), camera_model.distortionCoeffs())
# Convert scanline numbers to plane normal vectors
planes = self.scanline_numbers_to_planes(scanline_associations)
camera_rays = project_pixels_to_3d_rays(rectified_camera_coordinates, camera_model)
intersection_points = ray_plane_intersections(camera_rays, planes)
self.point_cloud_msg = sensor_msgs.msg.PointCloud()
self.point_cloud_msg.header.stamp = rospy.Time.now()
self.point_cloud_msg.header.frame_id = 'base_link'
channels = []
channel = sensor_msgs.msg.ChannelFloat32()
channel.name = "intensity"
points = []
intensities_list = []
for i in range(number_of_valid_points):
point = geometry_msgs.msg.Point32()
point.x = intersection_points[0, i][0]
point.y = intersection_points[0, i][1]
point.z = intersection_points[0, i][2]
if point.z < 0:
print scanline_associations[0, i]
print rectified_camera_coordinates[0, i]
points.append(point)
intensity = intensities[0, i]
intensities_list.append(intensity)
channel.values = intensities_list
channels.append(channel)
self.point_cloud_msg.channels = channels
self.point_cloud_msg.points = points
return self.point_cloud_msg
def scanline_numbers_to_planes(self, scanline_numbers):
rows = scanline_numbers.height
cols = scanline_numbers.width
normal_vectors_x = cv.CreateMat(rows, cols, cv.CV_32FC1)
cv.Set(normal_vectors_x, -1)
normal_vectors_y = cv.CreateMat(rows, cols, cv.CV_32FC1)
cv.Set(normal_vectors_y, 0)
normal_vectors_z = cv.CreateMat(rows, cols, cv.CV_32FC1)
cv.Copy(scanline_numbers, normal_vectors_z)
cv.ConvertScale(normal_vectors_z, normal_vectors_z, scale=self.pixels_per_scanline)
cv.AddS(normal_vectors_z, -self.center_pixel, normal_vectors_z)
cv.ConvertScale(normal_vectors_z, normal_vectors_z, scale=1.0/self.projector_model.fx())
normal_vectors = cv.CreateMat(rows, cols, cv.CV_32FC3)
cv.Merge(normal_vectors_x, normal_vectors_y, normal_vectors_z, None, normal_vectors)
# Bring the normal vectors into camera coordinates
cv.Transform(normal_vectors, normal_vectors, self.projector_to_camera_rotation_matrix)
normal_vectors_split = [None]*3
for i in range(3):
normal_vectors_split[i] = cv.CreateMat(rows, cols, cv.CV_32FC1)
cv.Split(normal_vectors, normal_vectors_split[0], normal_vectors_split[1], normal_vectors_split[2], None)
n_dot_p = cv.CreateMat(rows, cols, cv.CV_32FC1)
cv.SetZero(n_dot_p)
for i in range(3):
cv.ScaleAdd(normal_vectors_split[i], self.projector_to_camera_translation_vector[i], n_dot_p, n_dot_p)
planes = cv.CreateMat(rows, cols, cv.CV_32FC4)
cv.Merge(normal_vectors_split[0], normal_vectors_split[1], normal_vectors_split[2], n_dot_p, planes)
return planes
def display_scanline_associations(self, associations):
display_image = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
cv.ConvertScale(associations, display_image, 255.0/self.number_of_scanlines)
cv.NamedWindow("associations", flags=0)
cv.ShowImage("associations", display_image)
cv.WaitKey(800)
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))
def project_onto_depth(xyz, z_desired):
""" Given an arbitrary vector in XYZ, scales such that Z-value is
as desired. """
scale = z_desired / xyz[2]
xyz = tuple(el * scale for el in xyz)
return xyz
if __name__ == "__main__":
bag_path = sys.argv[1] # path to camera_info
bag = rosbag.Bag(bag_path)
camera_infos = bag.read_messages(topics='/camera/rgb/camera_info')
camera_info = camera_infos.next()[1]
model = PinholeCameraModel()
model.fromCameraInfo(camera_info)
# Fig. 1: How many meters per pixel at various depths?
print model.projectPixelTo3dRay((319.5, 239.5))
u_neutral = 319.5
v_neutral = 239.5
depths = [d * 0.10 for d in range(10,50)] # approximate cutoff distance: 4m
res = []
for depth in depths:
primus = model.projectPixelTo3dRay((u_neutral, v_neutral))
secundus = model.projectPixelTo3dRay((u_neutral+1, v_neutral))
primus_at_depth = project_onto_depth(primus, depth)
secundus_at_depth = project_onto_depth(secundus, depth)
dXYZ = tuple(s - p for p,s in zip(primus_at_depth, secundus_at_depth))
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 MarkerOccGrid(OccGridUtils):
'''
Handles updating occupancy grid when new data comes in.
TODO: Upon call can return some path to go to in order to find them.
'''
def __init__(self, image_sub, grid_res, grid_width, grid_height, grid_starting_pose):
super(self.__class__, self).__init__(res=grid_res, width=grid_width, height=grid_height, starting_pose=grid_starting_pose)
self.tf_listener = tf.TransformListener()
self.cam = PinholeCameraModel()
self.camera_info = image_sub.wait_for_camera_info()
if self.camera_info is None:
# Maybe raise an alarm here.
rospy.logerr("I don't know what to do without my camera info.")
self.cam.fromCameraInfo(self.camera_info)
def update_grid(self, timestamp):
'''
Takes marker information to update occupacy grid.
'''
x_y_position, height = self.get_tf(timestamp)
self.add_circle(x_y_position, self.calculate_visual_radius(height))
self.publish_grid()
def add_marker(self, marker, timestamp):
'''
Find the actual 3d pose of the marker and fill in the occupancy grid for that pose.
This works by:
1. Calculate unit vector between marker point and the image center in the image frame.
2. Use height measurement to find real life distance (m) between center point and marker center.
3. Use unit vec and magnitude to find dx and dy in meters.
3. Pass info to OccGridUtils.
'''
if marker[0] is None:
return
x_y_position, height = self.get_tf(timestamp)
if marker[2] < self.calculate_marker_area(height) * .6:
# If the detected region isn't big enough dont add it.
rospy.logwarn("Marker found but it is too small, not adding marker.")
return
# Calculate position of marker accounting for camera rotation.
dir_vector = unit_vector(np.array([self.cam.cx(), self.cam.cy()] - marker[0]))
trans, rot = self.tf_listener.lookupTransform("/map", "/downward", timestamp)
cam_rotation = tf.transformations.euler_from_quaternion(rot)[2] + np.pi / 2
dir_vector = np.dot(dir_vector, make_2D_rotation(cam_rotation))
marker_rotation = cam_rotation + marker[1]
trans, rot = self.tf_listener.lookupTransform("/map", "/base_link", timestamp)
if (np.abs(np.array(rot)[:2]) > .005).any():
rospy.logwarn("We're at a weird angle, not adding marker.")
magnitude = self.calculate_visual_radius(height, second_point=marker[0])
local_position = dir_vector[::-1] * magnitude
position = local_position + x_y_position
#print local_position
# Pose on ground plane from center
pose = Pose2D(x=position[0], y=position[1], theta=marker_rotation)
self.found_marker(pose)
# The image coordinate pose and real life pose are different.
pose = Pose2D(x=position[0], y=position[1], theta=marker_rotation)
# In meters with initial point at (0,0)
return pose
def get_tf(self, timestamp=None):
'''
x_y position and height in meters
'''
if timestamp is None:
timestamp = rospy.Time()
self.tf_listener.waitForTransform("/map", "/downward", timestamp, rospy.Duration(1.0))
trans, rot = self.tf_listener.lookupTransform("/map", "/downward", timestamp)
x_y_position = trans[:2]
self.tf_listener.waitForTransform("/ground", "/downward", timestamp, rospy.Duration(1.0))
trans, _ = self.tf_listener.lookupTransform("/ground", "/downward", timestamp)
height = np.nan_to_num(trans[2])
return x_y_position, height
def correct_height(self, measured_height, timestamp):
'''
Adjust the measured height from the seafloor using our orientation relative to it.
We assume the floor is flat (should be valid for transdec but maybe not for pool).
All the math is just solving triangles.
Note: If the roll or pitch is too far off, the range could be to a point that is not
planar to the floor directly under us - in which case this will fail.
TODO: See if this actually can produce better results.
'''
trans, rot = self.tf_listener.lookupTransform("/map", "/base_link", timestamp)
euler_rotation = tf.transformations.euler_from_quaternion(rot)
roll_offset = np.abs(np.sin(euler_rotation[0]) * measured_height)
pitch_offset = np.abs(np.sin(euler_rotation[1]) * measured_height)
height = np.sqrt(measured_height ** 2 - (roll_offset ** 2 + pitch_offset ** 2))
return height
def calculate_marker_area(self, height):
'''
Esitmate what the area of the marker should be so we don't add incorrect markers to the occupancy grid.
What we really don't want is to add markers to the grid that are on the edge since the direction and center of
the marker are off.
'''
MARKER_LENGTH = 1.22 # m
MARKER_WIDTH = .1524 # m
# Get m/px on the ground floor.
m = self.calculate_visual_radius(height)
if self.cam.cy() < self.cam.cx():
px = self.cam.cy()
else:
px = self.cam.cx()
m_px = m / px
marker_area_m = MARKER_WIDTH * MARKER_LENGTH
marker_area_px = marker_area_m / (m_px ** 2)
return marker_area_px
def calculate_visual_radius(self, height, second_point=None):
'''
Draws rays to find the radius of the FOV of the camera in meters.
It also can work to find the distance between two planar points some distance from the camera.
'''
mid_ray = np.array([0, 0, 1])
if second_point is None:
if self.cam.cy() < self.cam.cx():
second_point = np.array([self.cam.cx(), 0])
else:
second_point = np.array([0, self.cam.cy()])
edge_ray = unit_vector(self.cam.projectPixelTo3dRay(second_point))
# Calculate angle between vectors and use that to find r
theta = np.arccos(np.dot(mid_ray, edge_ray))
return np.tan(theta) * height
def run(sub):
global SPEED
global pub_cam_ray
fprint('Enabling cam_ray publisher')
pub_cam_ray = yield sub.nh.advertise('/dice/cam_ray', Marker)
yield sub.nh.sleep(1)
fprint('Connecting camera')
cam_info_sub = yield sub.nh.subscribe('/camera/front/left/camera_info',
CameraInfo)
fprint('Obtaining cam info message')
cam_info = yield cam_info_sub.get_next_message()
model = PinholeCameraModel()
model.fromCameraInfo(cam_info)
enable_service = sub.nh.get_service_client("/dice/enable", SetBool)
yield enable_service(SetBoolRequest(data=True))
dice_sub = yield sub.nh.subscribe('/dice/points', Point)
found = {}
history_tf = {}
while len(found) != 2:
fprint('Getting dice xy')
dice_xy = yield dice_sub.get_next_message()
found[dice_xy.z] = mil_ros_tools.rosmsg_to_numpy(dice_xy)[:2]
fprint(found)
out = yield get_transform(sub, model, found[dice_xy.z])
history_tf[dice_xy.z] = out
if len(found) > 1:
tmp = found.values()[0] - found.values()[1]
# Make sure the two points are at least 100 pixels off
diff = np.hypot(tmp[0], tmp[1])
fprint('Distance between buoys {}'.format(diff))
if diff < 40:
found = {}
yield enable_service(SetBoolRequest(data=False))
start = sub.move.zero_roll_and_pitch()
yield start.go()
for i in range(2):
fprint('Hitting dice {}'.format(i))
# Get one of the dice
dice, xy = found.popitem()
fprint('dice {}'.format(dice))
ray, base = history_tf[dice]
where = base + 3 * ray
fprint(where)
fprint('Moving!', msg_color='yellow')
fprint('Current position: {}'.format(sub.pose.position))
fprint('zrp')
yield sub.move.zero_roll_and_pitch().go(blind=True)
yield sub.nh.sleep(4)
fprint('hitting', msg_color='yellow')
yield sub.move.look_at(where).go(blind=True, speed=SPEED)
yield sub.nh.sleep(4)
yield sub.move.set_position(where).go(blind=True, speed=SPEED)
yield sub.nh.sleep(4)
fprint('going back', msg_color='yellow')
yield start.go(blind=True, speed=SPEED)
yield sub.nh.sleep(4)
def calibrate(self, image, info, depth):
model = PinholeCameraModel()
model.fromCameraInfo(info)
try:
cv_img = self.bridge.imgmsg_to_cv2(image, "bgr8")
except CvBridgeError as e:
print (e)
return False
try:
cv_depth = self.bridge.imgmsg_to_cv2(depth)
except CvBridgeError as e:
print (e)
return False
cv_depth = cv2.medianBlur(cv_depth, 5)
cv_img = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(
cv_img,
(9, 6),
None,
flags=cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_FILTER_QUADS | cv2.CALIB_CB_NORMALIZE_IMAGE,
)
if not ret:
rospy.logerr("Could not find chessboard corners")
return False
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.001)
cv2.cornerSubPix(cv_img, corners, (11, 11), (-1, -1), criteria)
corners = corners.reshape(1, -1, 2)[0]
points = []
ppoints = []
ppp = PoseArray()
ppp.header.stamp = rospy.Time.now()
ppp.header.frame_id = self.world_frame
for c in corners:
pt = list(model.projectPixelTo3dRay((c[0], c[1])))
pt[:] = [x / pt[2] for x in pt]
# depth image is noisy - let's make mean of few pixels
da = []
for x in range(int(c[0]) - 2, int(c[0]) + 3):
for y in range(int(c[1]) - 2, int(c[1]) + 3):
da.append(cv_depth[y, x] / 1000.0)
d = np.mean(da)
pt[:] = [x * d for x in pt]
ps = PointStamped()
ps.header.stamp = rospy.Time(0)
ps.header.frame_id = image.header.frame_id
ps.point.x = pt[0]
ps.point.y = pt[1]
ps.point.z = pt[2]
# transform 3D point from camera into the world coordinates
try:
ps = self.tfl.transformPoint(self.world_frame, ps)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logerr("can't get transform")
return False
pp = Pose()
pp.position.x = ps.point.x
pp.position.y = ps.point.y
pp.position.z = ps.point.z
pp.orientation.x = 0
pp.orientation.y = 0
pp.orientation.z = 0
pp.orientation.w = 1.0
ppp.poses.append(pp)
# store x,y -> here we assume that points are 2D (on tabletop)
points.append([self.rpm * ps.point.x, self.rpm * ps.point.y])
self.corners_pub.publish(ppp)
dx = (self.pix_label.width() - self.pix_label.pixmap().width()) / 2.0
dy = (self.pix_label.height() - self.pix_label.pixmap().height()) / 2.0
box_size = self.pix_label.pixmap().width() / 12.0
# TODO self.scene_origin ???
# generate requested table coordinates
for y in range(0, 6):
for x in range(0, 9):
px = 2 * box_size + x * box_size + dx
py = 2 * box_size + y * box_size + dy
ppoints.append([px, py])
h, status = cv2.findHomography(np.array(points), np.array(ppoints), cv2.LMEDS)
self.h_matrix = np.matrix(h)
# self.h_matrix = np.matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1.0]])
# store homography matrix to parameter server
s = str(self.h_matrix.tolist())
rospy.set_param("/art/interface/projected_gui/projector/" + self.proj_id + "/calibration_matrix", s)
print s
return True
class color_controller():
def __init__(self, publish_tf):
# To take our Ros Image into a cv message and subsequently a numpy array.
self.bridge = CvBridge()
# To make the pixel to vector projection
self.cam_model = PinholeCameraModel()
# We need CameraInfo in order to use PinholeCameraModel below.
rospy.Subscriber('camera_topic', CameraInfo, self.camera_callback)
self.hasCameraInfo = False
while not self.hasCameraInfo:
print 'waiting on camera info.'
rospy.sleep(0.5)
# We are using a depth image to get depth information of what we're tracking.
rospy.Subscriber('depth_image', Image, self.depth_callback)
# This package is just an extension of cmvision to provide tf tracking of the blobs provided by cmvision.
rospy.Subscriber('blobs', Blobs, self.blob_callback)
# Subscribe to image for debugging.
# rospy.Subscriber('thing', Image, self.image_callback)
self.listener = tf.TransformListener()
self.broadcaster = tf.TransformBroadcaster()
# Republish each blob as part of a blob.
self.blob_pub = rospy.Publisher('/blobs_3d', Blobs3d, queue_size=5)
self.publish_tf = publish_tf
# blobs is received from running cmvision. It's color blobs as defined by our color file.
def blob_callback(self, blobs):
# array of our color_models.
self.colors = {}
blobs3d = Blobs3d()
for blob in blobs.blobs:
# If we have multiple catches of a single color, we only want to publish the one with the maximum area. Fortunately they are sorted as such.
already_published = False
if blob.name in self.colors:
already_published = True
self.colors[blob.name] = color_model(blob, self.camera_info, self.parent_frame, self.depth_image, self.cam_model, self.listener, self.broadcaster)
# Make sure this blob isn't shitty.
if self.colors[blob.name].validate():
# Publish to tf if master wishes it upon Dobby the programming elf.
if self.publish_tf and not already_published:
self.colors[blob.name].publish()
# 3d blobs that are in this blobs3d list are published to the /blobs_3d topic.
blobs3d.blobs.append(self.colors[blob.name].toBlob3d())
blobs3d.header.frame_id = self.parent_frame
blobs3d.header.stamp = rospy.Time.now()
blobs3d.blob_count = len(blobs3d.blobs)
self.blob_pub.publish(blobs3d)
def depth_callback(self, image):
image_cv = self.bridge.imgmsg_to_cv2(image, image.encoding)
image_cv2 = np.array(image_cv, dtype=np.float32)
self.depth_image = image_cv2
def camera_callback(self, camera_info):
if not self.hasCameraInfo:
self.cam_model.fromCameraInfo(camera_info)
self.camera_info = camera_info
self.parent_frame = self.camera_info.header.frame_id
self.hasCameraInfo = True
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)
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 MarkerFinder():
def __init__(self):
self.tf_listener = tf.TransformListener()
self.search = False
self.last_image = None
self.last_image_timestamp = None
self.last_draw_image = None
# This may need to be changed if you want to use a different image feed.
self.image_sub = sub8_ros_tools.Image_Subscriber('/down/left/image_raw', self.image_cb)
self.image_pub = sub8_ros_tools.Image_Publisher("vision/channel_marker/target_info")
self.toggle = rospy.Service('vision/channel_marker/search', SetBool, self.toggle_search)
self.cam = PinholeCameraModel()
self.cam.fromCameraInfo(self.image_sub.wait_for_camera_info())
self.rviz = rviz.RvizVisualizer()
# self.occ_grid = MarkerOccGrid(self.image_sub, grid_res=.05, grid_width=500, grid_height=500,
# grid_starting_pose=Pose2D(x=250, y=250, theta=0))
if rospy.get_param("/orange_markers/use_boost"):
path = os.path.join(rospack.get_path('sub8_perception'),
'ml_classifiers/marker/' + rospy.get_param("/orange_markers/marker_classifier"))
self.boost = cv2.Boost()
rospy.loginfo("MARKER - Loading classifier from {}".format(path))
self.boost.load(path)
rospy.loginfo("MARKER - Classifier for marker loaded.")
else:
self.lower = np.array(rospy.get_param('/color/channel_marker/hsv_low'))
self.upper = np.array(rospy.get_param('/color/channel_marker/hsv_high'))
self.pose_service = rospy.Service("vision/channel_marker/pose", VisionRequest, self.request_marker)
self.kernel = np.ones((15, 15), np.uint8)
# Occasional status publisher
self.timer = rospy.Timer(rospy.Duration(1), self.publish_target_info)
print "MARKER - Got no patience for sittin' around!"
def toggle_search(self, srv):
if srv.data:
rospy.loginfo("MARKER - Looking for markers now.")
self.search = True
else:
rospy.loginfo("MARKER - Done looking for markers.")
self.search = False
return SetBoolResponse(success=srv.data)
def publish_target_info(self, *args):
if not self.search or self.last_image is None:
return
markers = self.find_marker(np.copy(self.last_image))
#self.occ_grid.update_grid(self.last_image_timestamp)
#self.occ_grid.add_marker(markers, self.last_image_timestamp)
if self.last_draw_image is not None: #and (markers is not None):
self.image_pub.publish(np.uint8(self.last_draw_image))
def image_cb(self, image):
'''Hang on to last image and when it was taken.'''
self.last_image = image
self.last_image_timestamp = self.image_sub.last_image_time
def calculate_threshold(self, img, agression=.5):
histr = cv2.calcHist([img], [0], None, [179], [0, 179])
threshold = np.uint8((179 - np.argmax(histr)) * agression)
return threshold
def get_2d_pose(self, mask):
# estimate covariance matrix and get corresponding eigenvectors
wh = np.where(mask)[::-1]
cov = np.cov(wh)
eig_vals, eig_vects = np.linalg.eig(cov)
# use index of max eigenvalue to find max eigenvector
i = np.argmax(eig_vals)
max_eigv = eig_vects[:, i] * np.sqrt(eig_vals[i])
# flip indices to find min eigenvector
min_eigv = eig_vects[:, 1 - i] * np.sqrt(eig_vals[1 - i])
# define center of pipe
center = np.average(wh, axis=1)
# define vertical vector (sub's current direction)
vert_vect = np.array([0.0, -1.0])
if max_eigv[1] > 0:
max_eigv = -max_eigv
min_eigv = -min_eigv
num = np.cross(max_eigv, vert_vect)
denom = np.linalg.norm(max_eigv) * np.linalg.norm(vert_vect)
angle_rad = np.arcsin(num / denom)
return center, angle_rad, [max_eigv, min_eigv]
def find_marker(self, img):
#img[:, -100:] = 0
img = cv2.GaussianBlur(img, (7, 7), 15)
last_image_timestamp = self.last_image_timestamp
if rospy.get_param("/orange_markers/use_boost"):
some_observations = machine_learning.boost.observe(img)
prediction = [int(x) for x in [self.boost.predict(obs) for obs in some_observations]]
mask = np.reshape(prediction, img[:, :, 2].shape).astype(np.uint8) * 255
else:
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, self.lower, self.upper)
kernel = np.ones((5,5),np.uint8)
mask = cv2.dilate(mask, kernel, iterations = 1)
mask = cv2.erode(mask, kernel, iterations = 2)
#mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, self.kernel)
contours, _ = cv2.findContours(np.copy(mask), cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
if len(contours) < 1:
rospy.logwarn("MARKER - No marker found.")
return None
# Find biggest area contour
self.last_draw_image = np.dstack([mask] * 3)
areas = [cv2.contourArea(c) for c in contours]
max_index = np.argmax(areas)
cnt = contours[max_index]
# Draw a miniAreaRect around the contour and find the area of that.
rect = cv2.minAreaRect(cnt)
box = cv2.cv.BoxPoints(rect)
box = np.int0(box)
mask = np.zeros(shape=mask.shape)
cv2.drawContours(self.last_draw_image, [box], 0, (0, 128, 255), 2)
cv2.drawContours(mask, [box], 0, 255, -1)
rect_area = cv2.contourArea(box)
center, angle_rad, [max_eigv, min_eigv] = self.get_2d_pose(mask)
cv2.line(self.last_draw_image, tuple(np.int0(center)), tuple(np.int0(center + (2 * max_eigv))), (0, 255, 30), 2)
cv2.line(self.last_draw_image, tuple(np.int0(center)), tuple(np.int0(center + (2 * min_eigv))), (0, 30, 255), 2)
# Check if the box is too big or small.
xy_position, height = self.get_tf(timestamp=last_image_timestamp)
expected_area = self.calculate_marker_area(height)
print "{} < {} < {} ({})".format(expected_area * .2, rect_area, expected_area * 2, expected_area)
if expected_area * .5 < rect_area < expected_area * 2:
#cv2.drawContours(self.last_draw_image, [box], 0, (255, 255, 255), -1)
self.rviz.draw_ray_3d(center, self.cam, np.array([1, .5, 0, 1]), frame='/downward',
_id=5, length=height, timestamp=last_image_timestamp)
else:
angle_rad = 0
max_eigv = np.array([0, -20])
min_eigv = np.array([-20, 0])
#cv2.drawContours(self.last_draw_image, [box], 0, (255, 0, 30), -1)
rospy.logwarn("MARKER - Size out of bounds!")
self.rviz.draw_ray_3d(center, self.cam, np.array([1, .5, 0, 1]), frame='/downward',
_id=5, length=height, timestamp=last_image_timestamp)
# Convert to a 3d pose to move the sub to.
abs_position = self.transform_px_to_m(center, last_image_timestamp)
q_rot = tf.transformations.quaternion_from_euler(0, 0, angle_rad)
return numpy_quat_pair_to_pose(abs_position, q_rot)
def request_marker(self, data):
if self.last_image is None:
return False # Fail if we have no images cached
timestamp = self.last_image_timestamp
goal_pose = self.find_marker(self.last_image)
found = (goal_pose is not None)
if not found:
resp = VisionRequestResponse(
found=found
)
else:
resp = VisionRequestResponse(
pose=PoseStamped(
header=Header(
stamp=timestamp,
frame_id='/down_camera'),
pose=goal_pose
),
found=found
)
return resp
def get_tf(self, timestamp=None, get_rotation=False):
'''
x_y position, height in meters and quat rotation of the sub if requested
'''
if timestamp is None:
timestamp = rospy.Time()
self.tf_listener.waitForTransform("/map", "/downward", timestamp, rospy.Duration(5.0))
trans, rot = self.tf_listener.lookupTransform("/map", "/downward", timestamp)
x_y_position = trans[:2]
self.tf_listener.waitForTransform("/ground", "/downward", timestamp, rospy.Duration(5.0))
trans, _ = self.tf_listener.lookupTransform("/ground", "/downward", timestamp)
height = np.nan_to_num(trans[2])
x_y_position = np.nan_to_num(x_y_position)
if get_rotation:
return x_y_position, rot, height
return x_y_position, height
def transform_px_to_m(self, m_position, timestamp):
'''
Finds the absolute position of the marker in meters.
'''
xy_position, q, height = self.get_tf(timestamp, get_rotation=True)
dir_vector = unit_vector(np.array([self.cam.cx(), self.cam.cy()]) - m_position)
cam_rotation = tf.transformations.euler_from_quaternion(q)[2] + np.pi / 2
print "MARKER - dir_vector:", dir_vector
print "MARKER - cam_rotation:", cam_rotation
dir_vector = np.dot(dir_vector, make_2D_rotation(cam_rotation))
# Calculate distance from middle of frame to marker in meters.
magnitude = self.calculate_visual_radius(height, second_point=m_position)
abs_position = np.append(xy_position + dir_vector[::-1] * magnitude, -SEARCH_DEPTH)
return abs_position
def calculate_visual_radius(self, height, second_point=None):
'''
Draws rays to find the radius of the FOV of the camera in meters.
It also can work to find the distance between two planar points some distance from the camera.
'''
mid_ray = np.array([0, 0, 1])
if second_point is None:
if self.cam.cy() < self.cam.cx():
second_point = np.array([self.cam.cx(), 0])
else:
second_point = np.array([0, self.cam.cy()])
edge_ray = unit_vector(self.cam.projectPixelTo3dRay(second_point))
# Calculate angle between vectors and use that to find r
theta = np.arccos(np.dot(mid_ray, edge_ray))
return np.tan(theta) * height
def calculate_marker_area(self, height):
'''
What we really don't want is to find markers that are on the edge since the direction and center of
the marker are off.
'''
MARKER_LENGTH = 1.22 # m
MARKER_WIDTH = .1524 # m
# Get m/px on the ground floor.
m = self.calculate_visual_radius(height)
if self.cam.cy() < self.cam.cx():
px = self.cam.cy()
else:
px = self.cam.cx()
m_px = m / px
marker_area_m = MARKER_WIDTH * MARKER_LENGTH
marker_area_px = marker_area_m / (m_px ** 2)
return marker_area_px
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))
class ceilingLocalizer():
def __init__(self, image_topic, camera_topic, robot_depth, robot_frame, pattern):
# A subscriber to the image topic
rospy.Subscriber(image_topic, Image, self.image_callback)
# To get which frame is the camera frame
rospy.Subscriber(camera_topic, CameraInfo, self.camera_callback)
# To make the pixel to vector projection
self.camModel = PinholeCameraModel()
# How far is the robot plane from the camera?
self.robot_depth = robot_depth
# What is the name of the frame we should publish?
self.robot_frame = robot_frame
# Pattern to match
self.image_orig = cv2.imread(pattern, 1)
image_bin = cv2.cvtColor(self.image_orig, cv2.COLOR_BGR2GRAY)
ret, image_bin = cv2.threshold(image_bin, 127,255,cv2.THRESH_BINARY_INV)
contours, hierarchy = cv2.findContours(image_bin, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
sorted_contours = sorted(contours, key = cv2.contourArea, reverse = True)
# Our pattern is the second contour because for some reason its finding the edges of our image as a contour
self.pattern = sorted_contours[1]
self.pattern_area = 200
# To convert images from ROS to CV formats
self.bridge = CvBridge()
# Publish frames to tf
self.broadcaster = tf.TransformBroadcaster()
# Publisher for edited video
self.video = rospy.Publisher('ceilingMarker', Image)
def camera_callback(self, cameraInfo):
self.cameraInfo = cameraInfo
self.camModel.fromCameraInfo(cameraInfo)
self.cameraFrame = cameraInfo.header.frame_id
def image_callback(self, image_in):
image_cv = self.bridge.imgmsg_to_cv(image_in, 'bgr8')
image_cv2 = numpy.array(image_cv, dtype=numpy.uint8)
#image_cv2 = cv2.GaussianBlur(image_cv2,(5,5), 100, 100)
# Convert the image from BGR to HSV
image_hsv_cv2 = cv2.cvtColor(image_cv2, cv2.COLOR_BGR2HSV)
# Convert the image to grayscale
image_gray = cv2.cvtColor(image_cv2, cv2.COLOR_BGR2GRAY)
# Threshold grayscale image
flag, thresh = cv2.threshold(image_gray, 230, 255, cv2.THRESH_BINARY)
# Make a copy for contour detection
thresh_copy = numpy.copy(thresh)
contours, hierarchy = cv2.findContours(thresh_copy, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
# Sort contours by how much they match our pattern
potential_markers = self.find_potential_markers(contours)
# Ranges in hsv
lower_red_hsv = numpy.array([0,50,50])
upper_red_hsv = numpy.array([10,255,255])
lower_blue_hsv = numpy.array([105,50,50])
upper_blue_hsv = numpy.array([120,255,255])
lower_red_hsv2 = numpy.array([160,50,50])
upper_red_hsv2 = numpy.array([179,255,255])
# Make masks from our ranges
mask_red_hsv = numpy.bitwise_or(cv2.inRange(image_hsv_cv2, lower_red_hsv, upper_red_hsv), cv2.inRange(image_hsv_cv2, lower_red_hsv2, upper_red_hsv2))
mask_blue_hsv = cv2.inRange(image_hsv_cv2, lower_blue_hsv, upper_blue_hsv)
# Front should be red, rear should be blue
front_mask = mask_red_hsv
rear_mask = mask_blue_hsv
if len(potential_markers) >= 2:
# Set one marker as the front, the other the rear
if self.isFrontDominant(potential_markers[0], front_mask, rear_mask):
front_marker = potential_markers[0]
rear_marker = potential_markers[1]
else
front_marker = potential_markers[1]
rear_marker = potential_markers[0]
# Find the centerpoints
[frontx,fronty] = self.find_center(front_marker)
[backx,backy] = self.find_center(rear_marker)
# Find the midpoint
midx = (frontx+backx)/2
midy = (fronty+backy)/2
[vx,vy,vz] = self.camModel.projectPixelTo3dRay((midx,midy))
marker_depth = self.robot_depth / vz
# Position of the robot relative to ceiling kinect
x = marker_depth * vx
y = marker_depth * vy
z = self.robot_depth
# Orientation of robot
yaw = math.atan2((fronty-backy), (frontx-backx))
quaternion = tf.transformations.quaternion_from_euler(0.0,0.0,yaw, axes='sxyz')
# Broadcast the frame to tf
self.broadcaster.sendTransform((x,y,z), quaternion, rospy.Time.now(), self.robot_frame, self.cameraFrame)
self.draw_orientation(image_cv2, (frontx, fronty), (rearx, reary))
else:
rospy.loginfo("No marker found")
cv2.imshow('image', image_cv2)
cv2.imshow('image_gray', image_gray)
cv2.imshow('pattern', self.image_orig)
cv2.imshow('image_corners', image_corners)
cv2.imshow('thresh', thresh)
cv2.waitKey(3)
# Convert back to ROS Image msg
image_cv = cv.fromarray(image_cv2)
image_out = self.bridge.cv_to_imgmsg(image_cv, 'bgr8')
self.video.publish(image_out)
def isFrontDominant(self, contour, front_mask, rear_mask):
# Is the contour more red or more blue?
x,y,w,h = cv2.boundingRect(contour)
x1 = x - w/2
y1 = y - h/2
x2 = x + w/2
y2 = y + h/2
cropped_front = front_mask[y1:y2, x1:x2]
cropped_rear = rear_mask[y1:y2, x1:x2]
front_ratio = numpy.count_nonzero(cropped_front)/cropped_front.size
rear_ratio = numpy.count_nonzero(cropped_rear)/cropped_rear.size
if front_ratio >= rear_ratio:
return True
else
return False
def find_potential_markers(self, contours):
# Get rid of small contours and order by match strength
potential_markers = []
for contour in contours:
if cv2.contourArea(contour) > self.pattern_area:
potential_markers.append(contour)
sorted_markers = sorted(potential_markers, key=self.match_pattern)
return sorted_markers
def match_pattern(self, contour):
# Match the contour against our pattern
match = cv2.matchShapes(contour,self.pattern, 1, 0.0)
return match
def draw_orientation(self, image, front, back):
# Draw a line across the whole image for debug
slope = float(front[1] - back[1]) / float(front[0] - back[0])
leftmosty = int((-front[0]*slope) + front[1])
rightmosty = int(((image.shape[1]-front[0])*slope)+front[1])
cv2.line(image, (image.shape[1]-1, rightmosty), (0, leftmosty), (0,255,0), 3)
def find_center(self, contour):
moments = cv2.moments(contour, True)
x = int(moments['m10']/moments['m00'])
y = int(moments['m01']/moments['m00'])
return x,y
class FaceDetector():
""" Measures face location, compiles important info. """
def __init__(self, info_path, image_topic):
# Create depth model for Asus Xtion camera
bag = rosbag.Bag(info_path)
infos = bag.read_messages(topics='/camera/rgb/camera_info')
info = infos.next()[1]
self.model = PinholeCameraModel()
self.model.fromCameraInfo(info)
# Ready the face detector
self.classifier = cv2.CascadeClassifier(os.environ['ROS_ROOT'] + '/../OpenCV/haarcascades/haarcascade_frontalface_alt.xml')
self.have_face = False
# Subscribe to images
self.have_image = False
self.have_t_ref = False
self.bridge = CvBridge()
self.sub = rospy.Subscriber(image_topic, Image, self.handle_image)
def handle_image(self, imgmsg):
if not self.have_t_ref:
self.t_ref = imgmsg.header.stamp.to_sec()
self.t_old = 0.0
self.have_t_ref = True
t_new = imgmsg.header.stamp.to_sec() - self.t_ref
dt = t_new - self.t_old
self.t_old = t_new
image = self.bridge.imgmsg_to_cv2(imgmsg)
if not self.have_image:
self.have_image = True
success = self.detect_face(image, dt)
def detect_face(self, image, dt):
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
faces = self.classifier.detectMultiScale(gray, 1.15, 1)
if len(faces) > 0:
(x,y,w,h) = faces[0]
self.face = [x, y, w, h]
self.face_size = w
cv2.rectangle(image, (x,y), (x+w,y+h), (0,255,0), 1) # plot detected face
if not self.have_face:
self.have_face = True
else:
if self.have_face:
x,y,w,h = self.expand_filter(1, dt)
cv2.rectangle(image, (x,y), (x+w,y+h), (255,0,255), 3) # plot expanded filter area
self.face = [x, y, w, h]
cv2.imshow('Image with Face', image)
cv2.waitKey(1)
def expand_filter(self, ci, dt):
""" Input: confidence interval N as in N*SD or N-sigma. """
(x, y, w, h) = self.face # break it down
mean_face = 0.1875 # m
mean_speed = 1.626 # m/s
sd_speed = 0.201 # m/s
meters_to_pixels = self.face_size / mean_face
print meters_to_pixels
mean_speed *= meters_to_pixels # convert
sd_speed *= meters_to_pixels # convert
expansion = (mean_speed + ci * sd_speed) * dt
print mean_speed, sd_speed
print dt, expansion
print ''
x -= expansion
y -= expansion
w += 2*expansion
h += 2*expansion
if x < 0:
x = 0
if y < 0:
y = 0
if x + w > 640:
w = 640 - x
if y + h > 480:
h = 480 - y
face = [x,y,w,h]
face = [int(el) for el in face] # integer-ize
return face
