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