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