def __init__(self, my_tf, debug, nh): """Initialize ScanTheCodePerception class.""" self.image_cache = deque() self.bridge = CvBridge() self.nh = nh self.last_cam_info = None self.debug = debug self.pers_points = [] self.model_tracker = ScanTheCodeModelTracker() self.pinhole_cam = PinholeCameraModel() self.my_tf = my_tf # %%%%%%%%%%%%debug%%%%%%%%%%%%%%%% self.count = 0 self.depths = []
def __init__(self, my_tf, debug, nh): """Initialize ScanTheCodePerception class.""" self.image_cache = deque() self.bridge = CvBridge() self.nh = nh self.last_cam_info = None self.debug = debug self.pers_points = [] self.model_tracker = ScanTheCodeModelTracker() self.camera_model = PinholeCameraModel() self.my_tf = my_tf self.rect_finder = RectangleFinderClustering() self.color_finder = ColorFinder() self.count = 0 self.depths = []
class ScanTheCodePerception(object): """Class that handles ScanTheCodePerception.""" def __init__(self, my_tf, debug, nh): """Initialize ScanTheCodePerception class.""" self.image_cache = deque() self.bridge = CvBridge() self.nh = nh self.last_cam_info = None self.debug = debug self.pers_points = [] self.model_tracker = ScanTheCodeModelTracker() self.pinhole_cam = PinholeCameraModel() self.my_tf = my_tf # %%%%%%%%%%%%debug%%%%%%%%%%%%%%%% self.count = 0 self.depths = [] def add_image(self, image): """Add an image to the image cache.""" if len(self.image_cache) > 20: self.image_cache.popleft() self.image_cache.append(image) def update_info(self, info): """Update the camera calibration info.""" self.last_cam_info = info def _convert_3d_2d(self, point): K = self.last_cam_info.K K = np.array(K).reshape(3, 3) pl = point pl = K.dot(pl) if pl[2] == 0: return (0, 0) pl[0] /= pl[2] pl[1] /= pl[2] if pl[0] < 0: pl[0] = 0 if pl[1] < 0: pl[1] = 0 return (int(pl[0]), int(pl[1])) @txros.util.cancellableInlineCallbacks def _get_3d_points_stereo(self, points_3d_enu, time): self.pers_points.extend(points_3d_enu) max_num = 1000 if len(self.pers_points) > max_num: self.pers_points = self.pers_points[len(self.pers_points) - max_num:len(self.pers_points)] points_3d = [] try: print "waiting on tf" trans = yield self.my_tf.get_transform("/stereo_left_cam", "/enu", time) except Exception as exp: print exp defer.returnValue(points_3d) transformation = trans.as_matrix() for point in self.pers_points: p = [point.x, point.y, point.z, 1] t_p = transformation.dot(p) if t_p[3] < 1E-15: raise ZeroDivisionError t_p[0] /= t_p[3] t_p[1] /= t_p[3] t_p[2] /= t_p[3] points_3d.append(t_p[0:3]) defer.returnValue(points_3d) def _get_top_left_point(self, points_3d): xmin = sys.maxint zmin = -sys.maxint ymin = sys.maxint for i, point in enumerate(points_3d): if point[1] < ymin: xmin = point[0] zmin = point[2] ymin = point[1] buff = .07 for i, point in enumerate(points_3d): if (point[1] < ymin + buff and point[1] > ymin - buff and point[0] < xmin): xmin = point[0] zmin = point[2] ymin = point[1] return xmin, ymin, zmin def _get_points_in_range(self, axis, lower, upper, points): in_range = [] idx = 0 if axis == 'y': idx = 1 if axis == 'z': idx = 2 for p in points: if p[idx] > lower and p[idx] < upper: in_range.append(p) return in_range def _get_depth(self, axis, points_3d): idx = 0 if axis == 'y': idx = 1 if axis == 'z': idx = 2 min_val, max_val = sys.maxint, -sys.maxint points_3d = np.array(points_3d) my_points = points_3d[:, idx] mean = np.mean(my_points) std = 1.5 * np.std(my_points) for p in my_points: if abs(p - mean) > std: # print p, mean continue if p < min_val: min_val = p if p > max_val: max_val = p return max_val - min_val def _get_2d_points_stc(self, points_3d): xmin, ymin, zmin = self._get_top_left_point(points_3d) ymin -= .15 xmin -= .05 points_3d = [[xmin, ymin, zmin], [xmin + .8, ymin + .75, zmin], [xmin, ymin + .75, zmin], [xmin + .8, ymin, zmin]] points_2d = [] for i in range(0, len(points_3d)): point = points_3d[i] points_2d.append(self._convert_3d_2d(point)) return points_2d def _get_rectangle(self, img, bounding_rect): xmin, ymin, xmax, ymax = bounding_rect roi = img[ymin:ymax, xmin:xmax] r = RectangleFinder() return r.get_rectangle(roi, self.debug) def _get_bounding_rect(self, points_2d): xmin = 1000 xmax = 0 ymin = 1000 ymax = 0 for i, point in enumerate(points_2d): if (point[0] < xmin): xmin = point[0] if (point[0] > xmax): xmax = point[0] if (point[1] > ymax): ymax = point[1] if (point[1] < ymin): ymin = point[1] return xmin, ymin, xmax, ymax def _get_closest_image(self, time): min_img = None min_diff = genpy.Duration(sys.maxint) for img in self.image_cache: diff = abs(time - img.header.stamp) if diff < min_diff: min_diff = diff min_img = img return min_img @txros.util.cancellableInlineCallbacks def search(self, scan_the_code): """Search for the colors in the scan the code object.""" if len(self.image_cache) == 0: defer.returnValue((False, None)) image_ros = self._get_closest_image(scan_the_code.header.stamp) try: image = self.bridge.imgmsg_to_cv2(image_ros, "bgr8") except CvBridgeError: print "Trouble converting image" defer.returnValue((False, None)) image_clone = image.copy() points_3d = yield self._get_3d_points_stereo(scan_the_code.points, image_ros.header.stamp) for i in points_3d: p = self._convert_3d_2d(i) po = (int(p[0]), int(p[1])) cv2.circle(image_clone, po, 2, (0, 255, 0), -1) points_2d = self._get_2d_points_stc(points_3d) self.debug.add_image(image_clone, "bounding_box", topic="bounding_box") xmin, ymin, xmax, ymax = self._get_bounding_rect(points_2d) xmin, ymin, xmax, ymax = int(xmin), int(ymin), int(xmax), int(ymax) cv2.rectangle(image_clone, (xmin, ymin), (xmax, ymax), (255, 0, 0), 2) h, w, r = image.shape print ymin, ymax, xmin, xmax if ymin == ymax or xmin == xmax or ymin < 0 or ymax > h or xmin < 0 or xmax > w: defer.returnValue((False, None)) cv2.rectangle(image_clone, (xmin, ymin), (xmax, ymax), (255, 0, 0), 2) self.debug.add_image(image_clone, "bounding_box", topic="bounding_box") succ, rect = self._get_rectangle(image, (xmin, ymin, xmax, ymax)) if not succ: defer.returnValue((False, None)) pnts = [] for p in rect: p1 = p[0] + xmin p2 = p[1] + ymin pnts.append((int(p1), int(p2))) self.mission_complete = self.model_tracker.update_model( image, pnts, self.debug) if (self.mission_complete): print "MISSION COMPLETE" defer.returnValue((True, self.model_tracker.colors)) else: defer.returnValue((False, None)) yield True @txros.util.cancellableInlineCallbacks def correct_pose(self, scan_the_code): """Check to see if we are looking at the corner of scan the code.""" self.count += 1 # %%%%%%%%%%%%%%%%%%%%%%%%DEBUG if self.count == 100: xs = np.arange(0, len(self.depths)) ys = self.depths # plt.plot(xs, ys) # plt.show() # %%%%%%%%%%%%%%%%%%%%%%%%DEBUG points_3d = yield self._get_3d_points_stereo(scan_the_code.points, self.nh.get_time()) xmin, ymin, zmin = self._get_top_left_point(points_3d) points_oi = self._get_points_in_range('y', ymin - .1, ymin + .2, points_3d) if len(points_oi) == 0: defer.returnValue(False) # %%%%%%%%%%%%%%%%%%%%%%%%DEBUG image_ros = self._get_closest_image(scan_the_code.header.stamp) while image_ros is None: image_ros = self._get_closest_image(scan_the_code.header.stamp) # %%%%%%%%%%%%%%%%%%%%%%%%DEBUG image_ros = self.bridge.imgmsg_to_cv2(image_ros, "bgr8").copy() depth = self._get_depth('z', points_oi) print "DEPTH: ", depth self.depths.append(depth) if depth > .3: # %%%%%%%%%%%%%%%%%%%%%%%%DEBUG cv2.putText(image_ros, str(depth), (10, 30), 1, 2, (0, 0, 255)) self.debug.add_image(image_ros, "in_range", topic="in_range") # %%%%%%%%%%%%%%%%%%%%%%%%DEBUG defer.returnValue(False) # %%%%%%%%%%%%%%%%%%%%%%%%DEBUG cv2.putText(image_ros, str(depth), (10, 30), 1, 2, (0, 255, 0)) self.debug.add_image(image_ros, "in_range1", topic="in_range") # %%%%%%%%%%%%%%%%%%%%%%%%DEBUG defer.returnValue(True)
class ScanTheCodePerception(object): """Class that handles ScanTheCodePerception.""" def __init__(self, my_tf, debug, nh): """Initialize ScanTheCodePerception class.""" self.image_cache = deque() self.bridge = CvBridge() self.nh = nh self.last_cam_info = None self.debug = debug self.pers_points = [] self.model_tracker = ScanTheCodeModelTracker() self.pinhole_cam = PinholeCameraModel() self.my_tf = my_tf # %%%%%%%%%%%%debug%%%%%%%%%%%%%%%% self.count = 0 self.depths = [] def add_image(self, image): """Add an image to the image cache.""" if len(self.image_cache) > 20: self.image_cache.popleft() self.image_cache.append(image) def update_info(self, info): """Update the camera calibration info.""" self.last_cam_info = info def _convert_3d_2d(self, point): K = self.last_cam_info.K K = np.array(K).reshape(3, 3) pl = point pl = K.dot(pl) if pl[2] == 0: return (0, 0) pl[0] /= pl[2] pl[1] /= pl[2] if pl[0] < 0: pl[0] = 0 if pl[1] < 0: pl[1] = 0 return(int(pl[0]), int(pl[1])) @txros.util.cancellableInlineCallbacks def _get_3d_points_stereo(self, points_3d_enu, time): self.pers_points.extend(points_3d_enu) max_num = 1000 if len(self.pers_points) > max_num: self.pers_points = self.pers_points[len(self.pers_points) - max_num:len(self.pers_points)] points_3d = [] try: trans = yield self.my_tf.get_transform("/stereo_right_cam", "/enu", time) except Exception as exp: print exp defer.returnValue(points_3d) transformation = trans.as_matrix() for point in self.pers_points: p = [point.x, point.y, point.z, 1] t_p = transformation.dot(p) if t_p[3] < 1E-15: raise ZeroDivisionError t_p[0] /= t_p[3] t_p[1] /= t_p[3] t_p[2] /= t_p[3] points_3d.append(t_p[0:3]) defer.returnValue(points_3d) def _get_top_left_point(self, points_3d): xmin = sys.maxint zmin = -sys.maxint ymin = sys.maxint for i, point in enumerate(points_3d): if point[1] < ymin: xmin = point[0] zmin = point[2] ymin = point[1] buff = .07 for i, point in enumerate(points_3d): if(point[1] < ymin + buff and point[1] > ymin - buff and point[0] < xmin): xmin = point[0] zmin = point[2] ymin = point[1] return xmin, ymin, zmin def _get_points_in_range(self, axis, lower, upper, points): in_range = [] idx = 0 if axis == 'y': idx = 1 if axis == 'z': idx = 2 for p in points: if p[idx] > lower and p[idx] < upper: in_range.append(p) return in_range def _get_depth(self, axis, points_3d): idx = 0 if axis == 'y': idx = 1 if axis == 'z': idx = 2 min_val, max_val = sys.maxint, -sys.maxint points_3d = np.array(points_3d) my_points = points_3d[:, idx] mean = np.mean(my_points) std = 1.5 * np.std(my_points) for p in my_points: if abs(p - mean) > std: # print p, mean continue if p < min_val: min_val = p if p > max_val: max_val = p return max_val - min_val def _get_2d_points_stc(self, points_3d): xmin, ymin, zmin = self._get_top_left_point(points_3d) ymin -= .15 xmin -= .05 points_3d = [[xmin, ymin, zmin], [xmin + .8, ymin + .75, zmin], [xmin, ymin + .75, zmin], [xmin + .8, ymin, zmin]] points_2d = [] for i in range(0, len(points_3d)): point = points_3d[i] points_2d.append(self._convert_3d_2d(point)) return points_2d def _get_rectangle(self, img, bounding_rect): xmin, ymin, xmax, ymax = bounding_rect roi = img[ymin:ymax, xmin:xmax] r = RectangleFinder() return r.get_rectangle(roi, self.debug) def _get_bounding_rect(self, points_2d): xmin = 1000 xmax = 0 ymin = 1000 ymax = 0 for i, point in enumerate(points_2d): if(point[0] < xmin): xmin = point[0] if(point[0] > xmax): xmax = point[0] if(point[1] > ymax): ymax = point[1] if(point[1] < ymin): ymin = point[1] return xmin, ymin, xmax, ymax def _get_closest_image(self, time): min_img = None min_diff = genpy.Duration(sys.maxint) for img in self.image_cache: diff = abs(time - img.header.stamp) if diff < min_diff: min_diff = diff min_img = img return min_img @txros.util.cancellableInlineCallbacks def search(self, scan_the_code): """Search for the colors in the scan the code object.""" if len(self.image_cache) == 0: defer.returnValue((False, None)) image_ros = self._get_closest_image(scan_the_code.header.stamp) try: image = self.bridge.imgmsg_to_cv2(image_ros, "bgr8") except CvBridgeError: print "Trouble converting image" defer.returnValue((False, None)) image_clone = image.copy() points_3d = yield self._get_3d_points_stereo(scan_the_code.points, image_ros.header.stamp) for i in points_3d: p = self._convert_3d_2d(i) po = (int(p[0]), int(p[1])) cv2.circle(image_clone, po, 2, (0, 255, 0), -1) points_2d = self._get_2d_points_stc(points_3d) self.debug.add_image(image_clone, "bounding_box", topic="bounding_box") xmin, ymin, xmax, ymax = self._get_bounding_rect(points_2d) xmin, ymin, xmax, ymax = int(xmin), int(ymin), int(xmax), int(ymax) cv2.rectangle(image_clone, (xmin, ymin), (xmax, ymax), (255, 0, 0), 2) h, w, r = image.shape if ymin == ymax or xmin == xmax or ymin < 0 or ymax > h or xmin < 0 or xmax > w: defer.returnValue((False, None)) cv2.rectangle(image_clone, (xmin, ymin), (xmax, ymax), (255, 0, 0), 2) self.debug.add_image(image_clone, "bounding_box", topic="bounding_box") succ, rect = self._get_rectangle(image, (xmin, ymin, xmax, ymax)) if not succ: defer.returnValue((False, None)) pnts = [] for p in rect: p1 = p[0] + xmin p2 = p[1] + ymin pnts.append((int(p1), int(p2))) self.mission_complete = self.model_tracker.update_model(image, pnts, self.debug) if(self.mission_complete): print "MISSION COMPLETE" defer.returnValue((True, self.model_tracker.colors)) else: defer.returnValue((False, None)) yield True @txros.util.cancellableInlineCallbacks def correct_pose(self, scan_the_code): """Check to see if we are looking at the corner of scan the code.""" self.count += 1 # %%%%%%%%%%%%%%%%%%%%%%%%DEBUG # if self.count == 100: # xs = np.arange(0, len(self.depths)) # ys = self.depths # plt.plot(xs, ys) # plt.show() # %%%%%%%%%%%%%%%%%%%%%%%%DEBUG points_3d = yield self._get_3d_points_stereo(scan_the_code.points, self.nh.get_time()) xmin, ymin, zmin = self._get_top_left_point(points_3d) points_oi = self._get_points_in_range('y', ymin - .1, ymin + .2, points_3d) if len(points_oi) == 0: defer.returnValue(False) # %%%%%%%%%%%%%%%%%%%%%%%%DEBUG image_ros = self._get_closest_image(scan_the_code.header.stamp) count = 0 while image_ros is None: yield self.nh.sleep(.5) image_ros = self._get_closest_image(scan_the_code.header.stamp) yield self.nh.sleep(.3) count += 1 if count == 20: defer.returnValue(False) # %%%%%%%%%%%%%%%%%%%%%%%%DEBUG image_ros = self.bridge.imgmsg_to_cv2(image_ros, "bgr8").copy() depth = self._get_depth('z', points_oi) fprint("DEPTH: {}".format(depth), msg_color="green") self.depths.append(depth) if depth > .15: # %%%%%%%%%%%%%%%%%%%%%%%%DEBUG cv2.putText(image_ros, str(depth), (10, 30), 1, 2, (0, 0, 255)) self.debug.add_image(image_ros, "in_range", topic="in_range") # %%%%%%%%%%%%%%%%%%%%%%%%DEBUG defer.returnValue(False) # %%%%%%%%%%%%%%%%%%%%%%%%DEBUG cv2.putText(image_ros, str(depth), (10, 30), 1, 2, (0, 255, 0)) self.debug.add_image(image_ros, "in_range1", topic="in_range") # %%%%%%%%%%%%%%%%%%%%%%%%DEBUG defer.returnValue(True)