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 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 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 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
class OrangeRectangleFinder(): """ Node which finds orange rectangular objects in image frame. This can be used for the path marker challenge and to detect the lid of the bins challenge. The node estimates the 2d and 3d position/orientation of this object and returns this estimate when service is called. Unit tests for this node is in test_path_marker.py Finding the marker works as follows: * blur image * threshold image mostly for highly saturated, orange/yellow/red objects * run canny edge detection on thresholded image * find contours on edge frame * filter contours to find those that may be contours by: * checking # of sides in appox polygon * checking ratio of length/width close to known model * estimates 3D pose using cv2.solvePnP with known object demensions and camera model * Use translation vector from PnP and direction vector from 2d contour for pose * Transform this frames pose into /map frame * Plug this frames pose in /map into a kalman filter to reduce noise TODO: Allow for two objects to be identifed at once, both filtered through its own KF """ # Coordinate axes for debugging image REFERENCE_POINTS = np.array([[0, 0, 0], [0.3, 0, 0], [0, 0.3, 0], [0, 0, 0.3]], dtype=np.float) def __init__(self): self.debug_gui = False self.enabled = False self.cam = None # Constants from launch config file self.debug_ros = rospy.get_param("~debug_ros", True) self.canny_low = rospy.get_param("~canny_low", 100) self.canny_ratio = rospy.get_param("~canny_ratio", 3.0) self.thresh_hue_high = rospy.get_param("~thresh_hue_high", 60) self.thresh_saturation_low = rospy.get_param("~thresh_satuation_low", 100) self.min_contour_area = rospy.get_param("~min_contour_area", 100) self.epsilon_range = rospy.get_param("~epsilon_range", (0.01, 0.1)) self.epsilon_step = rospy.get_param("~epsilon_step", 0.01) self.shape_match_thresh = rospy.get_param("~shape_match_thresh", 0.4) self.min_found_count = rospy.get_param("~min_found_count", 10) self.timeout_seconds = rospy.get_param("~timeout_seconds", 2.0) # Default to scale model of path marker. Please use set_geometry service # to set to correct model of object. length = rospy.get_param("~length", 1.2192) width = rospy.get_param("~width", 0.1524) self.rect_model = RectFinder(length, width) self.do_3D = rospy.get_param("~do_3D", True) camera = rospy.get_param("~image_topic", "/camera/down/left/image_rect_color") self.tf_listener = tf.TransformListener() # Create kalman filter to track 3d position and direction vector for marker in /map frame self.state_size = 5 # X, Y, Z, DY, DX self.filter = cv2.KalmanFilter(self.state_size, self.state_size) self.filter.transitionMatrix = 1. * np.eye(self.state_size, dtype=np.float32) self.filter.measurementMatrix = 1. * np.eye(self.state_size, dtype=np.float32) self.filter.processNoiseCov = 1e-5 * np.eye(self.state_size, dtype=np.float32) self.filter.measurementNoiseCov = 1e-4 * np.eye(self.state_size, dtype=np.float32) self.filter.errorCovPost = 1. * np.eye(self.state_size, dtype=np.float32) self.reset() self.service_set_geometry = rospy.Service('~set_geometry', SetGeometry, self._set_geometry_cb) if self.debug_ros: self.debug_pub = Image_Publisher("~debug_image") self.markerPub = rospy.Publisher('~marker', Marker, queue_size=10) self.service2D = rospy.Service('~2D', VisionRequest2D, self._vision_cb_2D) if self.do_3D: self.service3D = rospy.Service('~pose', VisionRequest, self._vision_cb_3D) self.toggle = rospy.Service('~enable', SetBool, self._enable_cb) self.image_sub = Image_Subscriber(camera, self._img_cb) self.camera_info = self.image_sub.wait_for_camera_info() assert self.camera_info is not None self.cam = PinholeCameraModel() self.cam.fromCameraInfo(self.camera_info) def _set_geometry_cb(self, req): self.rect_model = RectFinder.from_polygon(req.model) self.reset() rospy.loginfo("Resetting rectangle model to LENGTH=%f, WIDTH=%f", self.rect_model.length, self.rect_model.width) return {'success': True} def _send_debug_marker(self): ''' Sends a rviz marker in the camera frame with the estimated pose of the object. This marker is a scaled cube with the demensions of the model. Only called if debug_ros param == True ''' if self.last3d is None or not self.found: return m = Marker() m.header.frame_id = '/map' m.header.stamp = self.last_found_time_3D m.ns = "orange_rectangle" m.id = 0 m.type = 1 m.action = 0 # Real demensions of path marker m.scale.x = self.rect_model.length m.scale.y = self.rect_model.width m.scale.z = 0.05 m.pose.position = numpy_to_point(self.last3d[0]) m.pose.orientation = numpy_to_quaternion(self.last3d[1]) m.color.r = 0.0 m.color.g = 0.5 m.color.b = 0.0 m.color.r = 1.0 m.color.a = 1.0 m.lifetime = rospy.Duration(self.timeout_seconds) self.markerPub.publish(m) def _enable_cb(self, x): if x.data != self.enabled: self.reset() self.tf_listener.clear() self.enabled = x.data return SetBoolResponse(success=True) def _vision_cb_3D(self, req): res = VisionRequestResponse() if self.last_found_time_3D is None or self.image_sub.last_image_time is None: res.found = False return res dt = (self.image_sub.last_image_time - self.last_found_time_3D).to_sec() if dt < 0 or dt > self.timeout_seconds: res.found = False elif (self.last3d is None or not self.enabled): res.found = False else: res.pose.header.frame_id = "/map" res.pose.header.stamp = self.last_found_time_3D res.pose.pose.position = numpy_to_point(self.last3d[0]) res.pose.pose.orientation = numpy_to_quaternion(self.last3d[1]) res.found = True return res def _vision_cb_2D(self, req): res = VisionRequest2DResponse() if (self.last2d is None or not self.enabled): res.found = False else: res.header.frame_id = self.cam.tfFrame() res.header.stamp = self.last_found_time_2D res.pose.x = self.last2d[0][0] res.pose.y = self.last2d[0][1] res.camera_info = self.camera_info res.max_x = self.camera_info.width res.max_y = self.camera_info.height if self.last2d[1][0] < 0: self.last2d[1][0] = -self.last2d[1][0] self.last2d[1][1] = -self.last2d[1][1] angle = np.arctan2(self.last2d[1][1], self.last2d[1][0]) res.pose.theta = angle res.found = True return res def reset(self): self.last_found_time_2D = None self.last_found_time_3D = None self.last2d = None self.last3d = None self._clear_filter(None) def _clear_filter(self, state): ''' Reset filter and found state. This will ensure that the object is seen consistently before vision request returns true ''' rospy.loginfo("MARKER LOST") self.found_count = 0 self.found = False self.last3d = None self.filter.errorCovPre = 1. * np.eye(self.state_size, dtype=np.float32) if state is not None: self.found_count = 1 state = np.array(state, dtype=np.float32) self.filter.statePost = state def _update_kf(self, (x, y, z, dy, dx)): ''' Updates the kalman filter using the pose estimation from the most recent frame. Also tracks time since last seen and how often is has been seen to set the boolean "found" for the vision request ''' if self.last_found_time_3D is None: # First time found, set initial KF pose to this frame self._clear_filter((x, y, z, dy, dx)) self.last_found_time_3D = self.image_sub.last_image_time return dt = (self.image_sub.last_image_time - self.last_found_time_3D).to_sec() self.last_found_time_3D = self.image_sub.last_image_time if dt < 0 or dt > self.timeout_seconds: rospy.logwarn("Timed out since last saw marker, resetting. DT={}".format(dt)) self._clear_filter((x, y, z, dy, dx)) return self.found_count += 1 measurement = 1. * np.array([x, y, z, dy, dx], dtype=np.float32) self.filter.predict() estimated = self.filter.correct(measurement) if self.found_count > self.min_found_count: angle = np.arctan2(estimated[3], estimated[4]) self.last3d = ((estimated[0], estimated[1], estimated[2]), quaternion_from_euler(0.0, 0.0, angle)) if not self.found: rospy.loginfo("Marker Found") self.found = True
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 CameraHelper: """ A helper class to take pictures with the Sawyer camera and unproject points from the images """ def __init__(self, camera_name, base_frame, table_height): """ Initialize the instance :param camera_name: The camera name. One of (head_camera, right_hand_camera) :param base_frame: The frame for the robot base :param table_height: The table height with respect to base_frame """ self.camera_name = camera_name self.base_frame = base_frame self.table_height = table_height self.image_queue = Queue.Queue() self.pinhole_camera_model = PinholeCameraModel() self.tf_listener = tf.TransformListener() camera_info_topic = "/io/internal_camera/{}/camera_info".format( camera_name) camera_info = rospy.wait_for_message(camera_info_topic, CameraInfo) self.pinhole_camera_model.fromCameraInfo(camera_info) cameras = intera_interface.Cameras() cameras.set_callback(camera_name, self.__show_image_callback, rectify_image=True) def __show_image_callback(self, img_data): """ Image callback for the camera :param img_data: The image received from the camera """ if self.image_queue.empty(): self.image_queue.put(img_data) def take_single_picture(self): """ Take one picture from the specified camera :returns: The image """ with self.image_queue.mutex: self.image_queue.queue.clear() cameras = intera_interface.Cameras() cameras.start_streaming(self.camera_name) image_data = self.image_queue.get(block=True, timeout=None) cameras.stop_streaming(self.camera_name) return image_data def project_point_on_table(self, point): """ Projects the 2D point from the camera image on the table :param point: The 2D point in the form (x, y) :return: The 3D point in the coordinate space of the frame that was specified when the object was initialized """ # Get camera frame name camera_frame = self.pinhole_camera_model.tfFrame() # Check that both frames exist if self.tf_listener.frameExists( self.base_frame) and self.tf_listener.frameExists( camera_frame): # Get transformation time = self.tf_listener.getLatestCommonTime( self.base_frame, camera_frame) camera_translation_base, camera_orientation_base = self.tf_listener.lookupTransform( self.base_frame, camera_frame, time) # Unproject point unprojected_ray_camera = self.pinhole_camera_model.projectPixelTo3dRay( point) # Rotate ray based on the frame transformation camera_frame_rotation_matrix = tf.transformations.quaternion_matrix( camera_orientation_base) unprojected_ray_base = numpy.dot( camera_frame_rotation_matrix[:3, :3], unprojected_ray_camera) # Intersect ray with base plane point_height = camera_translation_base[2] - self.table_height factor = -point_height / unprojected_ray_base[2] intersection = camera_translation_base + unprojected_ray_base * factor return intersection
class ros_cv_testing_node: def __init__(self): # Get information for this particular camera camera_info = get_single('head_camera/depth_registered/camera_info', CameraInfo) print camera_info # Set information for the camera self.cam_model = PinholeCameraModel() self.cam_model.fromCameraInfo(camera_info) # Subscribe to the camera's image topic and depth image topic self.rgb_image_sub = rospy.Subscriber("head_camera/rgb/image_raw", Image, self.on_rgb) self.depth_image_sub = rospy.Subscriber( "head_camera/depth_registered/image_raw", Image, self.on_depth) # Publish where the button is in the base link frame self.point_pub = rospy.Publisher('button_point', PointStamped, queue_size=10) # Set up connection to CvBridge self.bridge = CvBridge() # Open up viewing windows cv2.namedWindow('depth') cv2.namedWindow('rgb') # Set up the class variables self.rgb_image = None self.rgb_image_time = None self.depth_image = None self.center = None self.return_point = PointStamped() self.tf_listener = TransformListener() def on_rgb(self, image): # Convert image to something that cv2 can work with try: self.rgb_image = self.bridge.imgmsg_to_cv2(image, 'bgr8') except e: print e return self.rgb_image_time = image.header.stamp # Get height and width of image h = self.rgb_image.shape[0] w = self.rgb_image.shape[1] # Create empty image color_dst = numpy.empty((h, w), 'uint8') # Convert picture to grayscale cv2.cvtColor(self.rgb_image, cv2.COLOR_RGB2GRAY, color_dst) # Find circles given the camera image dp = 1.1 minDist = 90 circles = cv2.HoughCircles(color_dst, cv2.cv.CV_HOUGH_GRADIENT, dp, minDist) # If no circles are detected then exit the program if circles == None: print "No circles found using these parameters" sys.exit() circles = numpy.uint16(numpy.around(circles)) # Draw the center of the circle closest to the top ycoord = [] for i in circles[0, :]: ycoord.append(i[1]) # Find the top circle in the frame top_circ_y_coor = min(ycoord) x_coor = 0 y_coor = 0 for i in circles[0, :]: if i[1] == top_circ_y_coor: # draw the center of the circle #cv2.circle(self.rgb_image,(i[0],i[1]),2,(0,0,255),3) # draw outer circle #cv2.circle(self.rgb_image,(i[0],i[1]),i[2],(0,255,0),2) x_coor = i[0] y_coor = i[1] cv2.circle(self.rgb_image, (327, 247), 2, (0, 0, 255), 3) # Set the coordinates for the center of the circle self.center = (x_coor, y_coor) #self.center = (328,248) cv2.imshow('rgb', self.rgb_image) cv2.waitKey(1) def on_depth(self, image): # Use numpy so that cv2 can use image self.depth_image = numpy.fromstring(image.data, dtype='float32').reshape( (480, 640)) nmask = numpy.isnan(self.depth_image) #Find the minimum and the maximum of the depth image Dmin = self.depth_image[~nmask].min() Dmax = self.depth_image[~nmask].max() # If a circle has been found find its depth and apply that to the ray if self.center != None: ray = self.cam_model.projectPixelTo3dRay(self.center) depth = self.depth_image[self.center[1]][self.center[0]] # If the depth is not a number exit if math.isnan(depth): print "Incomplete information on depth at this point" return # Otherwise mulitply the depth by the unit vector of the ray else: print "depth", depth cam_coor = [depth * ray[0], depth * ray[1], depth * ray[2]] #print "camera frame coordinate:", cam_coor else: return # Rescale to [0,1] cv2.imshow('depth', self.depth_image / 2.0) cv2.waitKey(1) # Only use depth if the RGB image is running if self.rgb_image_time is None: rospy.loginfo('not using depth cause no RGB yet') return time_since_rgb = image.header.stamp - self.rgb_image_time # Only use depth if it is close in time to the RGB image if time_since_rgb > rospy.Duration(0.5): rospy.loginfo('not using depth because time since RGB is ' + str(time_since_rgb)) #return # Find transform from camera frame to world frame self.tf_listener.waitForTransform(self.cam_model.tfFrame(), "base_link", image.header.stamp, rospy.Duration(1.0)) # Set up the point to be published self.return_point.header.stamp = image.header.stamp self.return_point.header.frame_id = self.cam_model.tfFrame() self.return_point.point.x = cam_coor[0] self.return_point.point.y = cam_coor[1] self.return_point.point.z = cam_coor[2] # Transform point to the baselink frame self.return_point = self.tf_listener.transformPoint( "base_link", self.return_point) print "world frame coordinate:", self.return_point.point.x, \ self.return_point.point.y,self.return_point.point.z, "\n" self.point_pub.publish(self.return_point)
class CLAHE_generator: def __init__(self): self.camera = rospy.get_param('~camera_topic', '/camera/front/left/image_rect_color') # Instantiate remaining variables and objects self.last_image_time = None self.last_image = None self.tf_listener = tf.TransformListener() self.status = '' self.est = None self.visual_id = 0 self.enabled = False self.bridge = CvBridge() # Image Subscriber and Camera Information self.image_sub = Image_Subscriber(self.camera, self.image_cb) self.camera_info = self.image_sub.wait_for_camera_info() ''' These variables store the camera information required to perform the transformations on the coordinates to move from the subs perspective to our global map perspective. This information is also necessary to perform the least squares intersection which will find the 3D coordinates of the torpedo board based on 2D observations from the Camera. More info on this can be found in sub8_vision_tools. ''' self.camera_info = self.image_sub.wait_for_camera_info() self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(self.camera_info) self.frame_id = self.camera_model.tfFrame() self.image_pub = Image_Publisher("CLAHE/debug") # Debug self.debug = rospy.get_param('~debug', True) def image_cb(self, image): ''' Run each time an image comes in from ROS. If enabled, attempt to find the torpedo board. ''' self.last_image = image if self.last_image_time is not None and \ self.image_sub.last_image_time < self.last_image_time: # Clear tf buffer if time went backwards (nice for playing bags in # loop) self.tf_listener.clear() self.last_image_time = self.image_sub.last_image_time self.CLAHE(image) print('published') def CLAHE(self, cv_image): ''' CLAHE (Contrast Limited Adaptive Histogram Equalization) This increases the contrast between color channels and allows us to better differentiate colors under certain lighting conditions. ''' clahe = cv2.createCLAHE(clipLimit=9.5, tileGridSize=(4, 4)) # convert from BGR to LAB color space lab = cv2.cvtColor(cv_image, cv2.COLOR_BGR2LAB) l, a, b = cv2.split(lab) # split on 3 different channels l2 = clahe.apply(l) # apply CLAHE to the L-channel lab = cv2.merge((l2, a, b)) # merge channels cv_image = cv2.cvtColor(lab, cv2.COLOR_LAB2BGR) self.image_pub.publish(cv_image)
class Colorama(object): def __init__(self): info_topic = camera_root + "/camera_info" image_topic = camera_root + "/image_raw" # Change this to rect on boat self.tf_listener = tf.TransformListener() # Map id => [{color, error}, ...] for determining when a color is good self.colored = {} db_request = rospy.ServiceProxy("/database/requests", ObjectDBQuery) self.make_request = lambda **kwargs: db_request( ObjectDBQueryRequest(**kwargs)) rospy.Service("/vision/buoy_colorizer", VisionRequest, self.got_request) self.image_history = ImageHistory(image_topic) # Wait for camera info, and exit if not found try: fprint("Waiting for camera info on: '{}'".format(info_topic)) camera_info_msg = rospy.wait_for_message(info_topic, CameraInfo, timeout=3) except rospy.exceptions.ROSException: fprint("Camera info not found! Terminating.", msg_color="red") rospy.signal_shutdown("Camera not found!") return fprint("Camera info found!", msg_color="green") self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(camera_info_msg) self.debug = DebugImage("/colorama/debug", prd=.5) # These are color mappings from Hue values [0, 360] self.color_map = { 'red': np.radians(0), 'yellow': np.radians(60), 'green': np.radians(120), 'blue': np.radians(240) } # Some tunable parameters self.update_time = .5 # s self.saturation_reject = 50 self.value_reject = 50 self.hue_error_reject = .4 # rads # To decide when to accept color observations self.error_tolerance = .4 # rads self.spottings_req = 4 self.similarity_reject = .1 # If two colors are within this amount of error, reject rospy.Timer(ros_t(self.update_time), self.do_update) def valid_color(self, _id): """ Either returns valid color or None depending if the color is valid or not """ if _id not in self.colored: return None object_data = self.colored[_id] print "object_data", object_data # Maps color => [err1, err2, ..] potential_colors = {} for data in object_data: color, err = data['color'], data['err'] if color not in potential_colors: potential_colors[color] = [] potential_colors[color].append(err) potential_colors_avg = {} for color, errs in potential_colors.iteritems(): if len(errs) > self.spottings_req: potential_colors_avg[color] = np.average(errs) print "potential_colors_avg", potential_colors_avg if len(potential_colors_avg) == 1: # No debate about the color color = potential_colors_avg.keys()[0] err = potential_colors_avg[color] fprint("Only one color found, error: {} (/ {})".format( err, self.error_tolerance)) if len(potential_colors[color] ) > self.spottings_req and err <= self.error_tolerance: return color elif len(potential_colors_avg) > 1: # More than one color, see if one of them is still valid fprint("More than one color detected. Removing all.") self.colored[_id] = [] return None def got_request(self, req): # Threading blah blah something unsafe color = self.valid_color(req.target_id) if color is None: fprint("ID {} is NOT a valid color".format(req.target_id), msg_color='red') return VisionRequestResponse(found=False) return VisionRequestResponse(found=True, color=color) def do_update(self, *args): resp = self.make_request(name='all') if resp.found: # temp time_of_marker = rospy.Time.now() - ros_t( .2) # header.stamp not filled out image_holder = self.image_history.get_around_time(time_of_marker) if not image_holder.contains_valid_image: return header = navigator_tools.make_header( frame="/enu", stamp=image_holder.time) #resp.objects[0].header image = image_holder.image self.debug.image = np.copy(image) cam_tf = self.camera_model.tfFrame() fprint("Getting transform between /enu and {}...".format(cam_tf)) self.tf_listener.waitForTransform("/enu", cam_tf, time_of_marker, ros_t(1)) t_mat44 = self.tf_listener.asMatrix( cam_tf, header) # homogenous 4x4 transformation matrix for obj in resp.objects: if len(obj.points) > 0 and obj.name == "totem": fprint("{} {}".format(obj.id, "=" * 50)) print obj.position # Get objects position in camera frame and make sure it's in view object_cam = t_mat44.dot( np.append(navigator_tools.point_to_numpy(obj.position), 1)) object_px = map( int, self.camera_model.project3dToPixel(object_cam[:3])) if not self._object_in_frame(object_cam): continue #print object_px points_np = np.array( map(navigator_tools.point_to_numpy, obj.points)) # We dont want points below a certain level points_np = points_np[points_np[:, 2] > -2.5] # Shove ones in there to make homogenous points points_np_homo = np.hstack( (points_np, np.ones((points_np.shape[0], 1)))).T points_cam = t_mat44.dot(points_np_homo).T points_px = map(self.camera_model.project3dToPixel, points_cam[:, :3]) #print points_px roi = self._get_ROI_from_points(points_px) color_info = self._get_color_from_ROI( roi, image) # hue, string_color, error bgr = (0, 0, 0) if color_info is not None: hue, color, error = color_info c = (int(hue), 255, 255) hsv = np.array([[c]])[:, :3].astype(np.uint8) bgr = cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR)[0, 0] bgr = tuple(bgr.astype(np.uint8).tolist()) if hue is not None: if obj.id not in self.colored: self.colored[obj.id] = [] self.colored[obj.id].append({ 'color': color, 'err': error }) if self.valid_color(obj.id): fprint("COLOR IS VALID", msg_color='green') self.make_request( cmd= '{name}={bgr[2]},{bgr[1]},{bgr[0]},{_id}'. format(name=obj.name, _id=obj.id, bgr=bgr)) [ cv2.circle(self.debug.image, tuple(map(int, p)), 2, bgr, -1) for p in points_px ] cv2.circle(self.debug.image, tuple(object_px), 10, bgr, -1) font = cv2.FONT_HERSHEY_SIMPLEX cv2.putText(self.debug.image, str(obj.id), tuple(object_px), font, 1, bgr, 2) print '\n' * 2 def _get_ROI_from_points(self, image_points): # Probably will return a set of contours cnt = np.array(image_points).astype(np.float32) rect = cv2.minAreaRect(cnt) box = cv2.cv.BoxPoints(rect) box = np.int0(box) fprint("Drawing rectangle") cv2.drawContours(self.debug.image, [box], 0, (0, 0, 255), 2) return box def _get_color_from_ROI(self, roi, img): mask = np.zeros(img.shape[:2], np.uint8) cv2.drawContours(mask, [roi], 0, 255, -1) bgr = cv2.mean(img, mask) # We have to treat that bgr value as a 3d array to get cvtColor to work bgr = np.array([[bgr]])[:, :3].astype(np.uint8) h, s, v = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)[0, 0] # Now check that s and v are in a good range if s < self.saturation_reject or v < self.value_reject: fprint("The colors aren't expressive enough. Rejecting.", msg_color='red') return None # Compute hue error in SO2 hue_angle = np.radians(h * 2) c = np.cos(hue_angle) s = np.sin(hue_angle) error = np.inf likely_color = 'bazinga' for color, h_val in self.color_map.iteritems(): cg = np.cos(h_val) sg = np.sin(h_val) this_error = np.abs(np.arctan2(sg * c - cg * s, cg * c + sg * s)) if this_error < error: error = this_error likely_color = color if error > self.hue_error_reject: fprint( "Closest color was {} with an error of {} rads (> {}) Rejecting." .format(likely_color, np.round(error, 3), self.hue_error_reject), msg_color='red') return None fprint("Likely color: {} with an hue error of {} rads.".format( likely_color, np.round(error, 3))) return [ np.degrees(self.color_map[likely_color]) / 2.0, likely_color, error ] def _object_in_frame(self, object_point): """ Returns if the object is in frame given the centroid of the object. `object_point` should be in the camera_model's frame. """ print object_point if object_point[2] < 0: return False px = np.array(self.camera_model.project3dToPixel(object_point)) resoultion = self.camera_model.fullResolution() if np.any([0, 0] > px) or np.any(px > resoultion): return False return True
class VampireIdentifier: def __init__(self): # Pull constants from config file self.override = False self.lower = [0, 0, 0] self.upper = [0, 0, 0] self.min_trans = 0 self.max_velocity = 0 self.timeout = 0 self.min_observations = 0 self.camera = rospy.get_param('~camera_topic', '/camera/down/image_rect_color') self.goal = None self.last_config = None self.reconfigure_server = DynamicReconfigureServer( VampireIdentifierConfig, self.reconfigure) # Instantiate remaining variables and objects self._observations = deque() self._pose_pairs = deque() self._times = deque() self.last_image_time = None self.last_image = None self.tf_listener = tf.TransformListener() self.status = '' self.est = None self.visual_id = 0 self.enabled = False self.bridge = CvBridge() # Image Subscriber and Camera Information self.image_sub = Image_Subscriber(self.camera, self.image_cb) self.camera_info = self.image_sub.wait_for_camera_info() self.camera_info = self.image_sub.wait_for_camera_info() self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(self.camera_info) self.frame_id = self.camera_model.tfFrame() # Ros Services so mission can be toggled and info requested rospy.Service('~enable', SetBool, self.toggle_search) self.multi_obs = MultiObservation(self.camera_model) rospy.Service('~pose', VisionRequest, self.request_buoy) self.image_pub = Image_Publisher("drac_vision/debug") self.point_pub = rospy.Publisher("drac_vision/points", Point, queue_size=1) self.mask_image_pub = rospy.Publisher('drac_vision/mask', Image, queue_size=1) # Debug self.debug = rospy.get_param('~debug', True) @staticmethod def parse_string(threshes): ret = [float(thresh.strip()) for thresh in threshes.split(',')] if len(ret) != 3: raise ValueError('not 3') return ret def reconfigure(self, config, level): try: self.override = config['override'] self.goal = config['target'] self.lower = self.parse_string(config['dyn_lower']) self.upper = self.parse_string(config['dyn_upper']) self.min_trans = config['min_trans'] self.max_velocity = config['max_velocity'] self.timeout = config['timeout'] self.min_observations = config['min_obs'] except ValueError as e: rospy.logwarn('Invalid dynamic reconfigure: {}'.format(e)) return self.last_config if self.override: # Dynamic Values for testing self.lower = np.array(self.lower) self.upper = np.array(self.upper) else: # Hard Set for use in Competition if self.goal == 'drac': self.lower = rospy.get_param('~dracula_low_thresh', [0, 0, 80]) self.upper = rospy.get_param('~dracula_high_thresh', [0, 0, 80]) else: raise ValueError('Invalid Target Name') self.last_config = config rospy.loginfo('Params succesfully updated via dynamic reconfigure') return config def image_cb(self, image): ''' Run each time an image comes in from ROS. ''' if not self.enabled: return self.last_image = image if self.last_image_time is not None and \ self.image_sub.last_image_time < self.last_image_time: # Clear tf buffer if time went backwards (nice for playing bags in # loop) self.tf_listener.clear() self.last_image_time = self.image_sub.last_image_time self.acquire_targets(image) def toggle_search(self, srv): ''' Callback for standard ~enable service. If true, start looking at frames for buoys. ''' if srv.data: rospy.loginfo("TARGET ACQUISITION: enabled") self.enabled = True else: rospy.loginfo("TARGET ACQUISITION: disabled") self.enabled = False return SetBoolResponse(success=True) def request_buoy(self, srv): ''' Callback for 3D vision request. Uses recent observations of target board specified in target_name to attempt a least-squares position estimate. Ignoring orientation of board. ''' if not self.enabled: return VisionRequestResponse(found=False) # buoy = self.buoys[srv.target_name] if self.est is None: return VisionRequestResponse(found=False) return VisionRequestResponse(pose=PoseStamped( header=Header(stamp=self.last_image_time, frame_id='/map'), pose=Pose(position=Point(*self.est))), found=True) def clear_old_observations(self): ''' Observations older than two seconds are discarded. ''' time = rospy.Time.now() i = 0 while i < len(self._times): if time - self._times[i] > self.timeout: self._times.popleft() self._observations.popleft() self._pose_pairs.popleft() else: i += 1 # print('Clearing') def add_observation(self, obs, pose_pair, time): ''' Add a new observation associated with an object ''' self.clear_old_observations() # print('Adding...') if slen(self._observations) == 0 or np.linalg.norm( self._pose_pairs[-1][0] - pose_pair[0]) > self.min_trans: self._observations.append(obs) self._pose_pairs.append(pose_pair) self._times.append(time) def get_observations_and_pose_pairs(self): ''' Fetch all recent observations + clear old ones ''' self.clear_old_observations() return (self._observations, self._pose_pairs) def detect(self, c): ''' Verify the shape in the masked image is large enough to be a valid target. This changes depending on target Vampire, as does the number of targets we want. ''' target = "unidentified" peri = cv2.arcLength(c, True) if peri < self.min_contour_area or peri > self.max_contour_area: return target approx = cv2.approxPolyDP(c, 0.04 * peri, True) target = "Target Aquisition Successful" return target def mask_image(self, cv_image, lower, upper): mask = cv2.inRange(cv_image, lower, upper) # Remove anything not within the bounds of our mask output = cv2.bitwise_and(cv_image, cv_image, mask=mask) print('ree') if (self.debug): try: # print(output) self.mask_image_pub.publish( self.bridge.cv2_to_imgmsg(np.array(output), 'bgr8')) except CvBridgeError as e: print(e) return output def acquire_targets(self, cv_image): # Take in the data and get its dimensions. height, width, channels = cv_image.shape # create NumPy arrays from the boundaries lower = np.array(self.lower, dtype="uint8") upper = np.array(self.upper, dtype="uint8") # Generate a mask based on the constants. blurred = self.mask_image(cv_image, lower, upper) blurred = cv2.cvtColor(blurred, cv2.COLOR_BGR2GRAY) # Compute contours cnts = cv2.findContours(blurred.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) cnts = cnts[1] ''' We use OpenCV to compute our contours and then begin processing them to ensure we are identifying a proper target. ''' shape = '' peri_max = 0 max_x = 0 max_y = 0 m_shape = '' for c in cnts: # compute the center of the contour, then detect the name of the # shape using only the contour M = cv2.moments(c) if M["m00"] == 0: M["m00"] = .000001 cX = int((M["m10"] / M["m00"])) cY = int((M["m01"] / M["m00"])) self.point_pub.publish(Point(x=cX, y=cY)) shape = self.detect(c) # multiply the contour (x, y)-coordinates by the resize ratio, # then draw the contours and the name of the shape on the image c = c.astype("float") # c *= ratio c = c.astype("int") if shape == "Target Aquisition Successful": if self.debug: try: cv2.drawContours(cv_image, [c], -1, (0, 255, 0), 2) cv2.putText(cv_image, shape, (cX, cY), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2) self.image_pub.publish(cv_image) except CvBridgeError as e: print(e) # Grab the largest contour. Generally this is a safe bet but... We may need to tweak this for the three different vampires. peri = cv2.arcLength(c, True) if peri > peri_max: peri_max = peri max_x = cX max_y = cY m_shape = shape ''' Approximate 3D coordinates. ''' if m_shape == "Target Aquisition Successful": try: self.tf_listener.waitForTransform('/map', self.camera_model.tfFrame(), self.last_image_time, rospy.Duration(0.2)) except tf.Exception as e: rospy.logwarn( "Could not transform camera to map: {}".format(e)) return False (t, rot_q) = self.tf_listener.lookupTransform( '/map', self.camera_model.tfFrame(), self.last_image_time) R = mil_ros_tools.geometry_helpers.quaternion_matrix(rot_q) center = np.array([max_x, max_y]) self.add_observation(center, (np.array(t), R), self.last_image_time) observations, pose_pairs = self.get_observations_and_pose_pairs() if len(observations) > self.min_observations: self.est = self.multi_obs.lst_sqr_intersection( observations, pose_pairs) self.status = 'Pose found' else: self.status = '{} observations'.format(len(observations))
class BuoyFinder(object): ''' Node to find red, green, and yellow buoys in a single camera frame. Combines several observations and uses a least-squares approach to get a 3D position estimate of a buoy when requested. Intended to be modular so other approaches can be tried. Adding more sophistication to segmentation would increase reliability. TODO: Use same mask for yellow/green ''' def __init__(self): self.tf_listener = tf.TransformListener() self.enabled = False self.last_image = None self.last_image_time = None self.camera_model = None self.circle_finder = CircleFinder( 1.0 ) # Model radius doesn't matter beacause it's not being used for 3D pose # Various constants for tuning, debugging. See buoy_finder.yaml for more info self.min_observations = rospy.get_param('~min_observations') self.debug_ros = rospy.get_param('~debug/ros', True) self.debug_cv = rospy.get_param('~debug/cv', False) self.min_contour_area = rospy.get_param('~min_contour_area') self.max_circle_error = rospy.get_param('~max_circle_error') self.max_velocity = rospy.get_param('~max_velocity') self.roi_y = rospy.get_param('~roi_y') self.roi_height = rospy.get_param('~roi_height') camera = rospy.get_param('~camera_topic', '/camera/front/right/image_rect_color') self.buoys = {} for color in ['red', 'yellow', 'green']: self.buoys[color] = Buoy(color, debug_cv=self.debug_cv) if self.debug_cv: cv2.waitKey(1) self.debug_images = {} # Keep latest odom message for sanity check self.last_odom = None self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_cb, queue_size=3) self.image_sub = Image_Subscriber(camera, self.image_cb) if self.debug_ros: self.rviz = rviz.RvizVisualizer(topic='~markers') self.mask_pub = Image_Publisher('~mask_image') rospy.Timer(rospy.Duration(1), self.print_status) self.camera_info = self.image_sub.wait_for_camera_info() self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(self.camera_info) self.frame_id = self.camera_model.tfFrame() self.multi_obs = MultiObservation(self.camera_model) rospy.Service('~enable', SetBool, self.toggle_search) rospy.Service('~2D', VisionRequest2D, self.request_buoy) rospy.Service('~pose', VisionRequest, self.request_buoy3d) rospy.loginfo("BUOY FINDER: initialized successfully") def odom_cb(self, odom): self.last_odom = odom def toggle_search(self, srv): ''' Callback for standard ~enable service. If true, start looking at frames for buoys. ''' if srv.data: rospy.loginfo("BUOY FINDER: enabled") self.enabled = True else: rospy.loginfo("BUOY FINDER: disabled") self.enabled = False return SetBoolResponse(success=True) def request_buoy(self, srv): ''' Callback for 2D vision request. Returns centroid of buoy found with color specified in target_name if found. ''' if not self.enabled or srv.target_name not in self.buoys: return VisionRequest2DResponse(found=False) response = self.find_single_buoy(srv.target_name) if response is False or response is None: return VisionRequest2DResponse(found=False) center, radius = response return VisionRequest2DResponse(header=Header( stamp=self.last_image_time, frame_id=self.frame_id), pose=Pose2D( x=center[0], y=center[1], ), max_x=self.last_image.shape[0], max_y=self.last_image.shape[1], camera_info=self.image_sub.camera_info, found=True) def request_buoy3d(self, srv): ''' Callback for 3D vision request. Uses recent observations of buoy specified in target_name to attempt a least-squares position estimate. As buoys are spheres, orientation is meaningless. ''' if srv.target_name not in self.buoys or not self.enabled: return VisionRequestResponse(found=False) buoy = self.buoys[srv.target_name] if buoy.est is None: return VisionRequestResponse(found=False) return VisionRequestResponse(pose=PoseStamped( header=Header(stamp=self.last_image_time, frame_id='/map'), pose=Pose(position=Point(*buoy.est))), found=True) def image_cb(self, image): ''' Run each time an image comes in from ROS. If enabled, attempt to find each color buoy. ''' if not self.enabled: return # Crop out some of the top and bottom to exclude the floor and surface reflections height = image.shape[0] roi_y = int(self.roi_y * height) roi_height = height - int(self.roi_height * height) self.roi = (0, roi_y, roi_height, image.shape[1]) self.last_image = image[self.roi[1]:self.roi[2], self.roi[0]:self.roi[3]] self.image_area = self.last_image.shape[0] * self.last_image.shape[1] if self.debug_ros: # Create a blacked out debug image for putting masks in self.mask_image = np.zeros(self.last_image.shape, dtype=image.dtype) if self.last_image_time is not None and self.image_sub.last_image_time < self.last_image_time: # Clear tf buffer if time went backwards (nice for playing bags in loop) self.tf_listener.clear() self.last_image_time = self.image_sub.last_image_time self.find_buoys() if self.debug_ros: self.mask_pub.publish(self.mask_image) def print_status(self, _): ''' Called at 1 second intervals to display the status (not found, n observations, FOUND) for each buoy. ''' if self.enabled: rospy.loginfo("STATUS: RED='%s', GREEN='%s', YELLOW='%s'", self.buoys['red'].status, self.buoys['green'].status, self.buoys['yellow'].status) def find_buoys(self): ''' Run find_single_buoy for each color of buoy ''' for buoy_name in self.buoys: self.find_single_buoy(buoy_name) def is_circular_contour(self, cnt): ''' Check that a contour is close enough to a circle (using hue invariants) to be a pottential buoy. ''' return self.circle_finder.verify_contour(cnt) <= self.max_circle_error def get_best_contour(self, contours): ''' Attempts to find a good buoy contour among those found within the thresholded mask. If a good one is found, it return (contour, centroid, area), otherwise returns None. Right now the best contour is just the largest. @param contours Numpy array of contours from a particular buoy's mask @return A tuple (contour, error) where contour will be the best contour in an image or None if no contours pass criteria. Error will be a string describing why no good contour was found, or None if contour is not None. ''' if len(contours) == 0: return None, 'no contours in mask' circular_contours = filter(self.is_circular_contour, contours) if len(circular_contours) == 0: return None, 'fails circularity test' circles_sorted = sorted(circular_contours, key=cv2.contourArea, reverse=True) if cv2.contourArea( circles_sorted[0]) < self.min_contour_area * self.image_area: return None, 'fails area test' return circles_sorted[ 0], None # Return the largest contour that pases shape test def find_single_buoy(self, buoy_type): ''' Attempt to find one color buoy in the image. 1) Create mask for buoy's color in colorspace specified in paramaters 2) Select the largest contour in this mask 3) Approximate a circle around this contour 4) Store the center of this circle and the current tf between /map and camera as an observation 5) If observations for this buoy is now >= min_observations, approximate buoy position using the least squares tool imported ''' assert buoy_type in self.buoys.keys( ), "Buoys_2d does not know buoy color: {}".format(buoy_type) buoy = self.buoys[buoy_type] mask = buoy.get_mask(self.last_image) kernel = np.ones((5, 5), np.uint8) mask = cv2.erode(mask, kernel, iterations=2) mask = cv2.dilate(mask, kernel, iterations=2) _, contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE, offset=(self.roi[0], self.roi[1])) if self.debug_ros: cv2.add(self.mask_image.copy(), buoy.cv_colors, mask=mask, dst=self.mask_image) if self.debug_cv: self.debug_images[buoy_type] = mask.copy() cnt, err = self.get_best_contour(contours) if cnt is None: buoy.clear_old_observations() buoy.status = '{} w/ {} obs'.format(err, buoy.size()) return center, radius = cv2.minEnclosingCircle(cnt) if self.debug_ros: cv2.circle( self.mask_image, (int(center[0] - self.roi[0]), int(center[1]) - self.roi[1]), int(radius), buoy.cv_colors, 4) try: self.tf_listener.waitForTransform('/map', self.frame_id, self.last_image_time, rospy.Duration(0.2)) except tf.Exception as e: rospy.logwarn("Could not transform camera to map: {}".format(e)) return False if not self.sanity_check(center, self.last_image_time): buoy.status = 'failed sanity check' return False (t, rot_q) = self.tf_listener.lookupTransform('/map', self.frame_id, self.last_image_time) R = mil_ros_tools.geometry_helpers.quaternion_matrix(rot_q) buoy.add_observation(center, (np.array(t), R), self.last_image_time) observations, pose_pairs = buoy.get_observations_and_pose_pairs() if len(observations) > self.min_observations: buoy.est = self.multi_obs.lst_sqr_intersection( observations, pose_pairs) buoy.status = 'Pose found' if self.debug_ros: self.rviz.draw_sphere(buoy.est, color=buoy.draw_colors, scaling=(0.2286, 0.2286, 0.2286), frame='/map', _id=buoy.visual_id) else: buoy.status = '{} observations'.format(len(observations)) return center, radius def sanity_check(self, coordinate, timestamp): ''' Check if the observation is unreasonable. More can go here if we want. ''' if self.last_odom is None: return False linear_velocity = rosmsg_to_numpy(self.last_odom.twist.twist.linear) if np.linalg.norm(linear_velocity) > self.max_velocity: return False return True
class CLAHE_generator: def __init__(self): self.camera = rospy.get_param('~camera_topic', '/camera/front/left/image_rect_color') # Instantiate remaining variables and objects self.last_image_time = None self.last_image = None self.tf_listener = tf.TransformListener() self.status = '' self.est = None self.visual_id = 0 self.enabled = False self.bridge = CvBridge() # Image Subscriber and Camera Information self.image_sub = Image_Subscriber(self.camera, self.image_cb) self.camera_info = self.image_sub.wait_for_camera_info() ''' These variables store the camera information required to perform the transformations on the coordinates to move from the subs perspective to our global map perspective. This information is also necessary to perform the least squares intersection which will find the 3D coordinates of the torpedo board based on 2D observations from the Camera. More info on this can be found in sub8_vision_tools. ''' self.camera_info = self.image_sub.wait_for_camera_info() self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(self.camera_info) self.frame_id = self.camera_model.tfFrame() self.image_pub = Image_Publisher("CLAHE/debug") # Debug self.debug = rospy.get_param('~debug', True) def image_cb(self, image): ''' Run each time an image comes in from ROS. If enabled, attempt to find the torpedo board. ''' self.last_image = image if self.last_image_time is not None and \ self.image_sub.last_image_time < self.last_image_time: # Clear tf buffer if time went backwards (nice for playing bags in # loop) self.tf_listener.clear() self.last_image_time = self.image_sub.last_image_time self.CLAHE(image) print('published') def CLAHE(self, cv_image): ''' CLAHE (Contrast Limited Adaptive Histogram Equalization) This increases the contrast between color channels and allows us to better differentiate colors under certain lighting conditions. ''' clahe = cv2.createCLAHE(clipLimit=9.5, tileGridSize=(4, 4)) # convert from BGR to LAB color space lab = cv2.cvtColor(cv_image, cv2.COLOR_BGR2LAB) l, a, b = cv2.split(lab) # split on 3 different channels l2 = clahe.apply(l) # apply CLAHE to the L-channel lab = cv2.merge((l2, a, b)) # merge channels cv_image = cv2.cvtColor(lab, cv2.COLOR_LAB2BGR) self.image_pub.publish(cv_image)
class BuoyFinder(object): ''' Node to find red, green, and yellow buoys in a single camera frame. Combines several observations and uses a least-squares approach to get a 3D position estimate of a buoy when requested. Intended to be modular so other approaches can be tried. Adding more sophistication to segmentation would increase reliability. ''' def __init__(self): self.tf_listener = tf.TransformListener() self.enabled = False self.last_image = None self.last_image_time = None self.camera_model = None # Various constants for tuning, debugging. See buoy_finder.yaml for more info self.min_observations = rospy.get_param('~min_observations') self.debug_ros = rospy.get_param('~debug/ros', True) self.debug_cv = rospy.get_param('~debug/cv', False) self.min_contour_area = rospy.get_param('~min_contour_area') self.max_velocity = rospy.get_param('~max_velocity') camera = rospy.get_param('~camera_topic', '/camera/front/right/image_rect_color') self.buoys = {} for color in ['red', 'yellow', 'green']: self.buoys[color] = Buoy(color, debug_cv=self.debug_cv) self.buoys[color].load_segmentation() if self.debug_cv: cv2.waitKey(1) self.debug_images = {} # Keep latest odom message for sanity check self.last_odom = None self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_cb, queue_size=3) self.image_sub = Image_Subscriber(camera, self.image_cb) if self.debug_ros: self.rviz = rviz.RvizVisualizer(topic='~markers') self.mask_pub = Image_Publisher('~mask_image') rospy.Timer(rospy.Duration(1), self.print_status) self.camera_info = self.image_sub.wait_for_camera_info() self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(self.camera_info) self.frame_id = self.camera_model.tfFrame() self.multi_obs = MultiObservation(self.camera_model) rospy.Service('~enable', SetBool, self.toggle_search) rospy.Service('~2D', VisionRequest2D, self.request_buoy) rospy.Service('~pose', VisionRequest, self.request_buoy3d) rospy.loginfo("BUOY FINDER: initialized successfully") def odom_cb(self, odom): self.last_odom = odom def toggle_search(self, srv): ''' Callback for standard ~enable service. If true, start looking at frames for buoys. ''' if srv.data: rospy.loginfo("BUOY FINDER: enabled") self.enabled = True else: rospy.loginfo("BUOY FINDER: disabled") self.enabled = False return SetBoolResponse(success=True) def request_buoy(self, srv): ''' Callback for 2D vision request. Returns centroid of buoy found with color specified in target_name if found. ''' if not self.enabled or srv.target_name not in self.buoys: return VisionRequest2DResponse(found=False) response = self.find_single_buoy(srv.target_name) if response is False or response is None: return VisionRequest2DResponse(found=False) center, radius = response return VisionRequest2DResponse( header=Header(stamp=self.last_image_time, frame_id=self.frame_id), pose=Pose2D( x=center[0], y=center[1], ), max_x=self.last_image.shape[0], max_y=self.last_image.shape[1], camera_info=self.image_sub.camera_info, found=True ) def request_buoy3d(self, srv): ''' Callback for 3D vision request. Uses recent observations of buoy specified in target_name to attempt a least-squares position estimate. As buoys are spheres, orientation is meaningless. ''' if srv.target_name not in self.buoys or not self.enabled: return VisionRequestResponse(found=False) buoy = self.buoys[srv.target_name] if buoy.est is None: return VisionRequestResponse(found=False) return VisionRequestResponse( pose=PoseStamped( header=Header(stamp=self.last_image_time, frame_id='/map'), pose=Pose( position=Point(*buoy.est) ) ), found=True ) def image_cb(self, image): ''' Run each time an image comes in from ROS. If enabled, attempt to find each color buoy. ''' if not self.enabled: return # Crop out some of the top and bottom to exclude the floor and surface reflections height = image.shape[0] roi_y = int(0.2 * height) roi_height = height - int(0.2 * height) self.roi = (0, roi_y, roi_height, image.shape[1]) self.last_image = image[self.roi[1]:self.roi[2], self.roi[0]:self.roi[3]] if self.debug_ros: # Create a blacked out debug image for putting masks in self.mask_image = np.zeros(self.last_image.shape, dtype=image.dtype) if self.last_image_time is not None and self.image_sub.last_image_time < self.last_image_time: # Clear tf buffer if time went backwards (nice for playing bags in loop) self.tf_listener.clear() self.last_image_time = self.image_sub.last_image_time self.find_buoys() if self.debug_ros: self.mask_pub.publish(self.mask_image) def print_status(self, _): ''' Called at 1 second intervals to display the status (not found, n observations, FOUND) for each buoy. ''' if self.enabled: rospy.loginfo("STATUS: RED='%s', GREEN='%s', YELLOW='%s'", self.buoys['red'].status, self.buoys['green'].status, self.buoys['yellow'].status) def find_buoys(self): ''' Run find_single_buoy for each color of buoy ''' for buoy_name in self.buoys: self.find_single_buoy(buoy_name) def get_best_contour(self, contours): ''' Attempts to find a good buoy contour among those found within the thresholded mask. If a good one is found, it return (contour, centroid, area), otherwise returns None. Right now the best contour is just the largest. TODO: Use smarter contour filtering methods, like checking this it is circle like ''' if len(contours) > 0: cnt = max(contours, key=cv2.contourArea) area = cv2.contourArea(cnt) if area < self.min_contour_area: return None M = cv2.moments(cnt) cx = int(M['m10'] / M['m00']) cy = int(M['m01'] / M['m00']) tpl_center = (int(cx), int(cy)) return cnt, tpl_center, area else: return None def find_single_buoy(self, buoy_type): ''' Attempt to find one color buoy in the image. 1) Create mask for buoy's color in colorspace specified in paramaters 2) Select the largest contour in this mask 3) Approximate a circle around this contour 4) Store the center of this circle and the current tf between /map and camera as an observation 5) If observations for this buoy is now >= min_observations, approximate buoy position using the least squares tool imported ''' assert buoy_type in self.buoys.keys(), "Buoys_2d does not know buoy color: {}".format(buoy_type) buoy = self.buoys[buoy_type] mask = buoy.get_mask(self.last_image) kernel = np.ones((5, 5), np.uint8) mask = cv2.erode(mask, kernel, iterations=2) mask = cv2.dilate(mask, kernel, iterations=2) _, contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE, offset=(self.roi[0], self.roi[1])) ret = self.get_best_contour(contours) if ret is None: buoy.clear_old_observations() buoy.status = 'not seen w/ {} obs'.format(buoy.size()) return contour, tuple_center, area = ret true_center, rad = cv2.minEnclosingCircle(contour) if self.debug_ros: cv2.add(self.mask_image.copy(), buoy.cv_colors, mask=mask, dst=self.mask_image) cv2.circle(self.mask_image, (int(true_center[0] - self.roi[0]), int(true_center[1]) - self.roi[1]), int(rad), buoy.cv_colors, 2) if self.debug_cv: self.debug_images[buoy_type] = mask.copy() try: self.tf_listener.waitForTransform('/map', self.frame_id, self.last_image_time, rospy.Duration(0.2)) except tf.Exception as e: rospy.logwarn("Could not transform camera to map: {}".format(e)) return False if not self.sanity_check(tuple_center, self.last_image_time): buoy.status = 'failed sanity check' return False (t, rot_q) = self.tf_listener.lookupTransform('/map', self.frame_id, self.last_image_time) R = mil_ros_tools.geometry_helpers.quaternion_matrix(rot_q) buoy.add_observation(true_center, (np.array(t), R), self.last_image_time) observations, pose_pairs = buoy.get_observations_and_pose_pairs() if len(observations) > self.min_observations: buoy.est = self.multi_obs.lst_sqr_intersection(observations, pose_pairs) buoy.status = 'Pose found' if self.debug_ros: self.rviz.draw_sphere(buoy.est, color=buoy.draw_colors, scaling=(0.2286, 0.2286, 0.2286), frame='/map', _id=buoy.visual_id) else: buoy.status = '{} observations'.format(len(observations)) return tuple_center, rad def sanity_check(self, coordinate, timestamp): ''' Check if the observation is unreasonable. More can go here if we want. ''' if self.last_odom is None: return False linear_velocity = rosmsg_to_numpy(self.last_odom.twist.twist.linear) if np.linalg.norm(linear_velocity) > self.max_velocity: return False return True
class ros_cv_testing_node: def __init__(self): # Get information for this particular camera camera_info = get_single('head_camera/depth_registered/camera_info', CameraInfo) print camera_info # Set information for the camera self.cam_model = PinholeCameraModel() self.cam_model.fromCameraInfo(camera_info) # Subscribe to the camera's image topic and depth image topic self.rgb_image_sub = rospy.Subscriber("head_camera/rgb/image_raw", Image, self.on_rgb) self.depth_image_sub = rospy.Subscriber("head_camera/depth_registered/image_raw", Image, self.on_depth) # Publish where the button is in the base link frame self.point_pub = rospy.Publisher('button_point', PointStamped, queue_size=10) # Set up connection to CvBridge self.bridge = CvBridge() # Open up viewing windows cv2.namedWindow('depth') cv2.namedWindow('rgb') # Set up the class variables self.rgb_image = None self.rgb_image_time = None self.depth_image = None self.center = None self.return_point = PointStamped() self.tf_listener = TransformListener() def on_rgb(self, image): # Convert image to something that cv2 can work with try: self.rgb_image = self.bridge.imgmsg_to_cv2(image, 'bgr8') except e: print e return self.rgb_image_time = image.header.stamp # Get height and width of image h = self.rgb_image.shape[0] w = self.rgb_image.shape[1] # Create empty image color_dst = numpy.empty((h,w), 'uint8') # Convert picture to grayscale cv2.cvtColor(self.rgb_image, cv2.COLOR_RGB2GRAY, color_dst) # Find circles given the camera image dp = 1.1 minDist = 90 circles = cv2.HoughCircles(color_dst, cv2.cv.CV_HOUGH_GRADIENT, dp, minDist) # If no circles are detected then exit the program if circles == None: print "No circles found using these parameters" sys.exit() circles = numpy.uint16(numpy.around(circles)) # Draw the center of the circle closest to the top ycoord = [] for i in circles[0,:]: ycoord.append(i[1]) # Find the top circle in the frame top_circ_y_coor = min(ycoord) x_coor = 0 y_coor = 0 for i in circles[0,:]: if i[1] == top_circ_y_coor: # draw the center of the circle #cv2.circle(self.rgb_image,(i[0],i[1]),2,(0,0,255),3) # draw outer circle #cv2.circle(self.rgb_image,(i[0],i[1]),i[2],(0,255,0),2) x_coor = i[0] y_coor = i[1] cv2.circle(self.rgb_image,(327,247),2,(0,0,255),3) # Set the coordinates for the center of the circle self.center = (x_coor, y_coor) #self.center = (328,248) cv2.imshow('rgb', self.rgb_image) cv2.waitKey(1) def on_depth(self, image): # Use numpy so that cv2 can use image self.depth_image = numpy.fromstring(image.data, dtype='float32').reshape((480,640)) nmask = numpy.isnan(self.depth_image) #Find the minimum and the maximum of the depth image Dmin = self.depth_image[~nmask].min() Dmax = self.depth_image[~nmask].max() # If a circle has been found find its depth and apply that to the ray if self.center!=None: ray = self.cam_model.projectPixelTo3dRay(self.center) depth = self.depth_image[self.center[1]][self.center[0]] # If the depth is not a number exit if math.isnan(depth): print "Incomplete information on depth at this point" return # Otherwise mulitply the depth by the unit vector of the ray else: print "depth", depth cam_coor = [depth*ray[0],depth*ray[1],depth*ray[2]] #print "camera frame coordinate:", cam_coor else: return # Rescale to [0,1] cv2.imshow('depth', self.depth_image / 2.0) cv2.waitKey(1) # Only use depth if the RGB image is running if self.rgb_image_time is None: rospy.loginfo('not using depth cause no RGB yet') return time_since_rgb = image.header.stamp - self.rgb_image_time # Only use depth if it is close in time to the RGB image if time_since_rgb > rospy.Duration(0.5): rospy.loginfo('not using depth because time since RGB is ' + str(time_since_rgb)) #return # Find transform from camera frame to world frame self.tf_listener.waitForTransform(self.cam_model.tfFrame(), "base_link", image.header.stamp, rospy.Duration(1.0)) # Set up the point to be published self.return_point.header.stamp = image.header.stamp self.return_point.header.frame_id = self.cam_model.tfFrame() self.return_point.point.x = cam_coor[0] self.return_point.point.y = cam_coor[1] self.return_point.point.z = cam_coor[2] # Transform point to the baselink frame self.return_point = self.tf_listener.transformPoint("base_link", self.return_point) print "world frame coordinate:", self.return_point.point.x, \ self.return_point.point.y,self.return_point.point.z, "\n" self.point_pub.publish(self.return_point)
class torp_vision: def __init__(self): # Pull constants from config file self.lower = rospy.get_param('~lower_color_threshold', [0, 0, 80]) self.upper = rospy.get_param( '~higher_color_threshold', [200, 200, 250]) self.min_contour_area = rospy.get_param('~min_contour_area', .001) self.max_contour_area = rospy.get_param('~max_contour_area', 400) self.min_trans = rospy.get_param('~min_trans', .05) self.max_velocity = rospy.get_param('~max_velocity', 1) self.timeout = rospy.Duration( rospy.get_param('~timeout_seconds'), 250000) self.min_observations = rospy.get_param('~min_observations', 8) self.camera = rospy.get_param('~camera_topic', '/camera/front/left/image_rect_color') # Instantiate remaining variables and objects self._observations = deque() self._pose_pairs = deque() self._times = deque() self.last_image_time = None self.last_image = None self.tf_listener = tf.TransformListener() self.status = '' self.est = None self.visual_id = 0 self.enabled = False self.bridge = CvBridge() # Image Subscriber and Camera Information self.image_sub = Image_Subscriber(self.camera, self.image_cb) self.camera_info = self.image_sub.wait_for_camera_info() ''' These variables store the camera information required to perform the transformations on the coordinates to move from the subs perspective to our global map perspective. This information is also necessary to perform the least squares intersection which will find the 3D coordinates of the torpedo board based on 2D observations from the Camera. More info on this can be found in sub8_vision_tools. ''' self.camera_info = self.image_sub.wait_for_camera_info() self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(self.camera_info) self.frame_id = self.camera_model.tfFrame() # Ros Services so mission can be toggled and info requested rospy.Service('~enable', SetBool, self.toggle_search) self.multi_obs = MultiObservation(self.camera_model) rospy.Service('~pose', VisionRequest, self.request_board3d) self.image_pub = Image_Publisher("torp_vision/debug") self.point_pub = rospy.Publisher( "torp_vision/points", Point, queue_size=1) self.mask_image_pub = rospy.Publisher( 'torp_vison/mask', Image, queue_size=1) # Debug self.debug = rospy.get_param('~debug', True) def image_cb(self, image): ''' Run each time an image comes in from ROS. If enabled, attempt to find the torpedo board. ''' if not self.enabled: return self.last_image = image if self.last_image_time is not None and \ self.image_sub.last_image_time < self.last_image_time: # Clear tf buffer if time went backwards (nice for playing bags in # loop) self.tf_listener.clear() self.last_image_time = self.image_sub.last_image_time self.acquire_targets(image) def toggle_search(self, srv): ''' Callback for standard ~enable service. If true, start looking at frames for buoys. ''' if srv.data: rospy.loginfo("TARGET ACQUISITION: enabled") self.enabled = True else: rospy.loginfo("TARGET ACQUISITION: disabled") self.enabled = False return SetBoolResponse(success=True) def request_board3d(self, srv): ''' Callback for 3D vision request. Uses recent observations of target board specified in target_name to attempt a least-squares position estimate. Ignoring orientation of board. ''' if not self.enabled: return VisionRequestResponse(found=False) # buoy = self.buoys[srv.target_name] if self.est is None: return VisionRequestResponse(found=False) return VisionRequestResponse( pose=PoseStamped( header=Header(stamp=self.last_image_time, frame_id='/map'), pose=Pose(position=Point(*self.est))), found=True) def clear_old_observations(self): # Observations older than two seconds are discarded. time = rospy.Time.now() i = 0 while i < len(self._times): if time - self._times[i] > self.timeout: self._times.popleft() self._observations.popleft() self._pose_pairs.popleft() else: i += 1 # print('Clearing') def size(self): return len(self._observations) def add_observation(self, obs, pose_pair, time): self.clear_old_observations() # print('Adding...') if self.size() == 0 or np.linalg.norm( self._pose_pairs[-1][0] - pose_pair[0]) > self.min_trans: self._observations.append(obs) self._pose_pairs.append(pose_pair) self._times.append(time) def get_observations_and_pose_pairs(self): self.clear_old_observations() return (self._observations, self._pose_pairs) def detect(self, c): # initialize the shape name and approximate the contour target = "unidentified" peri = cv2.arcLength(c, True) if peri < self.min_contour_area or peri > self.max_contour_area: return target approx = cv2.approxPolyDP(c, 0.04 * peri, True) if len(approx) == 4: target = "Target Aquisition Successful" elif len(approx) == 3 or len(approx) == 5: target = "Partial Target Acquisition" return target def CLAHE(self, cv_image): ''' CLAHE (Contrast Limited Adaptive Histogram Equalization) This increases the contrast between color channels and allows us to better differentiate colors under certain lighting conditions. ''' clahe = cv2.createCLAHE(clipLimit=5., tileGridSize=(4, 4)) # convert from BGR to LAB color space lab = cv2.cvtColor(cv_image, cv2.COLOR_BGR2LAB) l, a, b = cv2.split(lab) # split on 3 different channels l2 = clahe.apply(l) # apply CLAHE to the L-channel lab = cv2.merge((l2, a, b)) # merge channels cv_image = cv2.cvtColor(lab, cv2.COLOR_LAB2BGR) return cv_image def mask_image(self, cv_image, lower, upper): mask = cv2.inRange(cv_image, lower, upper) # Remove anything not within the bounds of our mask output = cv2.bitwise_and(cv_image, cv_image, mask=mask) # Resize to emphasize shapes # gray = cv2.cvtColor(output, cv2.COLOR_BGR2GRAY) # Blur image so our contours can better find the full shape. # blurred = cv2.GaussianBlur(gray, (5, 5), 0) if (self.debug): try: # print(output) self.mask_image_pub.publish( self.bridge.cv2_to_imgmsg(np.array(output), 'bgr8')) except CvBridgeError as e: print(e) return output def acquire_targets(self, cv_image): # Take in the data and get its dimensions. height, width, channels = cv_image.shape # Run CLAHE. cv_image = self.CLAHE(cv_image) # Now we generate a color mask to isolate only red in the image. This # is achieved through the thresholds which can be changed in the above # constants. # create NumPy arrays from the boundaries lower = np.array(self.lower, dtype="uint8") upper = np.array(self.upper, dtype="uint8") # Generate a mask based on the constants. blurred = self.mask_image(cv_image, lower, upper) blurred = cv2.cvtColor(blurred, cv2.COLOR_BGR2GRAY) # Compute contours cnts = cv2.findContours(blurred.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) cnts = cnts[1] ''' We use OpenCV to compute our contours and then begin processing them to ensure we are identifying a proper target. ''' shape = '' peri_max = 0 max_x = 0 max_y = 0 m_shape = '' for c in cnts: # compute the center of the contour, then detect the name of the # shape using only the contour M = cv2.moments(c) if M["m00"] == 0: M["m00"] = .000001 cX = int((M["m10"] / M["m00"])) cY = int((M["m01"] / M["m00"])) shape = self.detect(c) # multiply the contour (x, y)-coordinates by the resize ratio, # then draw the contours and the name of the shape on the image c = c.astype("float") # c *= ratio c = c.astype("int") if shape == "Target Aquisition Successful": if self.debug: try: cv2.drawContours(cv_image, [c], -1, (0, 255, 0), 2) cv2.putText(cv_image, shape, (cX, cY), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2) self.image_pub.publish(cv_image) except CvBridgeError as e: print(e) peri = cv2.arcLength(c, True) if peri > peri_max: peri_max = peri max_x = cX max_y = cY m_shape = shape ''' This is Kevin's Code, adapted for this project. We are trying to find the 3D coordinates of the torpedo board/target to give us a better idea of where we are trying to go and perform more accurate movements to align with the target. The first thing we need to do is convert from camera coordinates in pixels to 3D coordinates. Every time we succesfully get a target aquisition we add it to the counter. Once we observe it enough times we can be confident we are looking at the correct target. We then perform an least squares intersection from multiple angles to derive the approximate 3D coordinates. ''' if m_shape == "Target Aquisition Successful": try: self.tf_listener.waitForTransform('/map', self.camera_model.tfFrame(), self.last_image_time, rospy.Duration(0.2)) except tf.Exception as e: rospy.logwarn( "Could not transform camera to map: {}".format(e)) return False (t, rot_q) = self.tf_listener.lookupTransform( '/map', self.camera_model.tfFrame(), self.last_image_time) R = mil_ros_tools.geometry_helpers.quaternion_matrix(rot_q) center = np.array([max_x, max_y]) self.add_observation(center, (np.array(t), R), self.last_image_time) observations, pose_pairs = self.get_observations_and_pose_pairs() if len(observations) > self.min_observations: self.est = self.multi_obs.lst_sqr_intersection( observations, pose_pairs) self.status = 'Pose found' else: self.status = '{} observations'.format(len(observations))
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 Colorama(object): def __init__(self): info_topic = camera_root + "/camera_info" image_topic = camera_root + "/image_raw" # Change this to rect on boat self.tf_listener = tf.TransformListener() # Map id => [{color, error}, ...] for determining when a color is good self.colored = {} db_request = rospy.ServiceProxy("/database/requests", ObjectDBQuery) self.make_request = lambda **kwargs: db_request(ObjectDBQueryRequest(**kwargs)) rospy.Service("/vision/buoy_colorizer", VisionRequest, self.got_request) self.image_history = ImageHistory(image_topic) # Wait for camera info, and exit if not found try: fprint("Waiting for camera info on: '{}'".format(info_topic)) camera_info_msg = rospy.wait_for_message(info_topic, CameraInfo, timeout=3) except rospy.exceptions.ROSException: fprint("Camera info not found! Terminating.", msg_color="red") rospy.signal_shutdown("Camera not found!") return fprint("Camera info found!", msg_color="green") self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(camera_info_msg) self.debug = DebugImage("/colorama/debug", prd=.5) # These are color mappings from Hue values [0, 360] self.color_map = {'red': np.radians(0), 'yellow': np.radians(60), 'green': np.radians(120), 'blue': np.radians(240)} # Some tunable parameters self.update_time = .5 # s self.saturation_reject = 50 self.value_reject = 50 self.hue_error_reject = .4 # rads # To decide when to accept color observations self.error_tolerance = .4 # rads self.spottings_req = 4 self.similarity_reject = .1 # If two colors are within this amount of error, reject rospy.Timer(ros_t(self.update_time), self.do_update) def valid_color(self, _id): """ Either returns valid color or None depending if the color is valid or not """ if _id not in self.colored: return None object_data = self.colored[_id] print "object_data", object_data # Maps color => [err1, err2, ..] potential_colors = {} for data in object_data: color, err = data['color'], data['err'] if color not in potential_colors: potential_colors[color] = [] potential_colors[color].append(err) potential_colors_avg = {} for color, errs in potential_colors.iteritems(): if len(errs) > self.spottings_req: potential_colors_avg[color] = np.average(errs) print "potential_colors_avg", potential_colors_avg if len(potential_colors_avg) == 1: # No debate about the color color = potential_colors_avg.keys()[0] err = potential_colors_avg[color] fprint("Only one color found, error: {} (/ {})".format(err, self.error_tolerance)) if len(potential_colors[color]) > self.spottings_req and err <= self.error_tolerance: return color elif len(potential_colors_avg) > 1: # More than one color, see if one of them is still valid fprint("More than one color detected. Removing all.") self.colored[_id] = [] return None def got_request(self, req): # Threading blah blah something unsafe color = self.valid_color(req.target_id) if color is None: fprint("ID {} is NOT a valid color".format(req.target_id), msg_color='red') return VisionRequestResponse(found=False) return VisionRequestResponse(found=True, color=color) def do_update(self, *args): resp = self.make_request(name='all') if resp.found: # temp time_of_marker = rospy.Time.now() - ros_t(.2) # header.stamp not filled out image_holder = self.image_history.get_around_time(time_of_marker) if not image_holder.contains_valid_image: return header = navigator_tools.make_header(frame="/enu", stamp=image_holder.time) #resp.objects[0].header image = image_holder.image self.debug.image = np.copy(image) cam_tf = self.camera_model.tfFrame() fprint("Getting transform between /enu and {}...".format(cam_tf)) self.tf_listener.waitForTransform("/enu", cam_tf, time_of_marker, ros_t(1)) t_mat44 = self.tf_listener.asMatrix(cam_tf, header) # homogenous 4x4 transformation matrix for obj in resp.objects: if len(obj.points) > 0 and obj.name == "totem": fprint("{} {}".format(obj.id, "=" * 50)) print obj.position # Get objects position in camera frame and make sure it's in view object_cam = t_mat44.dot(np.append(navigator_tools.point_to_numpy(obj.position), 1)) object_px = map(int, self.camera_model.project3dToPixel(object_cam[:3])) if not self._object_in_frame(object_cam): continue #print object_px points_np = np.array(map(navigator_tools.point_to_numpy, obj.points)) # We dont want points below a certain level points_np = points_np[points_np[:, 2] > -2.5] # Shove ones in there to make homogenous points points_np_homo = np.hstack((points_np, np.ones((points_np.shape[0], 1)))).T points_cam = t_mat44.dot(points_np_homo).T points_px = map(self.camera_model.project3dToPixel, points_cam[:, :3]) #print points_px roi = self._get_ROI_from_points(points_px) color_info = self._get_color_from_ROI(roi, image) # hue, string_color, error bgr = (0, 0, 0) if color_info is not None: hue, color, error = color_info c = (int(hue), 255, 255) hsv = np.array([[c]])[:, :3].astype(np.uint8) bgr = cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR)[0, 0] bgr = tuple(bgr.astype(np.uint8).tolist()) if hue is not None: if obj.id not in self.colored: self.colored[obj.id] = [] self.colored[obj.id].append({'color':color, 'err':error}) if self.valid_color(obj.id): fprint("COLOR IS VALID", msg_color='green') self.make_request(cmd='{name}={bgr[2]},{bgr[1]},{bgr[0]},{_id}'.format(name=obj.name,_id= obj.id, bgr=bgr)) [cv2.circle(self.debug.image, tuple(map(int, p)), 2, bgr, -1) for p in points_px] cv2.circle(self.debug.image, tuple(object_px), 10, bgr, -1) font = cv2.FONT_HERSHEY_SIMPLEX cv2.putText(self.debug.image, str(obj.id), tuple(object_px), font, 1, bgr, 2) print '\n' * 2 def _get_ROI_from_points(self, image_points): # Probably will return a set of contours cnt = np.array(image_points).astype(np.float32) rect = cv2.minAreaRect(cnt) box = cv2.cv.BoxPoints(rect) box = np.int0(box) fprint("Drawing rectangle") cv2.drawContours(self.debug.image, [box], 0, (0, 0, 255), 2) return box def _get_color_from_ROI(self, roi, img): mask = np.zeros(img.shape[:2], np.uint8) cv2.drawContours(mask, [roi], 0, 255, -1) bgr = cv2.mean(img, mask) # We have to treat that bgr value as a 3d array to get cvtColor to work bgr = np.array([[bgr]])[:, :3].astype(np.uint8) h, s, v = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)[0, 0] # Now check that s and v are in a good range if s < self.saturation_reject or v < self.value_reject: fprint("The colors aren't expressive enough. Rejecting.", msg_color='red') return None # Compute hue error in SO2 hue_angle = np.radians(h * 2) c = np.cos(hue_angle) s = np.sin(hue_angle) error = np.inf likely_color = 'bazinga' for color, h_val in self.color_map.iteritems(): cg = np.cos(h_val) sg = np.sin(h_val) this_error = np.abs(np.arctan2(sg*c - cg*s, cg*c + sg*s)) if this_error < error: error = this_error likely_color = color if error > self.hue_error_reject: fprint("Closest color was {} with an error of {} rads (> {}) Rejecting.".format(likely_color, np.round(error, 3), self.hue_error_reject), msg_color='red') return None fprint("Likely color: {} with an hue error of {} rads.".format(likely_color, np.round(error, 3))) return [np.degrees(self.color_map[likely_color]) / 2.0, likely_color, error] def _object_in_frame(self, object_point): """ Returns if the object is in frame given the centroid of the object. `object_point` should be in the camera_model's frame. """ print object_point if object_point[2] < 0: return False px = np.array(self.camera_model.project3dToPixel(object_point)) resoultion = self.camera_model.fullResolution() if np.any([0,0] > px) or np.any(px > resoultion): return False return True
class PathMarkerFinder(): """ Node which finds orange path markers in image frame, estimates the 2d and 3d position/orientation of marker and returns this estimate when service is called. Unit tests for this node is in test_path_marker.py Finding the marker works as follows: * blur image * threshold image mostly for highly saturated, orange/yellow/red objects * run canny edge detection on thresholded image * find contours on edge frame * filter contours to find those that may be contours by: * checking # of sides in appox polygon * checking ratio of length/width close to known model * estimates 3D pose using cv2.solvePnP with known object demensions and camera model TODO: Implement Kalman filter to smooth pose estimate / eliminate outliers """ # Model of four corners of path marker, centered around 0 in meters PATH_MARKER = np.array([[0.6096, -0.0762, 0], [0.6096, 0.0762, 0], [-0.6096, 0.0762, 0], [-0.6096, -0.0762, 0]], dtype=np.float) # Coordinate axes for debugging image REFERENCE_POINTS = np.array( [[0, 0, 0], [0.3, 0, 0], [0, 0.3, 0], [0, 0, 0.3]], dtype=np.float) def __init__(self): self.debug_gui = False self.last2d = None self.last3d = None self.enabled = False self.cam = None self.last_image = None self.last_found_time = None # Constants from launch config file self.debug_ros = rospy.get_param("debug_ros", True) self.canny_low = rospy.get_param("canny_low", 100) self.canny_ratio = rospy.get_param("canny_ratio", 3.0) self.thresh_hue_high = rospy.get_param("thresh_hue_high", 60) self.thresh_saturation_low = rospy.get_param("thresh_satuation_low", 100) self.min_contour_area = rospy.get_param("min_contour_area", 100) self.length_width_ratio_err = rospy.get_param("length_width_ratio_err", 0.2) self.approx_polygon_thresh = rospy.get_param("approx_polygon_thresh", 10) camera = rospy.get_param("marker_camera", "/camera/down/left/image_rect_color") if self.debug_ros: self.debug_pub = Image_Publisher("debug_image") self.markerPub = rospy.Publisher('path_marker_visualization', Marker, queue_size=10) self.service2D = rospy.Service('/vision/path_marker/2D', VisionRequest2D, self.cb_2d) self.service3D = rospy.Service('/vision/path_marker/pose', VisionRequest, self.cb_3d) self.toggle = rospy.Service('/vision/path_marker/enable', SetBool, self.enable_cb) self.image_sub = Image_Subscriber(camera, self.img_cb) camera_info = self.image_sub.wait_for_camera_info() self.cam = PinholeCameraModel() self.cam.fromCameraInfo(camera_info) def sendDebugMarker(self): ''' Sends a rviz marker in the camera frame with the estimated pose of the path marker. This marker is a scaled cube with the demensions and color of the actual marker. Only called if debug_ros param == True ''' m = Marker() m.header.frame_id = self.cam.tfFrame() m.header.stamp = self.last_found_time m.ns = "path_markers" m.id = 0 m.type = 1 m.action = 0 m.pose.position = numpy_to_point(self.last3d[0]) m.pose.orientation = numpy_to_quaternion(self.last3d[1]) # Real demensions of path marker m.scale.x = 1.2192 m.scale.y = 0.1524 m.scale.z = 0.05 m.color.r = 0.0 m.color.g = 0.5 m.color.b = 0.0 m.color.r = 1.0 m.color.a = 1.0 m.lifetime = rospy.Duration(0) self.markerPub.publish(m) def enable_cb(self, x): self.enabled = x.data return SetBoolResponse(success=True) def sortRect(self, rect): ''' Given a contour of 4 points, returns the same 4 points sorted in a known way. Used so that indicies of contour line up to that in model for cv2.solvePnp p[0] = Top left corner of marker p[1] = Top right corner of marker p[2] = Bottom right corner of marker p[3] = Bottom left cornet of marker ''' def sort_contour_y(c): return c[0][1] def sort_contour_x(c): return c[0][0] sorted_y = np.array(sorted(rect, key=sort_contour_y)) sorted_x = np.array(sorted(rect, key=sort_contour_x)) sorted_rect = None if (np.linalg.norm(sorted_y[0] - sorted_y[1]) > np.linalg.norm(sorted_y[0] - sorted_y[2]) or np.linalg.norm(sorted_y[0] - sorted_y[1]) > np.linalg.norm(sorted_y[0] - sorted_y[3])): if sorted_x[1][0][1] > sorted_x[0][0][1]: sorted_x[0], sorted_x[1] = sorted_x[1].copy( ), sorted_x[0].copy() if sorted_x[2][0][1] > sorted_x[3][0][1]: sorted_x[2], sorted_x[3] = sorted_x[3].copy( ), sorted_x[2].copy() sorted_rect = sorted_x else: if sorted_y[0][0][0] > sorted_y[1][0][0]: sorted_y[0], sorted_y[1] = sorted_y[1].copy( ), sorted_y[0].copy() if sorted_y[3][0][0] > sorted_y[2][0][0]: sorted_y[3], sorted_y[2] = sorted_y[2].copy( ), sorted_y[3].copy() sorted_rect = sorted_y return sorted_rect def cb_3d(self, req): res = VisionRequestResponse() if (self.last2d == None or not self.enabled): res.found = False else: res.pose.header.frame_id = self.cam.tfFrame() res.pose.header.stamp = self.last_found_time res.pose.pose.position = numpy_to_point(self.last3d[0]) res.pose.pose.orientation = numpy_to_quaternion(self.last3d[1]) res.found = True return res def cb_2d(self, req): res = VisionRequest2DResponse() if (self.last3d == None or not self.enabled): res.found = False else: res.header.frame_id = self.cam.tfFrame() res.header.stamp = self.last_found_time res.pose.x = self.last2d[0][0] res.pose.y = self.last2d[0][1] res.pose.theta = self.last2d[1] res.found = True return res def get_3d_pose(self, p): i_points = np.array((p[0][0], p[1][0], p[2][0], p[3][0]), dtype=np.float) retval, rvec, tvec = cv2.solvePnP(PathMarkerFinder.PATH_MARKER, i_points, self.cam.intrinsicMatrix(), np.zeros((5, 1))) if tvec[2] < 0.3: return False self.last3d = (tvec.copy(), quaternion_from_euler(0.0, 0.0, self.last2d[1])) if self.debug_ros: refs, _ = cv2.projectPoints(PathMarkerFinder.REFERENCE_POINTS, rvec, tvec, self.cam.intrinsicMatrix(), np.zeros((5, 1))) refs = np.array(refs, dtype=np.int) cv2.line(self.last_image, (refs[0][0][0], refs[0][0][1]), (refs[1][0][0], refs[1][0][1]), (0, 0, 255)) # X axis refs cv2.line(self.last_image, (refs[0][0][0], refs[0][0][1]), (refs[2][0][0], refs[2][0][1]), (0, 255, 0)) # Y axis ref cv2.line(self.last_image, (refs[0][0][0], refs[0][0][1]), (refs[3][0][0], refs[3][0][1]), (255, 0, 0)) # Z axis ref return True def get_2d_pose(self, r): ''' Given a sorted 4 sided polygon, stores the centriod and angle for the next service call to get 2dpose. returns False if dx of polygon is 0, otherwise True ''' top_center = r[0][0] + (r[1][0] - r[0][0]) / 2.0 bot_center = r[2][0] + (r[3][0] - r[2][0]) / 2.0 center = bot_center + (top_center - bot_center) / 2.0 y = top_center[1] - bot_center[1] x = top_center[0] - bot_center[0] if x == 0: rospy.logerr("Contour dx is 0, strange...") return False angle = np.arctan(y / x) self.last2d = (center, angle) return True def valid_contour(self, contour): ''' Does various tests to filter out contours that are clearly not a valid path marker. * run approx polygon, check that sides == 4 * find ratio of length to width, check close to known ratio IRL ''' if cv2.contourArea(contour) < self.min_contour_area: return False # Checks that contour is 4 sided polygon = cv2.approxPolyDP(contour, self.approx_polygon_thresh, True) if len(polygon) != 4: return False rect = self.sortRect(polygon) for idx, p in enumerate(rect): cv2.putText(self.last_image, str(idx), (p[0][0], p[0][1]), cv2.FONT_HERSHEY_SCRIPT_COMPLEX, 1, (0, 0, 255)) length = np.linalg.norm(rect[0][0] - rect[3][0]) width = np.linalg.norm(rect[0][0] - rect[1][0]) if width == 0: rospy.logerr("Width == 0, strange...") return False length_width_ratio = length / width # Checks that ratio of length to width is similar to known demensions (8:1) if abs(length_width_ratio - 8.0) / 8.0 > self.length_width_ratio_err: return False if not self.get_2d_pose(rect): return False if not self.get_3d_pose(rect): return False return True def get_edges(self): ''' Proccesses latest image to find edges by: blurring and thresholding for highly saturated orangish objects then runs canny on threshold images and returns canny's edges ''' blur = cv2.blur(self.last_image, (5, 5)) hsv = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV) thresh = cv2.inRange(hsv, (0, self.thresh_saturation_low, 0), (self.thresh_hue_high, 255, 255)) return cv2.Canny(thresh, self.canny_low, self.canny_low * self.canny_ratio) def img_cb(self, img): if not self.enabled or self.cam == None: return self.last_image = img edges = self.get_edges() _, contours, _ = cv2.findContours(edges, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) # Check if each contour is valid for idx, c in enumerate(contours): if self.valid_contour(c): rospy.loginfo("Found path marker") self.last_found_time = self.image_sub.last_image_time if self.debug_ros: self.sendDebugMarker() cv2.drawContours(self.last_image, contours, idx, (0, 255, 0), 3) break else: if self.debug_ros: cv2.drawContours(self.last_image, contours, idx, (255, 0, 0), 3) if self.debug_ros: self.debug_pub.publish(self.last_image) if self.debug_gui: cv2.imshow("debug", self.last_image) cv2.waitKey(5)
def callback_pos(self, msg): if self.detector_switch == 'on': x_c = 0.0 y_c = 0.0 z_c = 0.0 x_org = 0.0 y_org = 0.0 z_org = 0.0 if self.img_detection_fnc: # original drone location: x_org = msg.position.x y_org = msg.position.y z_org = msg.position.z self.current_x = x_org self.current_y = y_org self.current_z = z_org # need to transform back to the Eular angle orientation_list = [ msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w ] self.current_yaw = round( self.RAD_to_DEG * euler_from_quaternion(orientation_list)[2]) # camera location relative to drone: x_c = 0.0 y_c = 0.0 z_c = 0.0 else: pass if self.pixel_center != []: pose = Pose() point_msg = PoseStamped() # create vectors to scale the normalized coordinates in camera frame t_1 = np.array([[x_org, y_org, z_org]]) t_2 = np.array([[x_c, y_c, z_c]]) # translation matrix from world to camera t_W2C = np.add(t_1, t_2) # transformation from image coordinate to camera coordinate cam_info = rospy.wait_for_message( "/firefly/vi_sensor/camera_depth/camera/camera_info", CameraInfo, timeout=None) img_proc = PinholeCameraModel() img_proc.fromCameraInfo(cam_info) cam_model_point = img_proc.projectPixelTo3dRay( self.pixel_center) cam_model_point = np.round(np.multiply( np.divide(cam_model_point, cam_model_point[2]), t_W2C[0, 2]), decimals=0) # creating a place holder to save the location info point_msg.pose.position.x = cam_model_point[0] point_msg.pose.position.y = cam_model_point[1] point_msg.pose.position.z = cam_model_point[2] point_msg.pose.orientation.x = 0 point_msg.pose.orientation.y = 0 point_msg.pose.orientation.z = 0 point_msg.pose.orientation.w = 1 point_msg.header.stamp = rospy.Time.now() point_msg.header.frame_id = img_proc.tfFrame() # initiate the coordinate (frame) transformation self.tf_listener.waitForTransform(img_proc.tfFrame(), "world", rospy.Time.now(), rospy.Duration(1.0)) # calculate the transformed coordinates tf_point = self.tf_listener.transformPose("world", point_msg) x_new = 0.0 # round(tf_point.pose.position.x, 2) y_new = 0.0 # round(tf_point.pose.position.y, 2) z_new = msg.position.z - 0.01 # z_new = (1 - 0.01 * self.decent_rate) * msg.position.z pose.position.x = x_new pose.position.y = y_new pose.position.z = z_new pose.orientation.x = msg.orientation.x pose.orientation.y = msg.orientation.y pose.orientation.z = msg.orientation.z pose.orientation.w = msg.orientation.w self.pos_pub.publish(pose) else: # If there is no landing mark detected pose = Pose() # original drone location: x_org = msg.position.x y_org = msg.position.y z_org = msg.position.z self.current_x = x_org self.current_y = y_org self.current_z = z_org # need to transform back to the Eular angle orientation_list = [ msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w ] self.current_yaw = round( self.RAD_to_DEG * euler_from_quaternion(orientation_list)[2]) pose.position.x = msg.position.x pose.position.y = msg.position.y pose.position.z = msg.position.z pose.orientation.x = msg.orientation.x pose.orientation.y = msg.orientation.y pose.orientation.z = msg.orientation.z pose.orientation.w = msg.orientation.w self.pos_pub.publish(pose) else: # if the detection switch doesn't turn on, skip the coordinate computation pass
class OrangeRectangleFinder(): """ Node which finds orange rectangular objects in image frame. This can be used for the path marker challenge and to detect the lid of the bins challenge. The node estimates the 2d and 3d position/orientation of this object and returns this estimate when service is called. Unit tests for this node is in test_path_marker.py Finding the marker works as follows: * blur image * threshold image mostly for highly saturated, orange/yellow/red objects * run canny edge detection on thresholded image * find contours on edge frame * filter contours to find those that may be contours by: * checking # of sides in appox polygon * checking ratio of length/width close to known model * estimates 3D pose using cv2.solvePnP with known object demensions and camera model * Use translation vector from PnP and direction vector from 2d contour for pose * Transform this frames pose into /map frame * Plug this frames pose in /map into a kalman filter to reduce noise TODO: Allow for two objects to be identifed at once, both filtered through its own KF """ # Coordinate axes for debugging image REFERENCE_POINTS = np.array( [[0, 0, 0], [0.3, 0, 0], [0, 0.3, 0], [0, 0, 0.3]], dtype=np.float) def __init__(self): self.debug_gui = False self.enabled = False self.cam = None # Constants from launch config file self.debug_ros = rospy.get_param("~debug_ros", True) self.canny_low = rospy.get_param("~canny_low", 100) self.canny_ratio = rospy.get_param("~canny_ratio", 3.0) self.thresh_hue_high = rospy.get_param("~thresh_hue_high", 60) self.thresh_saturation_low = rospy.get_param("~thresh_satuation_low", 100) self.min_contour_area = rospy.get_param("~min_contour_area", 100) self.epsilon_range = rospy.get_param("~epsilon_range", (0.01, 0.1)) self.epsilon_step = rospy.get_param("~epsilon_step", 0.01) self.shape_match_thresh = rospy.get_param("~shape_match_thresh", 0.4) self.min_found_count = rospy.get_param("~min_found_count", 10) self.timeout_seconds = rospy.get_param("~timeout_seconds", 2.0) # Default to scale model of path marker. Please use set_geometry service # to set to correct model of object. length = rospy.get_param("~length", 1.2192) width = rospy.get_param("~width", 0.1524) self.rect_model = RectFinder(length, width) self.do_3D = rospy.get_param("~do_3D", True) camera = rospy.get_param("~image_topic", "/camera/down/left/image_rect_color") self.tf_listener = tf.TransformListener() # Create kalman filter to track 3d position and direction vector for marker in /map frame self.state_size = 5 # X, Y, Z, DY, DX self.filter = cv2.KalmanFilter(self.state_size, self.state_size) self.filter.transitionMatrix = 1. * np.eye(self.state_size, dtype=np.float32) self.filter.measurementMatrix = 1. * np.eye(self.state_size, dtype=np.float32) self.filter.processNoiseCov = 1e-5 * np.eye(self.state_size, dtype=np.float32) self.filter.measurementNoiseCov = 1e-4 * np.eye(self.state_size, dtype=np.float32) self.filter.errorCovPost = 1. * np.eye(self.state_size, dtype=np.float32) self.reset() self.service_set_geometry = rospy.Service('~set_geometry', SetGeometry, self._set_geometry_cb) if self.debug_ros: self.debug_pub = Image_Publisher("~debug_image") self.markerPub = rospy.Publisher('~marker', Marker, queue_size=10) self.service2D = rospy.Service('~2D', VisionRequest2D, self._vision_cb_2D) if self.do_3D: self.service3D = rospy.Service('~pose', VisionRequest, self._vision_cb_3D) self.toggle = rospy.Service('~enable', SetBool, self._enable_cb) self.image_sub = Image_Subscriber(camera, self._img_cb) self.camera_info = self.image_sub.wait_for_camera_info() assert self.camera_info is not None self.cam = PinholeCameraModel() self.cam.fromCameraInfo(self.camera_info) def _set_geometry_cb(self, req): self.rect_model = RectFinder.from_polygon(req.model) self.reset() rospy.loginfo("Resetting rectangle model to LENGTH=%f, WIDTH=%f", self.rect_model.length, self.rect_model.width) return {'success': True} def _send_debug_marker(self): ''' Sends a rviz marker in the camera frame with the estimated pose of the object. This marker is a scaled cube with the demensions of the model. Only called if debug_ros param == True ''' if self.last3d is None or not self.found: return m = Marker() m.header.frame_id = '/map' m.header.stamp = self.last_found_time_3D m.ns = "orange_rectangle" m.id = 0 m.type = 1 m.action = 0 # Real demensions of path marker m.scale.x = self.rect_model.length m.scale.y = self.rect_model.width m.scale.z = 0.05 m.pose.position = numpy_to_point(self.last3d[0]) m.pose.orientation = numpy_to_quaternion(self.last3d[1]) m.color.r = 0.0 m.color.g = 0.5 m.color.b = 0.0 m.color.r = 1.0 m.color.a = 1.0 m.lifetime = rospy.Duration(self.timeout_seconds) self.markerPub.publish(m) def _enable_cb(self, x): if x.data != self.enabled: self.reset() self.tf_listener.clear() self.enabled = x.data return SetBoolResponse(success=True) def _vision_cb_3D(self, req): res = VisionRequestResponse() if self.last_found_time_3D is None or self.image_sub.last_image_time is None: res.found = False return res dt = (self.image_sub.last_image_time - self.last_found_time_3D).to_sec() if dt < 0 or dt > self.timeout_seconds: res.found = False elif (self.last3d is None or not self.enabled): res.found = False else: res.pose.header.frame_id = "/map" res.pose.header.stamp = self.last_found_time_3D res.pose.pose.position = numpy_to_point(self.last3d[0]) res.pose.pose.orientation = numpy_to_quaternion(self.last3d[1]) res.found = True return res def _vision_cb_2D(self, req): res = VisionRequest2DResponse() if (self.last2d is None or not self.enabled): res.found = False else: res.header.frame_id = self.cam.tfFrame() res.header.stamp = self.last_found_time_2D res.pose.x = self.last2d[0][0] res.pose.y = self.last2d[0][1] res.camera_info = self.camera_info res.max_x = self.camera_info.width res.max_y = self.camera_info.height if self.last2d[1][0] < 0: self.last2d[1][0] = -self.last2d[1][0] self.last2d[1][1] = -self.last2d[1][1] angle = np.arctan2(self.last2d[1][1], self.last2d[1][0]) res.pose.theta = angle res.found = True return res def reset(self): self.last_found_time_2D = None self.last_found_time_3D = None self.last2d = None self.last3d = None self._clear_filter(None) def _clear_filter(self, state): ''' Reset filter and found state. This will ensure that the object is seen consistently before vision request returns true ''' rospy.loginfo("MARKER LOST") self.found_count = 0 self.found = False self.last3d = None self.filter.errorCovPre = 1. * np.eye(self.state_size, dtype=np.float32) if state is not None: self.found_count = 1 state = np.array(state, dtype=np.float32) self.filter.statePost = state def _update_kf(self, (x, y, z, dy, dx)): ''' Updates the kalman filter using the pose estimation from the most recent frame. Also tracks time since last seen and how often is has been seen to set the boolean "found" for the vision request ''' if self.last_found_time_3D is None: # First time found, set initial KF pose to this frame self._clear_filter((x, y, z, dy, dx)) self.last_found_time_3D = self.image_sub.last_image_time return dt = (self.image_sub.last_image_time - self.last_found_time_3D).to_sec() self.last_found_time_3D = self.image_sub.last_image_time if dt < 0 or dt > self.timeout_seconds: rospy.logwarn( "Timed out since last saw marker, resetting. DT={}".format(dt)) self._clear_filter((x, y, z, dy, dx)) return self.found_count += 1 measurement = 1. * np.array([x, y, z, dy, dx], dtype=np.float32) self.filter.predict() estimated = self.filter.correct(measurement) if self.found_count > self.min_found_count: angle = np.arctan2(estimated[3], estimated[4]) self.last3d = ((estimated[0], estimated[1], estimated[2]), quaternion_from_euler(0.0, 0.0, angle)) if not self.found: rospy.loginfo("Marker Found") self.found = True