def test_monocular(self):
        ci = sensor_msgs.msg.CameraInfo()
        ci.width = 640
        ci.height = 480
        print ci
        cam = PinholeCameraModel()
        cam.fromCameraInfo(ci)
        #print cam.rectifyPoint((0, 0))

        print cam.project3dToPixel((0,0,0))
class ConverterToUV():
    def __init__(self, info_topic):
        self.camera_model = PinholeCameraModel()
        rospy.Subscriber(info_topic, CameraInfo, self.update_model)

    def update_model(self, info):
        self.camera_model.fromCameraInfo(info)
Exemple #3
1
    def handle_img_msg(self, img_msg):
        now = rospy.get_rostime()
        img = None
        bridge = cv_bridge.CvBridge()
        try:
            img = bridge.imgmsg_to_cv2(img_msg, 'bgr8')
        except cv_bridge.CvBridgeError as e:
            rospy.logerr( 'image message to cv conversion failed :' )
            rospy.logerr( e )
            print( e )
            return
   
        self.frame_index = self.timestamp_map[img_msg.header.stamp.to_nsec()]
        f = self.frame_map[self.frame_index][0]
        obs_centroid = np.array(f.trans) + np.array(self.offset)
        R = tf.transformations.quaternion_matrix(self.rotation_offset)
        rotated_centroid = R.dot(list(obs_centroid)+[1])
        obs_centroid = rotated_centroid[:3]
        #self.orient = list(f.rotq)
 
        self.marker.header.stamp = now
        self.marker.pose.position = Point(*list(obs_centroid))
        self.marker.pose.orientation = Quaternion(*self.orient)
        self.obs_marker.pose.position = Point(*list(obs_centroid))
        self.obs_marker.pose.orientation = Quaternion(*self.orient)
        self.add_bbox_lidar()

        md = self.metadata
        dims = np.array([md['l'], md['w'], md['h']])
        outputName = '/image_bbox'
   
        if obs_centroid is None:
            rospy.loginfo("Couldn't find obstacle centroid")
            imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
            return
        
        # print centroid info
        rospy.loginfo(str(obs_centroid))

        # case when obstacle is not in camera frame
        if obs_centroid[0]<2.5 :
            self.imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
            return
    
        # get bbox 
        R = tf.transformations.quaternion_matrix(self.orient)
        corners = [0.5*np.array([i,j,k])*dims for i in [-1,1] 
                    for j in [-1,1] for k in [-1,1]]
        corners = [obs_centroid + R.dot(list(c)+[1])[:3] for c in corners]
        projected_pts = []
        cameraModel = PinholeCameraModel()
        cam_info = load_cam_info(self.calib_file)
        cameraModel.fromCameraInfo(cam_info)
        projected_pts = [cameraModel.project3dToPixel(list(pt)+[1]) for pt in corners]
        projected_pts = np.array(projected_pts)
        center = np.mean(projected_pts, axis=0)
        out_img = drawBbox(img, projected_pts)
        self.imgOutput.publish(bridge.cv2_to_imgmsg(out_img, 'bgr8'))
def FindDist(color):

    cont = checkIfColorIsShowed(color)

    if cont == []:
        rospy.sleep(0.5)

        print "None"
        return None
    cnt = cont[0]
    M = cv2.moments(cnt)
    center_x = int(M['m10'] / M['m00'])
    center_y = 400

    imgCntr = (400, 400)
    camInfo = rospy.wait_for_message("camera/camera_info", CameraInfo)
    camera = PinholeCameraModel()
    camera.fromCameraInfo(camInfo)
    cenRay = camera.projectPixelTo3dRay(imgCntr)
    pixRay = camera.projectPixelTo3dRay((center_x, center_y))

    angle = math.acos(
        prod(pixRay, cenRay) / (math.sqrt(prod(pixRay, pixRay))) *
        (math.sqrt(prod(cenRay, cenRay))))

    dg = int(math.degrees(angle))

    if center_x >= imgCntr[0]:
        dg = 360 - dg

    LaserMessage = rospy.wait_for_message("scan", LaserScan)
    return retrunResult(LaserMessage, dg)
Exemple #5
0
def random_point_inside_fov(camera_info,
                            maxdist,
                            mindist=0,
                            Tcamera=np.eye(4)):
    """
  Generates a random XYZ point inside the camera field of view
  @type  camera_info: sensor_msgs.CameraInfo
  @param camera_info: Message with the meta information for a camera
  @type  maxdist:     float
  @param maxdist:     distance from the camera ref frame in the z direction
  @type  mindist:     float
  @param mindist:     distance from the camera ref frame in the z direction
  @type  Tcamera:     np.array
  @param Tcamera:     homogeneous transformation for the camera ref frame
  @rtype: array
  @return: The random XYZ point
  """
    cam_model = PinholeCameraModel()
    cam_model.fromCameraInfo(camera_info)
    z = np.random.uniform(mindist, maxdist)
    delta_x = cam_model.getDeltaX(cam_model.width / 2, z)
    delta_y = cam_model.getDeltaY(cam_model.height / 2, z)
    point = np.array([0, 0, z, 1])
    point[:2] = np.array([delta_x, delta_y
                          ]) * (2 * np.random.random_sample(2) - 1.)
    return np.dot(Tcamera, point)[:3]
 def from_camera_info(CameraConfig, message):
     ''' Load camera intrinsics from ROS camera info topic, initialize
     camera extrinsics with None value. '''
     model = PinholeCameraModel()
     model.fromCameraInfo(message)
     return CameraConfig(model.K, model.D, None,
                         (model.width, model.height), model.P, model.R)
class Helpers:
    def __init__(self, camera_info):
        self.ci = camera_info
        self.pcm = PinholeCameraModel()
        self.pcm.fromCameraInfo(self.ci)
        self._rectify_inited = False
        self._distort_inited = False

    def process_image(self, cv_image_raw, interpolation=cv2.INTER_NEAREST):
        ''' Undistort an image.
            To be more precise, pass interpolation= cv2.INTER_CUBIC
        '''
        if not self._rectify_inited:
            self._init_rectify_maps()

        cv_image_rectified = np.empty_like(cv_image_raw)
        res = cv2.remap(cv_image_raw, self.mapx, self.mapy, interpolation,
                        cv_image_rectified)
        return res

    def _init_rectify_maps(self):
        W = self.pcm.width
        H = self.pcm.height
        mapx = np.ndarray(shape=(H, W, 1), dtype='float32')
        mapy = np.ndarray(shape=(H, W, 1), dtype='float32')
        mapx, mapy = cv2.initUndistortRectifyMap(self.pcm.K, self.pcm.D,
                                                 self.pcm.R, self.pcm.P,
                                                 (W, H), cv2.CV_32FC1, mapx,
                                                 mapy)
        self.mapx = mapx
        self.mapy = mapy
        self._rectify_inited = True
Exemple #8
0
def dpx_to_distance(dx, dy, camera_name, current_ps, offset):
    print("The dx is " + str(dx) + " the dy is " + str(dy) + " and the camera name is " + camera_name)

    big_z_mappings = {'camera1': transform_broadcaster_mapping['camera1'][0][2] - current_ps.pose.position.z, # x-y plane
                      'camera2': transform_broadcaster_mapping['camera2'][0][1] - current_ps.pose.position.y, # x-z plane
                      'camera3': transform_broadcaster_mapping['camera3'][0][0] - current_ps.pose.position.x} # y-z plane

    #print("The frame_id for the current pose is " + current_ps.header.frame_id)
    camera_model = PinholeCameraModel()
    camera_model.fromCameraInfo(camera_info_mapping[camera_name])
    x, y, z = camera_model.projectPixelTo3dRay((dx, dy))
    #print " x : {} , y : {} , z : {}".format(x, y, z)
    x_center, y_center, z_center = camera_model.projectPixelTo3dRay((0, 0))
    big_z = abs(big_z_mappings[camera_name])
    #print("The big_z for " + camera_name + " is " + str(big_z))
    big_x = (x / z) * big_z  # Use law of similar trianges to solve
    big_y = (y / z) * big_z

    big_x_center = (x_center / z_center) * big_z
    big_y_center = (y_center / z_center) * big_z

    #print("The x_center  is " + str(x_center) + " the y_center  is " + str(y_center) + " and the z_center is " + str(
    #    z_center))
    #print(
    #"The x distance is " + str(big_x - big_x_center) + " the y distance is " + str(big_y - big_y_center) + " and the camera name is " + camera_name + "\n")
    if offset:
        return big_x - big_x_center, big_y - big_y_center
    else:
        return big_x, big_y
def broadcast_poses():
    robot = robot_interface.Robot_Interface()
    cam = camera.RGBD()

    not_read = True
    while not_read:
        try:
            cam_info = cam.read_info_data()
            if (not cam_info == None):
                not_read = False
        except:
            rospy.logerr('info not recieved')

    pcm = PCM()
    pcm.fromCameraInfo(cam_info)
    point = (208 + (323 - 208) / 2, 247 + (424 - 247) / 2)
    print(point)
    #(208.076538,247.264099) (323.411957,242.667325) (204.806457,424.053619) (324.232857,434.011618)
    dep_data = robot.get_img_data()[1]
    print(dep_data)
    dep = robot.get_depth(point, dep_data)
    print(dep)
    br = tf.TransformBroadcaster()
    td_points = pcm.projectPixelTo3dRay(point)
    # pose = np.array([td_points[0], td_points[1], 0.001 * dep])
    norm_pose = np.array(td_points)
    norm_pose = norm_pose / norm_pose[2]
    norm_pose = norm_pose * (dep)
    a = tf.transformations.quaternion_from_euler(ai=-2.355, aj=-3.14, ak=0.0)
    b = tf.transformations.quaternion_from_euler(ai=0.0, aj=0.0, ak=1.57)
    base_rot = tf.transformations.quaternion_multiply(a, b)
    br.sendTransform(norm_pose, base_rot, rospy.Time.now(), 'tree',
                     'head_camera_rgb_optical_frame')
def get_camera_model(bag, camera_name):
    camera_infos = [msg.message for msg in bag.read_messages(camera_name)]
    camera_info = camera_infos[0]
    from image_geometry import PinholeCameraModel
    camera_model = PinholeCameraModel()
    camera_model.fromCameraInfo(camera_info)
    return camera_model
Exemple #11
0
def process_msgs(msgs, cinfo, shift):
    ret = len(msgs) * [None]

    cam_model = PinholeCameraModel()
    if cinfo is not None:
        cam_model.fromCameraInfo(cinfo)

    offset = None
    for it in range(0, len(msgs)):
        cur_msg = msgs[it]
        out_msg = msg(cur_msg.header.stamp.to_sec(), list())
        if cinfo is not None:
            # rospy.loginfo("using cinfo")
            for det in cur_msg.points:
                xyz = np.array([det.x, det.y, det.z])
                if offset is None:
                    offset = xyz
                xyz = xyz - offset + shift
                # xyz = np.dot(xyz, R)
                out_msg.positions.append(xyz)
        else:
            # rospy.loginfo("not using cinfo")
            x = cur_msg.pose.pose.position.x
            y = cur_msg.pose.pose.position.y
            z = cur_msg.pose.pose.position.z
            out_msg.positions.append(np.array([x, y, z]))
        ret[it] = out_msg
    return ret
class CamPixelToPointServer:
    def __init__(self):
        self.camera_model = PinholeCameraModel()
        self.bridge = CvBridge()
        self.model_set = False
        self.tf_listener = TransformListener()

    def pixel_to_point(self,
                       cam_pos,
                       out_frame='map'):  # type: (np.ndarray) -> PointStamped
        if not self.model_set:
            self.camera_model.fromCameraInfo(
                rospy.wait_for_message('/camera/rgb/camera_info', CameraInfo))
            self.model_set = True
        x, y = int(cam_pos[0]), int(cam_pos[1])
        d = self.read_depth_simple(x, y)
        pos = np.array(self.camera_model.projectPixelTo3dRay((x, y))) * d

        point = PointStamped()
        point.header.frame_id = self.camera_model.tfFrame()
        point.point.x, point.point.y, point.point.z = pos[0], pos[1], pos[2]
        point = convert_frame(self.tf_listener, point, out_frame)
        return point

    def read_depth_simple(self, x, y):  # (int, int) -> float
        image = rospy.wait_for_message('/camera/depth_registered/image_raw',
                                       Image)
        image = self.bridge.imgmsg_to_cv2(image)
        return image[y, x]
Exemple #13
0
    def centroid_to_pose(self, rect, moment):
        """
        Publish the region of interest as a geometry_msgs/Pose message from
        the ROI or the center of mass coordinates
        @param rect - sensor_msgs/RegionOfInterest
        @param moment - object's center of mass
        """
        if not moment:
            moment = (int(rect[0] + rect[2] / 2), int(rect[1] + rect[3] / 2))
        u, v = int(moment[0]), int(moment[1])

        ps = Pose()
        ps.orientation.x = 0.0
        ps.orientation.y = 0.0
        ps.orientation.z = 0.0
        ps.orientation.w = 1.0

        cam = PinholeCameraModel()
        cam.fromCameraInfo(self.cam_info)
        (x, y, z) = cam.projectPixelTo3dRay((u, v))

        ps.position.x = x
        ps.position.y = y
        ps.position.z = z

        return ps
class RGBD_Project(object):
    def __init__(self):
        self.cam = RGBD()

        not_read = True
        while not_read:

            try:
                cam_info = self.cam.read_info_data()
                if (not cam_info == None):
                    not_read = False
            except:
                rospy.logerr('info not recieved')

        self.pcm = PCM()

        self.pcm.fromCameraInfo(cam_info)

    def deproject_pose(self, pose):
        """"
        pose = (u_x,u_y,z)

        u_x,u_y correspond to pixel value in image
        x corresponds to depth
        """

        td_points = self.pcm.projectPixelTo3dRay((pose[0], pose[1]))

        norm_pose = np.array(td_points)
        norm_pose = norm_pose / norm_pose[2]
        norm_pose = norm_pose * (cfg.MM_TO_M * pose[2])

        return norm_pose
Exemple #15
0
class CamPixelToPointServer:
    def __init__(self):
        self.camera_model = PinholeCameraModel()
        self.bridge = CvBridge()
        self.camera_model.fromCameraInfo(SubscriberValue('camera_info', CameraInfo).value)
        self.depth_image = SubscriberValue('camera_depth_image', Image, transform=self.bridge.imgmsg_to_cv2)
        self.service = rospy.Service('cam_pixel_to_point', CamPixelToPoint, self.handle_service)
        self.tf_listener = TransformListener()
        print('Service is ready.')

    def handle_service(self, req):  # type: (CamPixelToPoint) -> CamPixelToPointResponse
        x, y = int(req.cam_pixel.x), int(req.cam_pixel.y)
        methods = [self.read_depth_simple,
                   # self.read_depth_average,
                   self.read_depth_as_floor_depth]
        for method in methods:
            d = method(x, y)
            if not np.isnan(d):
                break

        pos = np.array(self.camera_model.projectPixelTo3dRay((x, y))) * d

        point = PointStamped()
        point.header.frame_id = self.camera_model.tfFrame()
        point.point.x, point.point.y, point.point.z = pos[0], pos[1], pos[2]
        return CamPixelToPointResponse(point)

    def read_depth_simple(self, x, y):  # (int, int) -> float
        return self.depth_image.value[y, x]

    def read_depth_average(self, x, y):  # (int, int) -> float
        print('Fallback to average')
        s = 5
        return np.nanmean(self.depth_image.value[y-s:y+s, x-s:x+s])

    def read_depth_as_floor_depth(self, x, y):  # (int, int) -> float
        print('Fallback to floor model')
        min_distance = 10.0
        # Extend the camera ray until it passes through where the floor should be. Use its length as the depth.
        camera_origin = PointStamped()
        camera_origin.header.frame_id = self.camera_model.tfFrame()
        camera_origin.point.x, camera_origin.point.y, camera_origin.point.z = 0.0, 0.0, 0.0
        point_along_ray = PointStamped()
        point_along_ray.header.frame_id = self.camera_model.tfFrame()
        point_along_ray.point.x, point_along_ray.point.y, point_along_ray.point.z = self.camera_model.projectPixelTo3dRay((x, y))

        self.tf_listener.waitForTransform('base_footprint', self.camera_model.tfFrame(), rospy.Time(rospy.get_time()), rospy.Duration(1))
        camera_origin = self.tf_listener.transformPoint('base_footprint', camera_origin)
        point_along_ray = self.tf_listener.transformPoint('base_footprint', point_along_ray)

        camera_origin = np_from_poinstamped(camera_origin)
        point_along_ray = np_from_poinstamped(point_along_ray)
        ray_dir = point_along_ray - camera_origin
        # Assuming this transformation was orthogonal, |ray_dir| = 1, at least approximately
        d = camera_origin[1]/max(-ray_dir[1], camera_origin[1]/min_distance)
        if d <= 0.01:
            d = np.nan
        return d
class DrawRect():
	def __init__(self, topic_image, topic_blob, topic_info, topic_out):
		
		self.sync1_callback = False #synchronize the image
		self.sync2_callback = False #synchronize the camera info
				
		self.bridge = CvBridge()
		rospy.Subscriber(topic_image, Image, self.image_callback)
		rospy.Subscriber(topic_blob, Blobs, self.blob_callback)		
		rospy.Subscriber(topic_info, CameraInfo, self.info_callback)		

		self.pub = rospy.Publisher(topic_out, PointStamped)		

	def image_callback(self, image):
		self.sync1_callback = True
		self.image = self.bridge.imgmsg_to_cv(image)
		self.image = np.asarray(self.image)

	def info_callback (self, info):
		#get the camera information		
		self.sync2_callback = True
		self.info = info
		
	def blob_callback(self, blob):
		if self.sync1_callback and self.sync2_callback and (blob.blob_count != 0):
			bleb = self.FindBiggestBlob (blob)
			z = self.image[bleb.y][bleb.x]  # depth in mm (INT!)
			z /= 1000.0  # now in meters (FLOAT!)
	
			self.UVtoXYZ = PinholeCameraModel()
			self.UVtoXYZ.fromCameraInfo(self.info)
			vec = self.UVtoXYZ.projectPixelTo3dRay ((bleb.x, bleb.y))	
			vec = [x * (z/vec[2]) for x in vec]

			self.P = PointStamped()
			self.P.header.frame_id = self.info.header.frame_id
			self.P.point.x = vec[0]
			#the np make the downward direction positive direction,
			#so we need to multiply to -1 to make upward direction
			#positive direction			
			self.P.point.y = vec[1]
			self.P.point.z = vec[2]
			
			print self.P			
			self.pub.publish(self.P)						
						
	def FindBiggestBlob(self, blob):
		#Assume the hat is the only green blob in front of the camera		
		x = 0		
		bleb = None
		while x < blob.blob_count:
			if (x == 0):
				bleb = blob.blobs[x]
			else:
				if (blob.blobs[x].area > bleb.area):
					bleb = blob.blobs[x]
			x = x + 1
  		return bleb
class WorldProjector(object):
    def __init__(self, camera_info, h_matrix, distorted_input=True):
        self.distorted_input = distorted_input
        self.camera_info = camera_info
        self.fisheye = False
        if camera_info.distortion_model == "fisheye":
            self.camera = FisheyeCameraModel()
            self.camera.from_camera_info(camera_info)
            self.fisheye = True
        else:
            self.camera = PinholeCameraModel()
            self.camera.fromCameraInfo(camera_info)
        self.h_matrix = h_matrix


    def pixel2ground(self, pixel_coord, distorted_input=None):
        """Returns the ground projection of a pixel."""
        # use default value if 'distorted_input' is not set
        if distorted_input is None:
            distorted_input = self.distorted_input
        if distorted_input:
            pixel_coord = self.rectify_pixel(pixel_coord)
        pixel_coord = np.array(pixel_coord)

        # normalize coordinates
        pixel_normalized = normalize_image_coordinates(
            pixel_coord,
            self.camera.width,
            self.camera.height)

        point = self._project_normalized_pixel(pixel_normalized)
        return point

    def _project_normalized_pixel(self, pixel_normalized):
        # create homogeneous coordinate
        pixel_coord = np.append(pixel_normalized, 1.0)
        # project to world
        ground_coord = np.dot(self.h_matrix, pixel_coord)
        # scale the coordinates appropriately -> transform to eucledian space
        ground_coord = ground_coord / ground_coord[2]

        return ground_coord


    def rectify_pixel(self, pixel_coord):
        """Calculates the rectified undistorted image coordinate of a pixel.

        Args:
            pixel_coord: Distorted pixel coordinates

        Returns: Undistorted pixel coordinates
        """
        if self.fisheye:
            return self.camera.undistort_point(pixel_coord)
        else:
            return self.camera.rectifyPoint(pixel_coord)
Exemple #18
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'))
Exemple #19
0
    def test_monocular(self):
        ci = sensor_msgs.msg.CameraInfo()
        ci.width = 640
        ci.height = 480
        print(ci)
        cam = PinholeCameraModel()
        cam.fromCameraInfo(ci)
        print(cam.rectifyPoint((0, 0)))

        print(cam.project3dToPixel((0, 0, 0)))
 def get_rays(self):
     model = PinholeCameraModel()
     model.fromCameraInfo(self.depth_info)
     self.array_rays = numpy.zeros((self.image_depth.height, self.image_depth.width, 3))
     
     for u in range(self.image_depth.height):
         for v in range(self.image_depth.width):
             ray = model.projectPixelTo3dRay((u, v))
             ray_z = [el / ray[2] for el in ray]  # normalize the ray so its Z-component equals 1.0
             self.array_rays[u, v, :] = ray_z
def create_cam_model(info_msg):
    from image_geometry import PinholeCameraModel
    import numpy as np
    cam_model = PinholeCameraModel()

    #  <have an info message >
    cam_model.fromCameraInfo(info_msg)
    print 'cam_model = ', np.array(cam_model.intrinsicMatrix())
    print 'cam tf = ', cam_model.tfFrame()
    return cam_model
Exemple #22
0
 def make_pixel_to_3d(self):
     cm = PinholeCameraModel()
     cm.fromCameraInfo(self.camera_info)
     self.p3d = np.zeros(
         [self.camera_info.height, self.camera_info.width, 3])
     for v in range(0, self.camera_info.height):
         for u in range(0, self.camera_info.width):
             uv_rect = cm.rectifyPoint((u, v))
             p = cm.projectPixelTo3dRay(uv_rect)
             self.p3d[v, u, :] = (p[0] / p[2], p[1] / p[2], 1.0
                                  )  # make homogeneous
Exemple #23
0
    def localize_detection(self, detection):
        """
            2d image detection -> 3d point in [map frame]
        """
        u = detection.x + detection.width / 2
        v = detection.y + detection.height / 2

        camera_info = None
        best_time = 100
        for ci in self.camera_infos:
            time = abs(ci.header.stamp.to_sec() -
                       detection.header.stamp.to_sec())
            if time < best_time:
                camera_info = ci
                best_time = time

        if not camera_info or best_time > 1:
            return False, None

        camera_model = PinholeCameraModel()
        camera_model.fromCameraInfo(camera_info)

        point = Point(
            ((u - camera_model.cx()) - camera_model.Tx()) / camera_model.fx(),
            ((v - camera_model.cy()) - camera_model.Ty()) / camera_model.fy(),
            1)

        localization = self.localize(detection.header, point,
                                     self.region_scope)

        if not localization:
            return False, None

        point_trans = None

        print "Face localized ", localization.pose.position.x, localization.pose.position.y
        if localization.pose.position.x == 0.0:
            print "Ignoring this one!"
            return False, None
        try:
            #(pos_trans, rot) = self.tf_listener.lookupTransform('/map', detection.header.frame_id, rospy.Time(0))
            point_trans = self.tf_listener.transformPoint(
                '/map',
                PointStamped(detection.header, localization.pose.position))
        except (tf.LookupException, tf.ConnectivityException,
                tf.ExtrapolationException):
            pass

        if point_trans == None:  # transformation failed
            return False, None

        print "Face detected at %d,%d" % (point_trans.point.x,
                                          point_trans.point.y)
        return True, point_trans.point
Exemple #24
0
def create_cam_model(info_msg):
    from image_geometry import PinholeCameraModel
    import numpy as np

    cam_model = PinholeCameraModel()

    #  <have an info message >
    cam_model.fromCameraInfo(info_msg)
    print "cam_model = ", np.array(cam_model.intrinsicMatrix())
    print "cam tf = ", cam_model.tfFrame()
    return cam_model
Exemple #25
0
    def get_rays(self):
        model = PinholeCameraModel()
        model.fromCameraInfo(self.depth_info)
        self.array_rays = numpy.zeros(
            (self.image_depth.height, self.image_depth.width, 3))

        for u in range(self.image_depth.height):
            for v in range(self.image_depth.width):
                ray = model.projectPixelTo3dRay((u, v))
                ray_z = [el / ray[2] for el in ray
                         ]  # normalize the ray so its Z-component equals 1.0
                self.array_rays[u, v, :] = ray_z
Exemple #26
0
class AtLocalization:
    """
    Handles image rectification and april tag detection.
    """
    def __init__(self, camera_info, tag_size):

        # init image rectification
        self.pcm = PinholeCameraModel()
        self.pcm.fromCameraInfo(camera_info)
        self.K_rect, self.mapx, self.mapy = self._init_rectification()

        # init apriltag detector
        self.at_detector = Detector(searchpath=['apriltags'],
                                    families='tag36h11',
                                    nthreads=4,
                                    quad_decimate=4.0,
                                    quad_sigma=0.0,
                                    refine_edges=1,
                                    decode_sharpening=0.25,
                                    debug=0)
        self.tag_size = tag_size
        self.camera_params = (self.K_rect[0, 0], self.K_rect[1, 1],
                              self.K_rect[0, 2], self.K_rect[1, 2])

    def _init_rectification(self):
        """
        Establish rectification mapping.
        """
        w = self.pcm.width
        h = self.pcm.height
        K_rect, roi = cv2.getOptimalNewCameraMatrix(self.pcm.K, self.pcm.D,
                                                    (w, h), 1.0)
        mapx = np.ndarray(shape=(h, w, 1), dtype='float32')
        mapy = np.ndarray(shape=(h, w, 1), dtype='float32')
        mapx, mapy = cv2.initUndistortRectifyMap(self.pcm.K, self.pcm.D,
                                                 self.pcm.R, self.pcm.P,
                                                 (w, h), cv2.CV_32FC1, mapx,
                                                 mapy)
        return K_rect, mapx, mapy

    def rectify(self, img_raw, interpolation=cv2.INTER_NEAREST):
        """
        Rectify image.
        """
        return cv2.remap(img_raw, self.mapx, self.mapy, interpolation)

    def detect(self, img):
        img_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        tags = self.at_detector.detect(img_gray,
                                       estimate_tag_pose=True,
                                       camera_params=self.camera_params,
                                       tag_size=self.tag_size)
        return tags
Exemple #27
0
 def camera_model(self, msg):
     pcm = PinholeCameraModel()
     pcm.fromCameraInfo(msg)
     (self.m1, self.m2) = cv2.fisheye.initUndistortRectifyMap(
         pcm.K, pcm.D[:4], pcm.R, pcm.P, (msg.width, msg.height),
         cv2.CV_32FC1)
     print('K\n{}\nD\n{}\nR\n{}\nP\n{}'.format(pcm.K, pcm.D[:4], pcm.R,
                                               pcm.P))
     #PP = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(pcm.K, pcm.D[:4],
     #    (msg.width, msg.height), pcm.R, pcm.P)
     #print('PP\n{}\nm1{}\nm2{}'.format(PP, self.m1, self.m2))
     self.subscribe_camera()
Exemple #28
0
class VisionNode(object):
    '''
    Base class to be used unify the interfacing for MIL's computer vision scripts.
    Handles the bootstrap of image subscription, enable/disable, etc.
    Provides a callback for new images which is expected to return
    '''
    __metaclass__ = abc.ABCMeta

    def __init__(self):
        self._objects_pub = rospy.Publisher("~identified_objects", ObjectsInImage, queue_size=3)
        self._camera_info = None
        self.camera_model = None
        self._enabled = False
        self._image_sub = Image_Subscriber("image", callback=self._img_cb)
        if rospy.get_param("~autostart", default=False):
            self._enable()
        else:
            self._disable()
        self._enable_srv = rospy.Service("~enable", SetBool, self._enable_cb)

    def _enable_cb(self, req):
        if req.data and not self._enabled:
            self._enable()
        elif not req.data and self._enabled:
            self._disable()
        return {'success': True}

    def _enable(self):
        if self._camera_info is None:
            self._camera_info = self._image_sub.wait_for_camera_info()
            self.camera_model = PinholeCameraModel()
            self.camera_model.fromCameraInfo(self._camera_info)
        self._enabled = True
        rospy.loginfo("Enabled.")

    def _disable(self):
        self._enabled = False
        rospy.loginfo("Disabled.")

    def _img_cb(self, img):
        if not self._enabled:
            return
        msg = ObjectsInImage()
        msg.header = self._image_sub.last_image_header
        msg.objects = self.find_objects(img)
        if not isinstance(msg.objects, list) or (len(msg.objects) and not isinstance(msg.objects[0], ObjectInImage)):
            rospy.logwarn("find_objects did not return a list of mil_msgs/ObjectInImage message. Ignoring.")
        self._objects_pub.publish(msg)

    @abc.abstractmethod
    def find_objects(self, img):
        pass
Exemple #29
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'))
class DrawFrame:
    def __init__(self):
        rospy.init_node('draw_frames', anonymous=True)
        self.listener = tf.TransformListener()
        camera_str = '/camera/color/image_raw'
        self.bridge = CvBridge()
        self.im_sub = rospy.Subscriber(camera_str, sensor_msgs.msg.Image,
                                       self.callback)

        cam_info_str = "/camera/color/camera_info"
        cam_info_msg = rospy.wait_for_message(cam_info_str,
                                              sensor_msgs.msg.CameraInfo)

        self.ph = PinholeCameraModel()
        self.ph.fromCameraInfo(cam_info_msg)

    def callback(self, ros_data):
        try:
            cv_im = self.bridge.imgmsg_to_cv2(ros_data, "bgr8")
        except CvBridgeError as e:
            print e

        (rows, cols, channels) = cv_im.shape
        if cols > 60 and rows > 60:
            cv2.circle(cv_im, (50, 50), 10, 255)

        # get ros time
        now = rospy.get_rostime()
        # wait for 1/30 seconds
        rospy.sleep(1. / 30.)
        # get tf transform between desired frames
        (trans, rot) = self.listener.lookupTransform('/camera_link',
                                                     '/jenga_tf', now)
        # use project3dToPixel for the origin, get it in camera (u,v)
        ps = PointStamped()
        ps.point.x = trans[0]
        ps.point.y = trans[1]
        ps.point.z = trans[2]
        ps.header = ros_data.header
        ps.header.stamp = now
        point = self.listener.transformPoint("/camera_link", ps)
        print(trans)
        print point
        uv = self.ph.project3dToPixel(
            (point.point.x, point.point.y, point.point.z))
        print uv
        # draw a circle at (u,v)
        cv2.circle(cv_im, (int(uv[0]), int(uv[1])), 10, 255)

        cv2.imshow('image', cv_im)
        cv2.waitKey(3)
    def remap(raw_img, camera_info: CameraInfo):
        cam = PinholeCameraModel()
        cam.fromCameraInfo(camera_info)
        map_x, map_y = cv2.initUndistortRectifyMap(
            cam.intrinsicMatrix(),
            cam.distortionCoeffs(),
            cam.R,
            cam.projectionMatrix(),
            (cam.width, cam.height),
            cv2.CV_32FC1,
        )
        rectified_image = cv2.remap(raw_img, map_x, map_y, cv2.INTER_CUBIC)

        return rectified_image
class RayTransformer():
    def __init__(self, topic):
        self.model = PinholeCameraModel()
        rospy.Subscriber(topic, CameraInfo, self.callback_info)

    def callback_info(self, info):
        self.model.fromCameraInfo(info)

    def projectPointCloudToPixels(self, cloud):
        pixel_cloud = []
        for point in cloud.points:
            pixel = self.model.project3dToPixel((point.x, point.y, point.z))
            pixel_cloud.append(pixel)
        return pixel_cloud
class PointFromPixel():
    """ Given a pixel location, find its 3D location in the world """
    def __init__(self, topic_camera_info, topic_depth):
        self.need_camera_info = True
        self.need_depth_image = True
        self.model = PinholeCameraModel()
        rospy.Subscriber(topic_camera_info, CameraInfo, self.callback_camera_info)
        rospy.Subscriber(topic_depth, Image, self.callback_depth_image)

    def callback_camera_info(self, info):
        """ Define Pinhole Camera Model parameters using camera info msg """
        if self.need_camera_info:
            rospy.loginfo('Got camera info!')
            self.model.fromCameraInfo(info)  # define model params
            self.frame = info.header.frame_id
            self.need_camera_info = False

    def callback_depth_image(self, depth_image):
        """ Get depth at chosen pixel using depth image """
        if self.need_depth_image:
            rospy.loginfo('Got depth image!')
            self.depth = img.fromstring("F", (depth_image.width, depth_image.height), depth_image.data)
            self.need_depth_image = False

    def calculate_3d_point(self, pixel):
        """ Project ray through chosen pixel, then use pixel depth to get 3d point """
        lookup = self.depth.load()
        depth = lookup[pixel[0], pixel[1]]  # lookup pixel in depth image
        ray = self.model.projectPixelTo3dRay(tuple(pixel))  # get 3d ray of unit length through desired pixel
        ray_z = [el / ray[2] for el in ray]  # normalize the ray so its Z-component equals 1.0
        pt = [el * depth for el in ray_z]  # multiply the ray by the depth; its Z-component should now equal the depth value
        point = PointStamped()
        point.header.frame_id = self.frame
        point.point.x = pt[0]
        point.point.y = pt[1]
        point.point.z = pt[2]
        return point

    def ray_plane_intersection(self, pixel, plane):
        """ Given plane parameters [a, b, c, d] as in ax+by+cz+d=0,
        finds intersection of 3D ray with the plane. """ 
        ray = self.model.projectPixelTo3dRay(tuple(pixel))  # get 3d ray of unit length through desired pixel
        scale = -plane[3] / (plane[0]*ray[0] + plane[1]*ray[1] + plane[2]*ray[2])

        point = PointStamped()
        point.header.frame_id = self.frame
        point.point.x = ray[0] * scale
        point.point.y = ray[1] * scale
        point.point.z = ray[2] * scale
        return point
Exemple #34
0
    def img_callback(self, msg):
        cv_image = self.bridge.imgmsg_to_cv2(msg,
                                             desired_encoding='passthrough')

        #Respond to attribute error if subscribers haven't ran yet
        try:
            objects, img = self.yolo.search_for_objects(cv_image)
            self.img_pub.publish(
                self.bridge.cv2_to_imgmsg(img, encoding='passthrough'))
            model = PinholeCameraModel()
            model.fromCameraInfo(self.cam_info)
        except AttributeError:
            print("waiting")
            return

        if (len(objects) != 0):
            for obj in objects:
                print(obj)

                xy = obj["Point"]
                dist = self.depth_finder.get_depth(int(xy[0]), int(xy[1]))
                depth = dist[0]

                vect = model.projectPixelTo3dRay((xy[0], xy[1]))
                xyz = [el / vect[2] for el in vect]

                stampedPoint = PointStamped()
                stampedPoint.header.frame_id = "head_rgbd_sensor_rgb_frame"
                stampedPoint.point.x = xyz[0] * depth
                stampedPoint.point.y = xyz[1] * depth
                stampedPoint.point.z = depth

                rospy.wait_for_service('transform_point')
                get_3d_points = rospy.ServiceProxy('transform_point',
                                                   LocalizePoint)
                resp = get_3d_points(stampedPoint)
                #print(resp.localizedPointMap)
                threeDPoint = resp.localizedPointMap
                dictMsg = {}
                dictMsg["name"] = obj["Label"]
                dictMsg["type"] = "object"
                dictMsg["coords"] = [
                    threeDPoint.point.x, threeDPoint.point.y,
                    threeDPoint.point.z
                ]
                dictMsg["others"] = {}
                self.nav_pub.publish(json.dumps(dictMsg))

        else:
            print("No objects found.")
def main(args):
    global image
    rospy.init_node('skeleton_viewer', anonymous=True)

    image_info_sub = rospy.Subscriber("/camera/rgb/camera_info", CameraInfo,
                                      image_info_cb)

    if test_mode:
        cap = cv2.VideoCapture(0)

        size = (int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)),
                int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)))
    else:
        ic = image_converter()

        cam_model = PinholeCameraModel()
        while (not camera_info) and (not rospy.is_shutdown()):
            time.sleep(1)
            print("waiting for camera info")

        cam_model.fromCameraInfo(camera_info)
        size = cam_model.fullResolution()
        print(cam_model.cx(), cam_model.cy(), cam_model.fullResolution())

    fps = 30
    rate = rospy.Rate(fps)
    fourcc = cv2.VideoWriter_fourcc(*'XVID')
    writer = cv2.VideoWriter()
    success = writer.open(file, fourcc, fps, size, True)

    while not rospy.is_shutdown():
        if test_mode:
            ret, image = cap.read()

        writer.write(image)

        cv2.imshow("Image window", image)

        if cv2.waitKey(1) & 0xFF == 27:
            break

        rate.sleep()

    if test_mode:
        cap.release()

    writer.release()
Exemple #36
0
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)
Exemple #37
0
    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
Exemple #38
0
def run(sub_singleton):
    global marker_found

    nh = sub_singleton._node_handle
    #next_pose = yield nh.get_service_client('/next_search_pose', SearchPose)
    cam = PinholeCameraModel()

    # Go to max height to see as much as we can.
    yield sub_singleton.move.depth(SEARCH_DEPTH).zero_roll_and_pitch().go()
    yield nh.sleep(1.0)

    # ---------------------------------------
    # MOST OF THIS IS FOR THE SEARCH PATTERN.
    # ---------------------------------------
    # This is just to get camera info
    resp = yield sub_singleton.channel_marker.get_2d()
    cam.fromCameraInfo(resp.camera_info)

    # intial_pose = sub_singleton.last_pose()
    # intial_height = yield sub_singleton.get_dvl_range()
    # radius = calculate_visual_radius(cam, intial_height)

    # s = SearchPoseRequest()
    # s.intial_position = (yield intial_pose).pose.pose
    # s.search_radius = radius
    # s.reset_search = True
    # yield next_pose(s)
    # s.reset_search = False

    marker_found = False
    goer = go_to_marker(nh, sub_singleton, cam)

    # while not marker_found:
    #     # Move in search pattern until we find the marker
    #     resp = yield next_pose(s)
    #     print resp

    #     if resp.area > .8:
    #         continue

    #     print pose_to_numpy(resp.target_pose)[0]
    #     yield sub_singleton.move.set_position(pose_to_numpy(resp.target_pose)[0]).zero_roll_and_pitch().go()
    #     print "Searcher arrived"

    yield goer
Exemple #39
0
def run(sub_singleton):
    global marker_found

    nh = sub_singleton._node_handle
    #next_pose = yield nh.get_service_client('/next_search_pose', SearchPose)
    cam = PinholeCameraModel()

    # Go to max height to see as much as we can.
    yield sub_singleton.move.depth(SEARCH_DEPTH).zero_roll_and_pitch().go()
    yield nh.sleep(1.0)

    # ---------------------------------------
    # MOST OF THIS IS FOR THE SEARCH PATTERN.
    # ---------------------------------------
    # This is just to get camera info
    resp = yield sub_singleton.channel_marker.get_2d()
    cam.fromCameraInfo(resp.camera_info)

    # intial_pose = sub_singleton.last_pose()
    # intial_height = yield sub_singleton.get_dvl_range()
    # radius = calculate_visual_radius(cam, intial_height)

    # s = SearchPoseRequest()
    # s.intial_position = (yield intial_pose).pose.pose
    # s.search_radius = radius
    # s.reset_search = True
    # yield next_pose(s)
    # s.reset_search = False

    marker_found = False
    goer = go_to_marker(nh, sub_singleton, cam)

    # while not marker_found:
    #     # Move in search pattern until we find the marker
    #     resp = yield next_pose(s)
    #     print resp

    #     if resp.area > .8:
    #         continue

    #     print pose_to_numpy(resp.target_pose)[0]
    #     yield sub_singleton.move.set_position(pose_to_numpy(resp.target_pose)[0]).zero_roll_and_pitch().go()
    #     print "Searcher arrived"

    yield goer
 def unit_vector_to_point(self, x, y):
     '''
 Creates a unit vector from the camera's frame, given a pixel point from the image
 '''
     height, width, depth = self.frame  # Obtain the dimensions of the frame
     cam_info = rospy.wait_for_message("/cameras/" + self.camera_viewer +
                                       "/camera_info",
                                       CameraInfo,
                                       timeout=None)
     img_proc = PinholeCameraModel()
     img_proc.fromCameraInfo(cam_info)
     # The origin of the camera is initally set to the top left corner
     # However the projectPixelTo3dRay uses the origin at the center of the camera. Hence the coordinates have to be converted
     x = x - width / 2
     y = height / 2 - y
     # Creates a unit vector to the given point. Unit vector passes through point from the center of the camera
     n = img_proc.projectPixelTo3dRay((x, y))
     return n
Exemple #41
0
def run(sub_singleton):
    global marker_found

    print "Searching"
    nh = sub_singleton._node_handle
    next_pose = yield nh.get_service_client('/next_search_pose', SearchPose)
    cam = PinholeCameraModel()

    # Go to max height to see as much as we can.
    yield sub_singleton.move.depth(0.6).zero_roll_and_pitch().go()
    yield nh.sleep(1.0)

    # This is just to get camera info
    resp = yield sub_singleton.channel_marker.get_2d()
    cam.fromCameraInfo(resp.camera_info)

    intial_pose = sub_singleton.last_pose()
    intial_height = yield sub_singleton.get_dvl_range()
    raidus = calculate_visual_radius(cam, intial_height)

    s = SearchPoseRequest()
    s.intial_position = (yield intial_pose).pose.pose
    s.search_radius = raidus
    s.reset_search = True
    yield next_pose(s)
    s.reset_search = False

    marker_found = False
    goer = go_to_marker(nh, sub_singleton, cam)

    while not marker_found:
        # Move in search pattern until we find the marker
        resp = yield next_pose(s)
        print resp

        if resp.area > .8:
            continue

        print pose_to_numpy(resp.target_pose)[0]
        yield sub_singleton.move.set_position(pose_to_numpy(resp.target_pose)[0]).zero_roll_and_pitch().go()
        print "Searcher arrived"

    yield goer
Exemple #42
0
def random_point_inside_fov(camera_info, maxdist, Tcamera=np.eye(4)):
  """
  Generates a random XYZ point inside the camera field of view
  @type  camera_info: sensor_msgs.CameraInfo
  @param camera_info: Message with the meta information for a camera
  @type  maxdist:     float
  @param maxdist:     distance from the camera ref frame in the z direction
  @type  Tcamera:     np.array
  @param Tcamera:     homogeneous transformation for the camera ref frame
  @rtype: array
  @return: The random XYZ point
  """
  cam_model = PinholeCameraModel()
  cam_model.fromCameraInfo(camera_info)
  z = maxdist*np.random.random()
  delta_x = cam_model.getDeltaX(cam_model.width/2, z)
  delta_y = cam_model.getDeltaY(cam_model.height/2, z)
  point = np.array([0, 0, z, 1])
  point[:2] = np.array([delta_x, delta_y]) * (2*np.random.random_sample(2) - 1.)
  return np.dot(Tcamera, point)[:3]
Exemple #43
0
def camera_fov_corners(camera_info, zdist, Tcamera=np.eye(4)):
  """
  Generates the 5 corners of the camera field of view
  @type  camera_info: sensor_msgs.CameraInfo
  @param camera_info: Message with the meta information for a camera
  @type  zdist:       float
  @param zdist:       distance from the camera ref frame in the z direction
  @type  Tcamera:     np.array
  @param Tcamera:     homogeneous transformation for the camera ref frame
  @rtype: list
  @return: The 5 corners of the camera field of view
  """
  cam_model = PinholeCameraModel()
  cam_model.fromCameraInfo(camera_info)
  delta_x = cam_model.getDeltaX(cam_model.width/2, zdist)
  delta_y = cam_model.getDeltaY(cam_model.height/2, zdist)
  corners = [Tcamera[:3,3]]
  for k in itertools.product([-1,1],[-1,1]):
    point = np.array([0, 0, zdist, 1])
    point[:2] = np.array([delta_x, delta_y]) * np.array(k)
    corners.append( np.dot(Tcamera, point)[:3] )
  return np.array(corners)
    def store_image_info_cb(msg, topic_name):
        ''' Stores the topic name, pinhole camera model object, and
            matrices from the camera to the map in pickle fiels.
        '''
        import pickle
        import os
        import rospkg

        print "Storing info"

        pinhole_camera_model = PinholeCameraModel()
        pinhole_camera_model.fromCameraInfo(msg)

        matrices = PixelConversion.get_matrices(msg.header.frame_id, "/map")

        

        filename = topic_name.replace("/", "_") + ".pickle"

        rospack = rospkg.RosPack()
        pkg_path = rospack.get_path('contamination_monitor')
        directory = os.path.join(pkg_path, "eval", "camera_map_tfs") 

        if not os.path.exists(directory):
            os.makedirs(directory)
        
        filepath = os.path.join(directory, filename)

        
        
        with open(filepath, 'wb+') as f:
            pickle.dump(topic_name, f)
            pickle.dump(pinhole_camera_model, f)
            pickle.dump(matrices, f)

            print "pickle dumped to ", filepath
    def get_point_cloud(self):

        # Scan the scene
        col_associations = self.get_projector_line_associations()

        # Project white light onto the scene to get the intensities of each pixel (for coloring our point cloud)
        illumination_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
        cv.Set(illumination_projection, 255)
        intensities_image = self.get_picture_of_projection(illumination_projection)

        # Turn projector off when done with it
        off_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
        cv.SetZero(off_projection)
        off_image_message = self.bridge.cv_to_imgmsg(off_projection, encoding="mono8")
        self.set_projection(off_image_message)

        camera_model = PinholeCameraModel()
        camera_model.fromCameraInfo(self.camera_info)
        
        valid_points_mask = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
        cv.CmpS(col_associations, -1, valid_points_mask, cv.CV_CMP_NE)
        
        number_of_valid_points = cv.CountNonZero(valid_points_mask)

        rectified_camera_coordinates = cv.CreateMat(1, number_of_valid_points, cv.CV_32FC2)
        scanline_associations = cv.CreateMat(1, number_of_valid_points, cv.CV_32FC1)
        intensities = cv.CreateMat(1, number_of_valid_points, cv.CV_8UC1)
        i = 0;
        for row in range(self.camera_info.height):
            for col in range(self.camera_info.width):
                if valid_points_mask[row, col] != 0:
                    rectified_camera_coordinates[0, i] = (col, row)
                    scanline_associations[0, i] = col_associations[row, col]
                    intensities[0, i] = intensities_image[row, col]
                    i += 1
       
        cv.UndistortPoints(rectified_camera_coordinates, rectified_camera_coordinates, camera_model.intrinsicMatrix(), camera_model.distortionCoeffs())

        # Convert scanline numbers to plane normal vectors
        planes = self.scanline_numbers_to_planes(scanline_associations)

        camera_rays = project_pixels_to_3d_rays(rectified_camera_coordinates, camera_model)
        
        intersection_points = ray_plane_intersections(camera_rays, planes)
        
        self.point_cloud_msg = sensor_msgs.msg.PointCloud()
        self.point_cloud_msg.header.stamp = rospy.Time.now()
        self.point_cloud_msg.header.frame_id = 'base_link'
        channels = []
        channel = sensor_msgs.msg.ChannelFloat32()
        channel.name = "intensity"
        points = []
        intensities_list = []

        for i in range(number_of_valid_points):
            point = geometry_msgs.msg.Point32()
            point.x = intersection_points[0, i][0]
            point.y = intersection_points[0, i][1]
            point.z = intersection_points[0, i][2]
            if point.z < 0:
                print scanline_associations[0, i]
                print rectified_camera_coordinates[0, i]
                
            points.append(point)
            intensity = intensities[0, i]
            intensities_list.append(intensity)
     
        channel.values = intensities_list
        channels.append(channel)
        self.point_cloud_msg.channels = channels
        self.point_cloud_msg.points = points

        return self.point_cloud_msg
class Scanner:
    def __init__(self):
        self.pixels_per_scanline = rospy.get_param('~pixels_per_scanline')
        self.scanner_info_file_name = rospy.get_param('~scanner_info_file_name')
        self.threshold = rospy.get_param('~threshold')        

        self.mutex = threading.RLock()
        self.image_update_flag = threading.Event()
        self.bridge = CvBridge()
        rospy.Subscriber("image_stream", sensor_msgs.msg.Image, self.update_image)
        rospy.loginfo("Waiting for camera info...")
        self.camera_info = rospy.wait_for_message('camera_info', sensor_msgs.msg.CameraInfo)
        rospy.loginfo("Camera info received.")

        rospy.loginfo("Waiting for projector info service...")
        rospy.wait_for_service('get_projector_info')
        rospy.loginfo("Projector info service found.")
        get_projector_info = rospy.ServiceProxy('get_projector_info', projector.srv.GetProjectorInfo)
        self.projector_info = get_projector_info().projector_info

        self.projector_model = PinholeCameraModel()
        self.projector_model.fromCameraInfo(self.projector_info)

        self.read_scanner_info()

        self.projector_to_camera_rotation_matrix = cv.CreateMat(3, 3, cv.CV_32FC1)
        cv.Rodrigues2(self.projector_to_camera_rotation_vector, self.projector_to_camera_rotation_matrix)        

        predistortmap_x = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_32FC1)
        predistortmap_y = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_32FC1)
        InitPredistortMap(self.projector_model.intrinsicMatrix(), self.projector_model.distortionCoeffs(), predistortmap_x, predistortmap_y)

        minVal, maxVal, minLoc, maxLoc = cv.MinMaxLoc(predistortmap_x)
        cv.AddS(predistortmap_x, -minVal, predistortmap_x)
        uncropped_projection_width = int(math.ceil(maxVal - minVal))
        self.center_pixel = self.projector_model.cx() - minVal

        minVal, maxVal, minLoc, maxLoc = cv.MinMaxLoc(predistortmap_y)
        cv.AddS(predistortmap_y, -minVal, predistortmap_y)
        uncropped_projection_height = int(math.ceil(maxVal - minVal))

        self.number_of_scanlines = int(math.ceil(float(uncropped_projection_width)/self.pixels_per_scanline))

        rospy.loginfo("Generating projection patterns...")

        graycodes = []
        for i in range(self.number_of_scanlines):
            graycodes.append(graycodemath.generate_gray_code(i))
    
        self.number_of_projection_patterns = int(math.ceil(math.log(self.number_of_scanlines, 2)))        
        self.predistorted_positive_projections = []
        self.predistorted_negative_projections = []

        for i in range(self.number_of_projection_patterns):
            cross_section = cv.CreateMat(1, uncropped_projection_width, cv.CV_8UC1)

            #Fill in cross section with the associated bit of each gray code
            for pixel in range(uncropped_projection_width):
                scanline = pixel // self.pixels_per_scanline
                scanline_value = graycodemath.get_bit(graycodes[scanline], i) * 255
                cross_section[0, pixel] = scanline_value

            #Repeat the cross section over the entire image
            positive_projection = cv.CreateMat(uncropped_projection_height, uncropped_projection_width, cv.CV_8UC1)
            cv.Repeat(cross_section, positive_projection)

            #Predistort the projections to compensate for projector optics so that that the scanlines are approximately planar
            predistorted_positive_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
            cv.Remap(positive_projection, predistorted_positive_projection, predistortmap_x, predistortmap_y, flags=cv.CV_INTER_NN)

            #Create a negative of the pattern for thresholding
            predistorted_negative_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
            cv.Not(predistorted_positive_projection, predistorted_negative_projection)
                
            #Fade the borders of the patterns to avoid artifacts at the edges of projection
            predistorted_positive_projection_faded = fade_edges(predistorted_positive_projection, 20)
            predistorted_negative_projection_faded = fade_edges(predistorted_negative_projection, 20)
                
            self.predistorted_positive_projections.append(predistorted_positive_projection_faded)
            self.predistorted_negative_projections.append(predistorted_negative_projection_faded)

        rospy.loginfo("Waiting for projection setting service...")
        rospy.wait_for_service('set_projection')
        rospy.loginfo("Projection setting service found.")
        self.set_projection = rospy.ServiceProxy('set_projection', projector.srv.SetProjection)

        self.pixel_associations_msg = None
        self.point_cloud_msg = None

        rospy.Service("~get_point_cloud", graycode_scanner.srv.GetPointCloud, self.handle_point_cloud_srv)

        point_cloud_pub = rospy.Publisher('~point_cloud', sensor_msgs.msg.PointCloud)

        rospy.loginfo("Ready.")

        rate = rospy.Rate(1)
        while not rospy.is_shutdown():
            if self.point_cloud_msg is not None:
                point_cloud_pub.publish(self.point_cloud_msg)
            rate.sleep()

    def read_scanner_info(self):
        try:
            input_stream = file(self.scanner_info_file_name, 'r')
        except IOError:
            rospy.loginfo('Specified scanner info file at ' + self.scanner_info_file_name + ' does not exist.')
            return
        info_dict = yaml.load(input_stream)
        try:
            self.projector_to_camera_translation_vector = tuple(info_dict['projector_to_camera_translation_vector'])
            self.projector_to_camera_rotation_vector = list_to_matrix(info_dict['projector_to_camera_rotation_vector'], 3, 1, cv.CV_32FC1)
        except (TypeError, KeyError):
            rospy.loginfo('Scanner info file at ' + self.scanner_info_file_name + ' is in an unrecognized format.')
            return
        rospy.loginfo('Scanner info successfully read from ' + self.scanner_info_file_name)

    def update_image(self, imagemsg):
        with self.mutex:
            if not self.image_update_flag.is_set():
                self.latest_image = self.bridge.imgmsg_to_cv(imagemsg, "mono8")
                self.image_update_flag.set()
    
    def get_next_image(self):
        with self.mutex:
            self.image_update_flag.clear()
        self.image_update_flag.wait()
        with self.mutex:
            return self.latest_image
                            
    def get_picture_of_projection(self, projection):
        image_message = self.bridge.cv_to_imgmsg(projection, encoding="mono8")
        self.set_projection(image_message)
        return self.get_next_image()
    
    def get_projector_line_associations(self):
        rospy.loginfo("Scanning...")
        positives = []
        negatives = []
        for i in range(self.number_of_projection_patterns):
            positives.append(self.get_picture_of_projection(self.predistorted_positive_projections[i]))
            negatives.append(self.get_picture_of_projection(self.predistorted_negative_projections[i]))
        
            
        rospy.loginfo("Thresholding...")
        strike_sum = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
        cv.SetZero(strike_sum)
        gray_codes = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
        cv.SetZero(gray_codes)
        for i in range(self.number_of_projection_patterns):
            difference = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
            cv.Sub(positives[i], negatives[i], difference)

            absolute_difference = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
            cv.AbsDiff(positives[i], negatives[i], absolute_difference)

            #Mark all the pixels that were "too close to call" and add them to the running total
            strike_mask = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
            cv.CmpS(absolute_difference, self.threshold, strike_mask, cv.CV_CMP_LT)
            strikes = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
            cv.Set(strikes, 1, strike_mask)
            cv.Add(strikes, strike_sum, strike_sum)
            
            #Set the corresponding bit in the gray_code
            bit_mask = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
            cv.CmpS(difference, 0, bit_mask, cv.CV_CMP_GT)
            bit_values = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
            cv.Set(bit_values, 2 ** i, bit_mask)
            cv.Or(bit_values, gray_codes, gray_codes)

        rospy.loginfo("Decoding...")
        # Decode every gray code into binary
        projector_line_associations = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
        cv.Copy(gray_codes, projector_line_associations)
        for i in range(cv.CV_MAT_DEPTH(cv.GetElemType(projector_line_associations)), -1, -1):
            projector_line_associations_bitshifted_right = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
            #Using ConvertScale with a shift of -0.5 to do integer division for bitshifting right
            cv.ConvertScale(projector_line_associations, projector_line_associations_bitshifted_right, (2 ** -(2 ** i)), -0.5)
            cv.Xor(projector_line_associations, projector_line_associations_bitshifted_right, projector_line_associations)
        
        rospy.loginfo("Post processing...")
        
        # Remove all pixels that were "too close to call" more than once
        strikeout_mask = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
        cv.CmpS(strike_sum, 1, strikeout_mask, cv.CV_CMP_GT)
        cv.Set(projector_line_associations, -1, strikeout_mask)
        
        # Remove all pixels that don't decode to a valid scanline number
        invalid_scanline_mask = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
        cv.InRangeS(projector_line_associations, 0, self.number_of_scanlines, invalid_scanline_mask)
        cv.Not(invalid_scanline_mask, invalid_scanline_mask)
        cv.Set(projector_line_associations, -1, invalid_scanline_mask)
        
        self.display_scanline_associations(projector_line_associations)

        return projector_line_associations

    def handle_point_cloud_srv(self, req):
        response = graycode_scanner.srv.GetPointCloudResponse()
        self.get_point_cloud()
        response.point_cloud = self.point_cloud_msg
        response.success = True
        return response

    def get_point_cloud(self):

        # Scan the scene
        col_associations = self.get_projector_line_associations()

        # Project white light onto the scene to get the intensities of each pixel (for coloring our point cloud)
        illumination_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
        cv.Set(illumination_projection, 255)
        intensities_image = self.get_picture_of_projection(illumination_projection)

        # Turn projector off when done with it
        off_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
        cv.SetZero(off_projection)
        off_image_message = self.bridge.cv_to_imgmsg(off_projection, encoding="mono8")
        self.set_projection(off_image_message)

        camera_model = PinholeCameraModel()
        camera_model.fromCameraInfo(self.camera_info)
        
        valid_points_mask = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
        cv.CmpS(col_associations, -1, valid_points_mask, cv.CV_CMP_NE)
        
        number_of_valid_points = cv.CountNonZero(valid_points_mask)

        rectified_camera_coordinates = cv.CreateMat(1, number_of_valid_points, cv.CV_32FC2)
        scanline_associations = cv.CreateMat(1, number_of_valid_points, cv.CV_32FC1)
        intensities = cv.CreateMat(1, number_of_valid_points, cv.CV_8UC1)
        i = 0;
        for row in range(self.camera_info.height):
            for col in range(self.camera_info.width):
                if valid_points_mask[row, col] != 0:
                    rectified_camera_coordinates[0, i] = (col, row)
                    scanline_associations[0, i] = col_associations[row, col]
                    intensities[0, i] = intensities_image[row, col]
                    i += 1
       
        cv.UndistortPoints(rectified_camera_coordinates, rectified_camera_coordinates, camera_model.intrinsicMatrix(), camera_model.distortionCoeffs())

        # Convert scanline numbers to plane normal vectors
        planes = self.scanline_numbers_to_planes(scanline_associations)

        camera_rays = project_pixels_to_3d_rays(rectified_camera_coordinates, camera_model)
        
        intersection_points = ray_plane_intersections(camera_rays, planes)
        
        self.point_cloud_msg = sensor_msgs.msg.PointCloud()
        self.point_cloud_msg.header.stamp = rospy.Time.now()
        self.point_cloud_msg.header.frame_id = 'base_link'
        channels = []
        channel = sensor_msgs.msg.ChannelFloat32()
        channel.name = "intensity"
        points = []
        intensities_list = []

        for i in range(number_of_valid_points):
            point = geometry_msgs.msg.Point32()
            point.x = intersection_points[0, i][0]
            point.y = intersection_points[0, i][1]
            point.z = intersection_points[0, i][2]
            if point.z < 0:
                print scanline_associations[0, i]
                print rectified_camera_coordinates[0, i]
                
            points.append(point)
            intensity = intensities[0, i]
            intensities_list.append(intensity)
     
        channel.values = intensities_list
        channels.append(channel)
        self.point_cloud_msg.channels = channels
        self.point_cloud_msg.points = points

        return self.point_cloud_msg

    def scanline_numbers_to_planes(self, scanline_numbers):
        rows = scanline_numbers.height
        cols = scanline_numbers.width
        normal_vectors_x = cv.CreateMat(rows, cols, cv.CV_32FC1)
        cv.Set(normal_vectors_x, -1)
        normal_vectors_y = cv.CreateMat(rows, cols, cv.CV_32FC1)
        cv.Set(normal_vectors_y, 0)
        normal_vectors_z = cv.CreateMat(rows, cols, cv.CV_32FC1)
        cv.Copy(scanline_numbers, normal_vectors_z)

        cv.ConvertScale(normal_vectors_z, normal_vectors_z, scale=self.pixels_per_scanline)
        cv.AddS(normal_vectors_z, -self.center_pixel, normal_vectors_z)
        cv.ConvertScale(normal_vectors_z, normal_vectors_z, scale=1.0/self.projector_model.fx())
        
        normal_vectors = cv.CreateMat(rows, cols, cv.CV_32FC3)
        cv.Merge(normal_vectors_x, normal_vectors_y, normal_vectors_z, None, normal_vectors)

        # Bring the normal vectors into camera coordinates
        cv.Transform(normal_vectors, normal_vectors, self.projector_to_camera_rotation_matrix)
        
        normal_vectors_split = [None]*3
        for i in range(3):
            normal_vectors_split[i] = cv.CreateMat(rows, cols, cv.CV_32FC1)
        cv.Split(normal_vectors, normal_vectors_split[0], normal_vectors_split[1], normal_vectors_split[2], None)

        n_dot_p = cv.CreateMat(rows, cols, cv.CV_32FC1)
        cv.SetZero(n_dot_p)
        for i in range(3):
            cv.ScaleAdd(normal_vectors_split[i], self.projector_to_camera_translation_vector[i], n_dot_p, n_dot_p)

        planes = cv.CreateMat(rows, cols, cv.CV_32FC4)
        cv.Merge(normal_vectors_split[0], normal_vectors_split[1], normal_vectors_split[2], n_dot_p, planes)
        

        return planes

    def display_scanline_associations(self, associations):
        display_image = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
        cv.ConvertScale(associations, display_image, 255.0/self.number_of_scanlines)
        cv.NamedWindow("associations", flags=0)
        cv.ShowImage("associations", display_image)
        cv.WaitKey(800)
class ARTagCornerGrabber():
    def __init__(self):
        self.file = os.environ['HOME'] + '/ar_tag_corners_' + str(rospy.Time.now().to_nsec()) + '.pickle'
        self.lis = tf.TransformListener()
        self.bridge = CvBridge()
        self.model = PinholeCameraModel()
        self.need_info = True
        rospy.Subscriber('/camera/rgb/camera_info', CameraInfo, self.info_callback)
        self.need_bounds = True
        rospy.Subscriber('/object_bounds', PolygonStamped, self.bounds_callback)

        while self.need_info and self.need_bounds:
            rospy.sleep(0.01)  # block until have CameraInfo and object bounds

        self.corners = []
        rospy.Subscriber('/camera/rgb/image_color', Image, self.image_callback, queue_size=1)

        #while not rospy.is_shutdown():
        #    self.get_tag_frame()

    def image_callback(self, image_msg):
        print 'Causing an image to be!'
        image = self.bridge.imgmsg_to_cv2(image_msg)

        self.get_tag_frame(image_msg.header.stamp)

        if len(self.corners) > 0:
            for corner in self.corners:
                cv2.circle(image, corner, 3, (255,0,255), -1)
            self.corners = []  # clear the corners
                
        cv2.imshow('Image with AR tag corners shown', image)
        cv2.waitKey(1)

    def info_callback(self, info):
        if self.need_info:
            self.model.fromCameraInfo(info)
            self.need_info = False
            rospy.loginfo('AR_TAG_PROJECTOR> Got CameraInfo!')

    def bounds_callback(self, bounds):
        if self.need_bounds:
            self.bounds = bounds
            self.need_bounds = False
            rospy.loginfo('AR_TAG_PROJECTOR> Got object bounds!')

    def get_tag_frame(self, time):
        print 'Updating tf frame!'
        corners = []
        source_frame = self.bounds.header.frame_id
        target_frame = '/camera_rgb_optical_frame'
        if True: #self.lis.frameExists(source_frame):
            try:
                self.lis.waitForTransform(target_frame, source_frame, time, rospy.Duration(3.0))
                for point in self.bounds.polygon.points:
                    ps = PointStamped()
                    ps.header.stamp = time
                    ps.header.frame_id = source_frame
                    ps.point.x = point.x
                    ps.point.y = point.y
                    ps.point.z = 0.0
                    corner_cam_frame = self.lis.transformPoint(target_frame, ps)
                    uv = self.model.project3dToPixel((corner_cam_frame.point.x,
                                                      corner_cam_frame.point.y,
                                                      corner_cam_frame.point.z))
                    corners.append(tuple(int(el) for el in uv))
                self.corners = corners                
                print corners
                with open(self.file, 'a') as f:
                    pickle.dump((time.to_time(), corners), f)
            except tf.Exception:
                rospy.loginfo('Could not do coordinate transformation.')
        else:
            rospy.loginfo('Frame {0} does not exist.'.format(source_frame))
def project_onto_depth(xyz, z_desired):
    """ Given an arbitrary vector in XYZ, scales such that Z-value is
    as desired. """
    scale = z_desired / xyz[2]
    xyz = tuple(el * scale for el in xyz)
    return xyz

if __name__ == "__main__":

    bag_path = sys.argv[1]  # path to camera_info
    bag = rosbag.Bag(bag_path)
    camera_infos = bag.read_messages(topics='/camera/rgb/camera_info')
    camera_info = camera_infos.next()[1]
    model = PinholeCameraModel()
    model.fromCameraInfo(camera_info)

    # Fig. 1: How many meters per pixel at various depths?
    print model.projectPixelTo3dRay((319.5, 239.5))
    u_neutral = 319.5
    v_neutral = 239.5
    depths = [d * 0.10 for d in range(10,50)]  # approximate cutoff distance: 4m
    res = []
    for depth in depths:
        primus = model.projectPixelTo3dRay((u_neutral, v_neutral))
        secundus = model.projectPixelTo3dRay((u_neutral+1, v_neutral))
        
        primus_at_depth = project_onto_depth(primus, depth)
        secundus_at_depth = project_onto_depth(secundus, depth)
        
        dXYZ = tuple(s - p for p,s in zip(primus_at_depth, secundus_at_depth))
class BoundingBoxFilter():
    def __init__(self, only_record):

        print only_record
        self.only_record = only_record
        self.file = os.environ['HOME'] + '/filter_corners_' + str(rospy.Time.now().to_nsec()) + '.pickle'

        self.lis = tf.TransformListener()

        self.need_info = True
        self.model = PinholeCameraModel()

        rospy.Subscriber('/camera/rgb/camera_info', CameraInfo, self.info_callback)

        while self.need_info:
            rospy.sleep(0.01)

        rospy.loginfo('Got CameraInfo! Proceeding to filter.')

        self.have_projections = False
        rospy.Subscriber('/object_vertex_projections', PointCloud, self.projections_callback)

        self.bridge = CvBridge()
        self.sub_image = rospy.Subscriber('/camera/rgb/image_color', Image, self.image_callback)#, queue_size=1)
        self.image_pub = rospy.Publisher('/image_out', Image)

    def info_callback(self, info):
        if self.need_info:
            self.model.fromCameraInfo(info)
            self.need_info = False

    def projections_callback(self, projections):
        self.projections = projections
        if not self.have_projections:
            rospy.loginfo('Got projections! Can now begin filtering.')
            self.have_projections = True

    def image_callback(self, image):
        if self.have_projections:
            self.projections.header.stamp = image.header.stamp
            try:
                #self.lis.waitForTransform('/camera_rgb_optical_frame', self.projections.header.frame_id, image.header.stamp, rospy.Duration(1.0))
                projections = self.lis.transformPointCloud('/camera_rgb_optical_frame', self.projections)  # transform to camera frame
            except Exception: 
                rospy.loginfo('Skipping image!')
                return  # skip this image; the filter probably hasn't caught up yet
            corners = []
            for point in projections.points:  # project rays onto camera image plane
                u, v = self.model.project3dToPixel((point.x,
                                                    point.y,
                                                    point.z))
                corners.append((int(u), int(v)))

            if self.only_record:
                with open(self.file, 'a') as f:
                    pickle.dump((rospy.Time.now().to_time(), corners), f)
                    rospy.loginfo('RECORDED FILTER POINTS!')

            else:
                array = self.bridge.imgmsg_to_cv2(image, "bgr8")
                for i in range(0, len(corners), 4):
                    cv2.rectangle(array, corners[i], corners[i+2],  (0,0,255), 3)
                #corners_convex = cv2.convexHull(numpy.array(corners))  # convex hull algorithm
                #cv2.fillConvexPoly(array, corners_convex, (0, 0, 255))  # fill convex hull
                #for [u, v] in corners:
                #    cv2.circle(array, (int(u), int(v)), 3, (255, 0, 0))  # draw circles at vertices for debugging
                image_new = self.bridge.cv2_to_imgmsg(array, "bgr8")
                image_new.header.stamp = rospy.Time.now()
                self.image_pub.publish(image_new)
Exemple #50
0
class MarkerOccGrid(OccGridUtils):
    '''
    Handles updating occupancy grid when new data comes in.
    TODO: Upon call can return some path to go to in order to find them.
    '''
    def __init__(self, image_sub, grid_res, grid_width, grid_height, grid_starting_pose):
        super(self.__class__, self).__init__(res=grid_res, width=grid_width, height=grid_height, starting_pose=grid_starting_pose)

        self.tf_listener = tf.TransformListener()

        self.cam = PinholeCameraModel()
        self.camera_info = image_sub.wait_for_camera_info()
        if self.camera_info is None:
            # Maybe raise an alarm here.
            rospy.logerr("I don't know what to do without my camera info.")

        self.cam.fromCameraInfo(self.camera_info)

    def update_grid(self, timestamp):
        '''
        Takes marker information to update occupacy grid.
        '''
        x_y_position, height = self.get_tf(timestamp)

        self.add_circle(x_y_position, self.calculate_visual_radius(height))
        self.publish_grid()

    def add_marker(self, marker, timestamp):
        '''
        Find the actual 3d pose of the marker and fill in the occupancy grid for that pose.
        This works by:
            1. Calculate unit vector between marker point and the image center in the image frame.
            2. Use height measurement to find real life distance (m) between center point and marker center.
            3. Use unit vec and magnitude to find dx and dy in meters.
            3. Pass info to OccGridUtils.
        '''
        if marker[0] is None:
            return

        x_y_position, height = self.get_tf(timestamp)

        if marker[2] < self.calculate_marker_area(height) * .6:
            # If the detected region isn't big enough dont add it.
            rospy.logwarn("Marker found but it is too small, not adding marker.")
            return

        # Calculate position of marker accounting for camera rotation.
        dir_vector = unit_vector(np.array([self.cam.cx(), self.cam.cy()] - marker[0]))
        trans, rot = self.tf_listener.lookupTransform("/map", "/downward", timestamp)
        cam_rotation = tf.transformations.euler_from_quaternion(rot)[2] + np.pi / 2
        dir_vector = np.dot(dir_vector, make_2D_rotation(cam_rotation))
        marker_rotation = cam_rotation + marker[1]

        trans, rot = self.tf_listener.lookupTransform("/map", "/base_link", timestamp)
        if (np.abs(np.array(rot)[:2]) > .005).any():
            rospy.logwarn("We're at a weird angle, not adding marker.")

        magnitude = self.calculate_visual_radius(height, second_point=marker[0])
        local_position = dir_vector[::-1] * magnitude
        position = local_position + x_y_position

        #print local_position

        # Pose on ground plane from center
        pose = Pose2D(x=position[0], y=position[1], theta=marker_rotation)
        self.found_marker(pose)

        # The image coordinate pose and real life pose are different.
        pose = Pose2D(x=position[0], y=position[1], theta=marker_rotation)
        # In meters with initial point at (0,0)
        return pose

    def get_tf(self, timestamp=None):
        '''
        x_y position and height in meters
        '''
        if timestamp is None:
            timestamp = rospy.Time()

        self.tf_listener.waitForTransform("/map", "/downward", timestamp, rospy.Duration(1.0))
        trans, rot = self.tf_listener.lookupTransform("/map", "/downward", timestamp)
        x_y_position = trans[:2]
        self.tf_listener.waitForTransform("/ground", "/downward", timestamp, rospy.Duration(1.0))
        trans, _ = self.tf_listener.lookupTransform("/ground", "/downward", timestamp)
        height = np.nan_to_num(trans[2])

        return x_y_position, height

    def correct_height(self, measured_height, timestamp):
        '''
        Adjust the measured height from the seafloor using our orientation relative to it.
        We assume the floor is flat (should be valid for transdec but maybe not for pool).
        All the math is just solving triangles.

        Note: If the roll or pitch is too far off, the range could be to a point that is not
            planar to the floor directly under us - in which case this will fail.

        TODO: See if this actually can produce better results.
        '''
        trans, rot = self.tf_listener.lookupTransform("/map", "/base_link", timestamp)
        euler_rotation = tf.transformations.euler_from_quaternion(rot)

        roll_offset = np.abs(np.sin(euler_rotation[0]) * measured_height)
        pitch_offset = np.abs(np.sin(euler_rotation[1]) * measured_height)

        height = np.sqrt(measured_height ** 2 - (roll_offset ** 2 + pitch_offset ** 2))
        return height

    def calculate_marker_area(self, height):
        '''
        Esitmate what the area of the marker should be so we don't add incorrect markers to the occupancy grid.
        What we really don't want is to add markers to the grid that are on the edge since the direction and center of
            the marker are off.
        '''
        MARKER_LENGTH = 1.22  # m
        MARKER_WIDTH = .1524  # m

        # Get m/px on the ground floor.
        m = self.calculate_visual_radius(height)
        if self.cam.cy() < self.cam.cx():
            px = self.cam.cy()
        else:
            px = self.cam.cx()

        m_px = m / px
        marker_area_m = MARKER_WIDTH * MARKER_LENGTH
        marker_area_px = marker_area_m / (m_px ** 2)

        return marker_area_px

    def calculate_visual_radius(self, height, second_point=None):
        '''
        Draws rays to find the radius of the FOV of the camera in meters.
        It also can work to find the distance between two planar points some distance from the camera.
        '''

        mid_ray = np.array([0, 0, 1])

        if second_point is None:
            if self.cam.cy() < self.cam.cx():
                second_point = np.array([self.cam.cx(), 0])
            else:
                second_point = np.array([0, self.cam.cy()])

        edge_ray = unit_vector(self.cam.projectPixelTo3dRay(second_point))

        # Calculate angle between vectors and use that to find r
        theta = np.arccos(np.dot(mid_ray, edge_ray))
        return np.tan(theta) * height
Exemple #51
0
def run(sub):
    global SPEED
    global pub_cam_ray
    fprint('Enabling cam_ray publisher')
    pub_cam_ray = yield sub.nh.advertise('/dice/cam_ray', Marker)

    yield sub.nh.sleep(1)

    fprint('Connecting camera')

    cam_info_sub = yield sub.nh.subscribe('/camera/front/left/camera_info',
                                          CameraInfo)

    fprint('Obtaining cam info message')
    cam_info = yield cam_info_sub.get_next_message()
    model = PinholeCameraModel()
    model.fromCameraInfo(cam_info)

    enable_service = sub.nh.get_service_client("/dice/enable", SetBool)
    yield enable_service(SetBoolRequest(data=True))

    dice_sub = yield sub.nh.subscribe('/dice/points', Point)

    found = {}
    history_tf = {}
    while len(found) != 2:
        fprint('Getting dice xy')
        dice_xy = yield dice_sub.get_next_message()
        found[dice_xy.z] = mil_ros_tools.rosmsg_to_numpy(dice_xy)[:2]
        fprint(found)
        out = yield get_transform(sub, model, found[dice_xy.z])
        history_tf[dice_xy.z] = out
        if len(found) > 1:
            tmp = found.values()[0] - found.values()[1]
            # Make sure the two points are at least 100 pixels off
            diff = np.hypot(tmp[0], tmp[1])
            fprint('Distance between buoys {}'.format(diff))
            if diff < 40:
                found = {}

    yield enable_service(SetBoolRequest(data=False))

    start = sub.move.zero_roll_and_pitch()
    yield start.go()

    for i in range(2):
        fprint('Hitting dice {}'.format(i))
        # Get one of the dice
        dice, xy = found.popitem()
        fprint('dice {}'.format(dice))
        ray, base = history_tf[dice]

        where = base + 3 * ray

        fprint(where)
        fprint('Moving!', msg_color='yellow')
        fprint('Current position: {}'.format(sub.pose.position))
        fprint('zrp')
        yield sub.move.zero_roll_and_pitch().go(blind=True)
        yield sub.nh.sleep(4)
        fprint('hitting', msg_color='yellow')
        yield sub.move.look_at(where).go(blind=True, speed=SPEED)
        yield sub.nh.sleep(4)
        yield sub.move.set_position(where).go(blind=True, speed=SPEED)
        yield sub.nh.sleep(4)
        fprint('going back', msg_color='yellow')
        yield start.go(blind=True, speed=SPEED)
        yield sub.nh.sleep(4)
Exemple #52
0
    def calibrate(self, image, info, depth):

        model = PinholeCameraModel()
        model.fromCameraInfo(info)

        try:
            cv_img = self.bridge.imgmsg_to_cv2(image, "bgr8")
        except CvBridgeError as e:
            print (e)
            return False

        try:
            cv_depth = self.bridge.imgmsg_to_cv2(depth)
        except CvBridgeError as e:
            print (e)
            return False

        cv_depth = cv2.medianBlur(cv_depth, 5)
        cv_img = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)

        ret, corners = cv2.findChessboardCorners(
            cv_img,
            (9, 6),
            None,
            flags=cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_FILTER_QUADS | cv2.CALIB_CB_NORMALIZE_IMAGE,
        )

        if not ret:

            rospy.logerr("Could not find chessboard corners")
            return False

        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.001)
        cv2.cornerSubPix(cv_img, corners, (11, 11), (-1, -1), criteria)
        corners = corners.reshape(1, -1, 2)[0]

        points = []
        ppoints = []

        ppp = PoseArray()
        ppp.header.stamp = rospy.Time.now()
        ppp.header.frame_id = self.world_frame

        for c in corners:

            pt = list(model.projectPixelTo3dRay((c[0], c[1])))
            pt[:] = [x / pt[2] for x in pt]

            # depth image is noisy - let's make mean of few pixels
            da = []
            for x in range(int(c[0]) - 2, int(c[0]) + 3):
                for y in range(int(c[1]) - 2, int(c[1]) + 3):
                    da.append(cv_depth[y, x] / 1000.0)

            d = np.mean(da)
            pt[:] = [x * d for x in pt]

            ps = PointStamped()
            ps.header.stamp = rospy.Time(0)
            ps.header.frame_id = image.header.frame_id
            ps.point.x = pt[0]
            ps.point.y = pt[1]
            ps.point.z = pt[2]

            # transform 3D point from camera into the world coordinates
            try:
                ps = self.tfl.transformPoint(self.world_frame, ps)
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                rospy.logerr("can't get transform")
                return False

            pp = Pose()
            pp.position.x = ps.point.x
            pp.position.y = ps.point.y
            pp.position.z = ps.point.z
            pp.orientation.x = 0
            pp.orientation.y = 0
            pp.orientation.z = 0
            pp.orientation.w = 1.0

            ppp.poses.append(pp)

            # store x,y -> here we assume that points are 2D (on tabletop)
            points.append([self.rpm * ps.point.x, self.rpm * ps.point.y])

        self.corners_pub.publish(ppp)

        dx = (self.pix_label.width() - self.pix_label.pixmap().width()) / 2.0
        dy = (self.pix_label.height() - self.pix_label.pixmap().height()) / 2.0
        box_size = self.pix_label.pixmap().width() / 12.0

        # TODO self.scene_origin ???
        # generate requested table coordinates
        for y in range(0, 6):
            for x in range(0, 9):

                px = 2 * box_size + x * box_size + dx
                py = 2 * box_size + y * box_size + dy

                ppoints.append([px, py])

        h, status = cv2.findHomography(np.array(points), np.array(ppoints), cv2.LMEDS)

        self.h_matrix = np.matrix(h)
        # self.h_matrix = np.matrix([[1,  0,  0], [0,  1,  0], [0,  0, 1.0]])

        # store homography matrix to parameter server
        s = str(self.h_matrix.tolist())
        rospy.set_param("/art/interface/projected_gui/projector/" + self.proj_id + "/calibration_matrix", s)
        print s

        return True
class color_controller():
    def __init__(self, publish_tf):
        # To take our Ros Image into a cv message and subsequently a numpy array.
        self.bridge = CvBridge()        

        # To make the pixel to vector projection
        self.cam_model = PinholeCameraModel()

        # We need CameraInfo in order to use PinholeCameraModel below.
        rospy.Subscriber('camera_topic', CameraInfo, self.camera_callback)
        self.hasCameraInfo = False

        while not self.hasCameraInfo:
            print 'waiting on camera info.'
            rospy.sleep(0.5)

        # We are using a depth image to get depth information of what we're tracking.
        rospy.Subscriber('depth_image', Image, self.depth_callback)

        # This package is just an extension of cmvision to provide tf tracking of the blobs provided by cmvision. 
        rospy.Subscriber('blobs', Blobs, self.blob_callback)

        # Subscribe to image for debugging.
        # rospy.Subscriber('thing', Image, self.image_callback)
        self.listener = tf.TransformListener()
        self.broadcaster = tf.TransformBroadcaster()

        # Republish each blob as part of a blob.
        self.blob_pub = rospy.Publisher('/blobs_3d', Blobs3d, queue_size=5)

        self.publish_tf = publish_tf



        # blobs is received from running cmvision. It's color blobs as defined by our color file.
    def blob_callback(self, blobs):
        # array of our color_models.
        self.colors = {}
        blobs3d = Blobs3d()

        for blob in blobs.blobs:
            # If we have multiple catches of a single color, we only want to publish the one with the maximum area. Fortunately they are sorted as such.
            already_published = False
            if blob.name in self.colors:
                already_published = True

            self.colors[blob.name] = color_model(blob, self.camera_info, self.parent_frame, self.depth_image, self.cam_model, self.listener, self.broadcaster)
            
            # Make sure this blob isn't shitty.
            if self.colors[blob.name].validate():

                # Publish to tf if master wishes it upon Dobby the programming elf.
                if self.publish_tf and not already_published:
                    self.colors[blob.name].publish()

                # 3d blobs that are in this blobs3d list are published to the /blobs_3d topic. 
                blobs3d.blobs.append(self.colors[blob.name].toBlob3d())

        blobs3d.header.frame_id = self.parent_frame
        blobs3d.header.stamp = rospy.Time.now() 
        blobs3d.blob_count = len(blobs3d.blobs) 
        self.blob_pub.publish(blobs3d) 


    def depth_callback(self, image):
        image_cv = self.bridge.imgmsg_to_cv2(image, image.encoding)
        image_cv2 = np.array(image_cv, dtype=np.float32)
        self.depth_image = image_cv2


    def camera_callback(self, camera_info):
        if not self.hasCameraInfo:
            self.cam_model.fromCameraInfo(camera_info)
            self.camera_info = camera_info
            self.parent_frame = self.camera_info.header.frame_id
        self.hasCameraInfo = True
class ScanTheCodePerception(object):
    """Class that handles ScanTheCodePerception."""

    def __init__(self, my_tf, debug, nh):
        """Initialize ScanTheCodePerception class."""
        self.image_cache = deque()
        self.bridge = CvBridge()
        self.nh = nh
        self.last_cam_info = None
        self.debug = debug
        self.pers_points = []
        self.model_tracker = ScanTheCodeModelTracker()
        self.camera_model = PinholeCameraModel()
        self.my_tf = my_tf
        self.rect_finder = RectangleFinderClustering()
        self.color_finder = ColorFinder()

        self.count = 0
        self.depths = []

    @txros.util.cancellableInlineCallbacks
    def _init(self):
        self.vel_sub = yield self.nh.subscribe("/velodyne_points", PointCloud2)
        self.image_sub = yield self.nh.subscribe("/stereo/right/image_rect_color", Image)
        defer.returnValue(self)

    def add_image(self, image):
        """Add an image to the image cache."""
        if len(self.image_cache) > 50:
            self.image_cache.popleft()
        self.image_cache.append(image)

    def update_info(self, info):
        """Update the camera calibration info."""
        self.last_cam_info = info
        self.camera_model.fromCameraInfo(info)

    def _get_top_left_point(self, points_3d):
        xmin = sys.maxint
        zmin = -sys.maxint
        ymin = sys.maxint
        for i, point in enumerate(points_3d):
            if point[1] < ymin:
                xmin = point[0]
                zmin = point[2]
                ymin = point[1]

        buff = 1
        for i, point in enumerate(points_3d):
            if(point[1] < ymin + buff and point[1] > ymin - buff and point[0] < xmin):
                xmin = point[0]
                zmin = point[2]
        return xmin, ymin, zmin

    def _get_points_in_range(self, axis, lower, upper, points):
        in_range = []
        idx = 0
        if axis == 'y':
            idx = 1
        if axis == 'z':
            idx = 2
        for p in points:
            if p[idx] > lower and p[idx] < upper:
                in_range.append(p)

        return in_range

    def _get_depth(self, axis, points_3d):
        idx = 0
        if axis == 'y':
            idx = 1
        if axis == 'z':
            idx = 2
        min_val, max_val = sys.maxint, -sys.maxint
        points_3d = np.array(points_3d)
        my_points = points_3d[:, idx]
        mean = np.mean(my_points)
        std = 1.5 * np.std(my_points)
        for p in my_points:
            if abs(p - mean) > std:
                # print p, mean
                continue
            if p < min_val:
                min_val = p
            if p > max_val:
                max_val = p

        return max_val - min_val

    def _get_2d_points_stc(self, points_3d):
        # xmin, ymin, zmin = self._get_top_left_point(points_3d)
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 10, 1.0)
        points_3d = np.float32(points_3d)
        ret, label, centers = cv2.kmeans(np.array(points_3d), 2, criteria, 10, 0)
        data = Counter(label.flatten())
        max_label = data.most_common(1)[0][0]
        c = centers[max_label]
        xmin, ymin, zmin = c[0], c[1], c[2]

        points_3d = [[xmin - .3, ymin - .3, zmin], [xmin + .3, ymin + .3, zmin],
                     [xmin - .3, ymin + .3, zmin], [xmin + .3, ymin - .3, zmin]]

        points_2d = map(lambda x: self.camera_model.project3dToPixel(x), points_3d)
        return points_2d

    def _get_bounding_rect(self, points_2d, img):
        xmin = np.inf
        xmax = -np.inf
        ymin = np.inf
        ymax = -np.inf
        h, w, r = img.shape
        for i, point in enumerate(points_2d):
            if(point[0] < xmin):
                xmin = point[0]
            if(point[0] > xmax):
                xmax = point[0]
            if(point[1] > ymax):
                ymax = point[1]
            if(point[1] < ymin):
                ymin = point[1]
        if xmin < 0:
            xmin = 1
        if ymin < 0:
            ymin = 1
        if xmax > w:
            xmax = w - 1
        if ymax > h:
            ymax = h - 1
        return xmin, ymin, xmax, ymax

    @txros.util.cancellableInlineCallbacks
    def get_stc_points(self, msg, stc_pos):
        trans = yield self.my_tf.get_transform("/stereo_right_cam", "/velodyne", msg.header.stamp)
        trans1 = yield self.my_tf.get_transform("/stereo_right_cam", "/enu", msg.header.stamp)
        stc_pos = rosmsg_to_numpy(stc_pos)
        stc_pos = np.append(stc_pos, 1)
        position = trans1.as_matrix().dot(stc_pos)
        if position[3] < 1E-15:
            raise ZeroDivisionError
        position[0] /= position[3]
        position[1] /= position[3]
        position[2] /= position[3]
        position = position[:3]

        stereo_points = []
        for point in pc2.read_points(msg, skip_nans=True):
            stereo_points.append(np.array([point[0], point[1], point[2], 1]))
        stereo_points = map(lambda x: trans.as_matrix().dot(x), stereo_points)
        points = []
        for p in stereo_points:
            if p[3] < 1E-15:
                raise ZeroDivisionError
            p[0] /= p[3]
            p[1] /= p[3]
            p[2] /= p[3]
            points.append(p[:3])

        points_keep = []
        for p in points:
            # print npl.norm(p - poition)
            if npl.norm(p - position) < 20:
                points_keep.append(p)
        points_keep = sorted(points_keep, key=lambda x: x[1])
        keep_num = int(.1 * len(points_keep))
        points_keep = points_keep[:keep_num]
        # self.pers_points.extend(points_keep)
        # max_num = 200
        # if len(self.pers_points) > max_num:
        #     self.pers_points = self.pers_points[len(self.pers_points) - max_num:len(self.pers_points)]

        defer.returnValue(points_keep)

    @txros.util.cancellableInlineCallbacks
    def search(self, scan_the_code):
        """Search for the colors in the scan the code object."""
        pntcloud = yield self.vel_sub.get_next_message()
        image_ros = yield self.image_sub.get_next_message()
        try:
            image = self.bridge.imgmsg_to_cv2(image_ros, "bgr8")
        except CvBridgeError:
            print "Trouble converting image"
            defer.returnValue((False, None))

        points_3d = yield self.get_stc_points(pntcloud, scan_the_code.position)

        image_clone = image.copy()

        # points_3d = yield self._get_3d_points_stereo(scan_the_code.points, image_ros.header.stamp)
        # points_2d = map(lambda x: self.camera_model.project3dToPixel(x), points_3d)
        points_2d = map(lambda x: self.camera_model.project3dToPixel(x), points_3d)
        for p in points_2d:
            po = (int(round(p[0])), int(round(p[1])))
            cv2.circle(image_clone, po, 2, (0, 255, 0), -1)

        points_2d = self._get_2d_points_stc(points_3d)
        xmin, ymin, xmax, ymax = self._get_bounding_rect(points_2d, image)
        xmin, ymin, xmax, ymax = int(xmin), int(ymin), int(xmax), int(ymax)

        cv2.rectangle(image_clone, (xmin, ymin), (xmax, ymax), (0, 255, 0), 2)
        self.debug.add_image(image_clone, "bounding_box", topic="bounding_box")

        # defer.returnValue((False, None))

        print xmin, ymin, xmax, ymax
        roi = image[ymin:ymax, xmin:xmax]
        succ, color_vec = self.rect_finder.get_rectangle(roi, self.debug)
        if not succ:
            defer.returnValue((False, None))

        self.mission_complete, colors = self.color_finder.check_for_colors(image, color_vec, self.debug)
        if self.mission_complete:
            print "MISSION COMPLETE"
            defer.returnValue((True, colors))
        defer.returnValue((False, None))

    @txros.util.cancellableInlineCallbacks
    def correct_pose(self, scan_the_code):
        """Check to see if we are looking at the corner of scan the code."""
        self.count += 1
        pntcloud = yield self.vel_sub.get_next_message()
        points_3d = yield self.get_stc_points(pntcloud, scan_the_code.position)
        xmin, ymin, zmin = self._get_top_left_point(points_3d)
        points_oi = self._get_points_in_range('y', ymin - .1, ymin + .2, points_3d)
        if len(points_oi) == 0:
            defer.returnValue(False)

        image_ros = yield self.image_sub.get_next_message()
        image_ros = self.bridge.imgmsg_to_cv2(image_ros, "bgr8").copy()

        depth = self._get_depth('z', points_oi)
        fprint("DEPTH: {}".format(depth), msg_color="green")
        self.depths.append(depth)
        # %%%%%%%%%%%%%%%%%%%%%%%%DEBUG
        cv2.putText(image_ros, str(depth), (10, 30), 1, 2, (0, 0, 255))
        self.debug.add_image(image_ros, "in_range", topic="in_range")
        # %%%%%%%%%%%%%%%%%%%%%%%%DEBUG
        if depth > .10:
            defer.returnValue(False)
        defer.returnValue(True)
    def handle_img_msg(self, img_msg):

        now = rospy.get_rostime()
        for m in self.markerArray.markers:
            m.action = Marker.DELETE

        img = None
        bridge = cv_bridge.CvBridge()
        try:
            img = bridge.imgmsg_to_cv2(img_msg, 'bgr8')
        except cv_bridge.CvBridgeError as e:
            rospy.logerr( 'image message to cv conversion failed :' )
            rospy.logerr( e )
            print( e )
            return

        timestamp = img_msg.header.stamp.to_nsec()
        self.frame_index = self.timestamp_map[timestamp]
        if self.offset :
            self.frame_index -= self.offset
        out_img = np.copy(img)
        for i, f in enumerate(self.frame_map[self.frame_index]):
            #f = self.frame_map[self.frame_index][0]
            #color = (255,255,0)
            color = kelly_colors_list[i % len(kelly_colors_list)]
            marker = self.create_marker() 
            marker.color.r = color[0]/255
            marker.color.g = color[1]/255
            marker.color.b = color[2]/255
            marker.color.a = 0.5
        
            marker.header.stamp = now
            marker.pose.position = Point(*f.trans)
            marker.pose.orientation = Quaternion(*f.rotq)

            marker.id = i
   
            self.markerArray.markers.append(marker) 
            #self.markOutput.publish(marker)
            

            md = self.metadata
            dims = np.array([md['l'], md['w'], md['h']])
            obs_centroid = np.array(f.trans)
            orient = list(f.rotq)
            #orient[2] -= 0.035
            #R = tf.transformations.quaternion_matrix((0,0,-0.0065,1))
            #obs_centroid = R.dot(list(obs_centroid)+[1])[:3]
    
   
            if obs_centroid is None:
                rospy.loginfo("Couldn't find obstacle centroid")
                continue
                #self.imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
                #return
            
            # print centroid info
            rospy.loginfo(str(obs_centroid))

            # case when obstacle is not in camera frame
            if obs_centroid[0]<3 :
                continue
                #self.imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
                #return
    
            # get bbox 
            R = tf.transformations.quaternion_matrix(orient)
            #R = tf.transformations.quaternion_matrix([0,0,0,1])
            corners = [0.5*np.array([i,j,k])*dims for i in [-1,1] 
                        for j in [-1,1] for k in [-1,1]]
            corners = np.array([obs_centroid + R.dot(list(c)+[1])[:3] for c in corners])
            #if self.ground_corr is not None:
            #    z_min, x_min, y_min = self.ground_corr.loc[timestamp]
            #    z_offset = z_min - min(corners[:,2])
            #    x_offset = x_min - min(corners[:,0])
            #    y_offset = y_min - min(corners[:,1])
            #    corr = np.array([0, 0, z_offset])
            #    #corr = np.array([0, 0,0])
            #    #corr = np.array([x_offset, y_offset, z_offset])
            #    corr[np.isnan(corr)]=0
            #    corners+=corr
            #print(corners)
            cameraModel = PinholeCameraModel()
            cam_info = load_cam_info(self.calib_file)
            cameraModel.fromCameraInfo(cam_info)
            projected_pts = [cameraModel.project3dToPixel(list(pt)+[1]) for pt in corners]
            projected_pts = np.array(projected_pts)
            center = np.mean(projected_pts, axis=0)
            out_img = drawBbox(out_img, projected_pts, color=color[::-1])

        for i, f in enumerate(self.frame_map_corrected[self.frame_index]):
            f = self.frame_map[self.frame_index][0]
            color = (255,255,0)
            color = kelly_colors_list[i % len(kelly_colors_list)]
            marker = self.create_marker()
            marker.color.r = 128
            marker.color.g = 0
            marker.color.b = 0
            marker.color.a = 0.5

            marker.header.stamp = now
            marker.pose.position = Point(*f.trans)
            marker.pose.orientation = Quaternion(*f.rotq)

            marker.id = i

            self.markerArray.markers.append(marker)
            # self.markOutput.publish(marker)


            md = self.metadata
            dims = np.array([md['l'], md['w'], md['h']])
            obs_centroid = np.array(f.trans)
            orient = list(f.rotq)
            # orient[2] -= 0.035
            # R = tf.transformations.quaternion_matrix((0,0,-0.0065,1))
            # obs_centroid = R.dot(list(obs_centroid)+[1])[:3]


            if obs_centroid is None:
                rospy.loginfo("Couldn't find obstacle centroid")
                continue
                # self.imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
                # return

            # print centroid info
            rospy.loginfo(str(obs_centroid))

            # case when obstacle is not in camera frame
            if obs_centroid[0] < 3:
                continue
                # self.imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
                # return

            # get bbox
            R = tf.transformations.quaternion_matrix(orient)
            # R = tf.transformations.quaternion_matrix([0,0,0,1])
            corners = [0.5 * np.array([i, j, k]) * dims for i in [-1, 1]
                       for j in [-1, 1] for k in [-1, 1]]
            corners = np.array([obs_centroid + R.dot(list(c) + [1])[:3] for c in corners])
            # if self.ground_corr is not None:
            #    z_min, x_min, y_min = self.ground_corr.loc[timestamp]
            #    z_offset = z_min - min(corners[:,2])
            #    x_offset = x_min - min(corners[:,0])
            #    y_offset = y_min - min(corners[:,1])
            #    corr = np.array([0, 0, z_offset])
            #    #corr = np.array([0, 0,0])
            #    #corr = np.array([x_offset, y_offset, z_offset])
            #    corr[np.isnan(corr)]=0
            #    corners+=corr
            # print(corners)
            cameraModel = PinholeCameraModel()
            cam_info = load_cam_info(self.calib_file)
            cameraModel.fromCameraInfo(cam_info)
            projected_pts = [cameraModel.project3dToPixel(list(pt) + [1]) for pt in corners]
            projected_pts = np.array(projected_pts)
            center = np.mean(projected_pts, axis=0)
            out_img = drawBbox(out_img, projected_pts, color=color[::-1])

        self.markOutput.publish(self.markerArray)
        self.imgOutput.publish(bridge.cv2_to_imgmsg(out_img, 'bgr8'))
Exemple #56
0
class MarkerFinder():
    def __init__(self):
        self.tf_listener = tf.TransformListener()

        self.search = False
        self.last_image = None
        self.last_image_timestamp = None
        self.last_draw_image = None

        # This may need to be changed if you want to use a different image feed.
        self.image_sub = sub8_ros_tools.Image_Subscriber('/down/left/image_raw', self.image_cb)
        self.image_pub = sub8_ros_tools.Image_Publisher("vision/channel_marker/target_info")

        self.toggle = rospy.Service('vision/channel_marker/search', SetBool, self.toggle_search)

        self.cam = PinholeCameraModel()
        self.cam.fromCameraInfo(self.image_sub.wait_for_camera_info())
        self.rviz = rviz.RvizVisualizer()

        # self.occ_grid = MarkerOccGrid(self.image_sub, grid_res=.05, grid_width=500, grid_height=500,
        #                               grid_starting_pose=Pose2D(x=250, y=250, theta=0))
        if rospy.get_param("/orange_markers/use_boost"):
            path = os.path.join(rospack.get_path('sub8_perception'),
                'ml_classifiers/marker/' + rospy.get_param("/orange_markers/marker_classifier"))

            self.boost = cv2.Boost()
            rospy.loginfo("MARKER - Loading classifier from {}".format(path))
            self.boost.load(path)
            rospy.loginfo("MARKER - Classifier for marker loaded.")
        else:
            self.lower = np.array(rospy.get_param('/color/channel_marker/hsv_low'))
            self.upper = np.array(rospy.get_param('/color/channel_marker/hsv_high'))

        self.pose_service = rospy.Service("vision/channel_marker/pose", VisionRequest, self.request_marker)

        self.kernel = np.ones((15, 15), np.uint8)

        # Occasional status publisher
        self.timer = rospy.Timer(rospy.Duration(1), self.publish_target_info)

        print "MARKER - Got no patience for sittin' around!"


    def toggle_search(self, srv):
        if srv.data:
            rospy.loginfo("MARKER - Looking for markers now.")
            self.search = True
        else:
            rospy.loginfo("MARKER - Done looking for markers.")
            self.search = False

        return SetBoolResponse(success=srv.data)

    def publish_target_info(self, *args):
        if not self.search or self.last_image is None:
            return

        markers = self.find_marker(np.copy(self.last_image))
        #self.occ_grid.update_grid(self.last_image_timestamp)
        #self.occ_grid.add_marker(markers, self.last_image_timestamp)

        if self.last_draw_image is not None: #and (markers is not None):
            self.image_pub.publish(np.uint8(self.last_draw_image))

    def image_cb(self, image):
        '''Hang on to last image and when it was taken.'''
        self.last_image = image
        self.last_image_timestamp = self.image_sub.last_image_time

    def calculate_threshold(self, img, agression=.5):
        histr = cv2.calcHist([img], [0], None, [179], [0, 179])
        threshold = np.uint8((179 - np.argmax(histr)) * agression)
        return threshold

    def get_2d_pose(self, mask):
        # estimate covariance matrix and get corresponding eigenvectors
        wh = np.where(mask)[::-1]
        cov = np.cov(wh)
        eig_vals, eig_vects = np.linalg.eig(cov)

        # use index of max eigenvalue to find max eigenvector
        i = np.argmax(eig_vals)
        max_eigv = eig_vects[:, i] * np.sqrt(eig_vals[i])

        # flip indices to find min eigenvector
        min_eigv = eig_vects[:, 1 - i] * np.sqrt(eig_vals[1 - i])

        # define center of pipe
        center = np.average(wh, axis=1)

        # define vertical vector (sub's current direction)
        vert_vect = np.array([0.0, -1.0])

        if max_eigv[1] > 0:
            max_eigv = -max_eigv
            min_eigv = -min_eigv

        num = np.cross(max_eigv, vert_vect)
        denom = np.linalg.norm(max_eigv) * np.linalg.norm(vert_vect)
        angle_rad = np.arcsin(num / denom)

        return center, angle_rad, [max_eigv, min_eigv]

    def find_marker(self, img):
        #img[:, -100:] = 0
        img = cv2.GaussianBlur(img, (7, 7), 15)
        last_image_timestamp = self.last_image_timestamp

        if rospy.get_param("/orange_markers/use_boost"):
            some_observations = machine_learning.boost.observe(img)
            prediction = [int(x) for x in [self.boost.predict(obs) for obs in some_observations]]
            mask = np.reshape(prediction, img[:, :, 2].shape).astype(np.uint8) * 255
        else:
            hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
            mask = cv2.inRange(hsv, self.lower, self.upper)


        kernel = np.ones((5,5),np.uint8)
        mask = cv2.dilate(mask, kernel, iterations = 1)
        mask = cv2.erode(mask, kernel, iterations = 2)

        #mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, self.kernel)
        contours, _ = cv2.findContours(np.copy(mask), cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
        if len(contours) < 1:
            rospy.logwarn("MARKER - No marker found.")
            return None

        # Find biggest area contour
        self.last_draw_image = np.dstack([mask] * 3)
        areas = [cv2.contourArea(c) for c in contours]
        max_index = np.argmax(areas)
        cnt = contours[max_index]

        # Draw a miniAreaRect around the contour and find the area of that.
        rect = cv2.minAreaRect(cnt)
        box = cv2.cv.BoxPoints(rect)
        box = np.int0(box)
        mask = np.zeros(shape=mask.shape)
        cv2.drawContours(self.last_draw_image, [box], 0, (0, 128, 255), 2)
        cv2.drawContours(mask, [box], 0, 255, -1)
        rect_area = cv2.contourArea(box)

        center, angle_rad, [max_eigv, min_eigv] = self.get_2d_pose(mask)

        cv2.line(self.last_draw_image, tuple(np.int0(center)), tuple(np.int0(center + (2 * max_eigv))), (0, 255, 30), 2)
        cv2.line(self.last_draw_image, tuple(np.int0(center)), tuple(np.int0(center + (2 * min_eigv))), (0, 30, 255), 2)

        # Check if the box is too big or small.
        xy_position, height = self.get_tf(timestamp=last_image_timestamp)
        expected_area = self.calculate_marker_area(height)

        print "{} < {} < {} ({})".format(expected_area * .2, rect_area, expected_area * 2, expected_area)
        if expected_area * .5 < rect_area < expected_area * 2:
            #cv2.drawContours(self.last_draw_image, [box], 0, (255, 255, 255), -1)
            self.rviz.draw_ray_3d(center, self.cam, np.array([1, .5, 0, 1]), frame='/downward',
                _id=5, length=height, timestamp=last_image_timestamp)
        else:
            angle_rad = 0
            max_eigv = np.array([0, -20])
            min_eigv = np.array([-20, 0])
            #cv2.drawContours(self.last_draw_image, [box], 0, (255, 0, 30), -1)
            rospy.logwarn("MARKER - Size out of bounds!")

        self.rviz.draw_ray_3d(center, self.cam, np.array([1, .5, 0, 1]), frame='/downward',
            _id=5, length=height, timestamp=last_image_timestamp)

        # Convert to a 3d pose to move the sub to.
        abs_position = self.transform_px_to_m(center, last_image_timestamp)
        q_rot = tf.transformations.quaternion_from_euler(0, 0, angle_rad)

        return numpy_quat_pair_to_pose(abs_position, q_rot)

    def request_marker(self, data):
        if self.last_image is None:
            return False  # Fail if we have no images cached

        timestamp = self.last_image_timestamp
        goal_pose = self.find_marker(self.last_image)
        found = (goal_pose is not None)

        if not found:
            resp = VisionRequestResponse(
                found=found
            )
        else:
            resp = VisionRequestResponse(
                pose=PoseStamped(
                    header=Header(
                        stamp=timestamp,
                        frame_id='/down_camera'),
                    pose=goal_pose
                ),
                found=found
            )
        return resp

    def get_tf(self, timestamp=None, get_rotation=False):
        '''
        x_y position, height in meters and quat rotation of the sub if requested
        '''
        if timestamp is None:
            timestamp = rospy.Time()

        self.tf_listener.waitForTransform("/map", "/downward", timestamp, rospy.Duration(5.0))
        trans, rot = self.tf_listener.lookupTransform("/map", "/downward", timestamp)
        x_y_position = trans[:2]
        self.tf_listener.waitForTransform("/ground", "/downward", timestamp, rospy.Duration(5.0))
        trans, _ = self.tf_listener.lookupTransform("/ground", "/downward", timestamp)

        height = np.nan_to_num(trans[2])
        x_y_position = np.nan_to_num(x_y_position)

        if get_rotation:
            return x_y_position, rot, height

        return x_y_position, height

    def transform_px_to_m(self, m_position, timestamp):
        '''
        Finds the absolute position of the marker in meters.
        '''
        xy_position, q, height = self.get_tf(timestamp, get_rotation=True)

        dir_vector = unit_vector(np.array([self.cam.cx(), self.cam.cy()]) - m_position)
        cam_rotation = tf.transformations.euler_from_quaternion(q)[2] + np.pi / 2
        print "MARKER - dir_vector:", dir_vector
        print "MARKER - cam_rotation:", cam_rotation
        dir_vector = np.dot(dir_vector, make_2D_rotation(cam_rotation))

        # Calculate distance from middle of frame to marker in meters.
        magnitude = self.calculate_visual_radius(height, second_point=m_position)
        abs_position = np.append(xy_position + dir_vector[::-1] * magnitude, -SEARCH_DEPTH)

        return abs_position

    def calculate_visual_radius(self, height, second_point=None):
        '''
        Draws rays to find the radius of the FOV of the camera in meters.
        It also can work to find the distance between two planar points some distance from the camera.
        '''

        mid_ray = np.array([0, 0, 1])

        if second_point is None:
            if self.cam.cy() < self.cam.cx():
                second_point = np.array([self.cam.cx(), 0])
            else:
                second_point = np.array([0, self.cam.cy()])

        edge_ray = unit_vector(self.cam.projectPixelTo3dRay(second_point))

        # Calculate angle between vectors and use that to find r
        theta = np.arccos(np.dot(mid_ray, edge_ray))
        return np.tan(theta) * height

    def calculate_marker_area(self, height):
        '''
        What we really don't want is to find markers that are on the edge since the direction and center of
            the marker are off.
        '''
        MARKER_LENGTH = 1.22  # m
        MARKER_WIDTH = .1524  # m

        # Get m/px on the ground floor.
        m = self.calculate_visual_radius(height)
        if self.cam.cy() < self.cam.cx():
            px = self.cam.cy()
        else:
            px = self.cam.cx()

        m_px = m / px
        marker_area_m = MARKER_WIDTH * MARKER_LENGTH
        marker_area_px = marker_area_m / (m_px ** 2)

        return marker_area_px
class Colorama(object):
    def __init__(self):
        info_topic = camera_root + "/camera_info"
        image_topic = camera_root + "/image_rect_color"

        self.tf_listener = tf.TransformListener()
        self.status_pub = rospy.Publisher("/database_color_status", ColoramaDebug, queue_size=1)

        self.odom = None 
        set_odom = lambda msg: setattr(self, "odom", navigator_tools.pose_to_numpy(msg.pose.pose))
        rospy.Subscriber("/odom", Odometry, set_odom)
        fprint("Waiting for odom...")
        while self.odom is None and not rospy.is_shutdown():
            rospy.sleep(1)
        fprint("Odom found!", msg_color='green')

        db_request = rospy.ServiceProxy("/database/requests", ObjectDBQuery)
        self.make_request = lambda **kwargs: db_request(ObjectDBQueryRequest(**kwargs))

        self.image_history = ImageHistory(image_topic)

        # Wait for camera info, and exit if not found
        fprint("Waiting for camera info on: '{}'".format(info_topic))
        while not rospy.is_shutdown():
            try:
                camera_info_msg = rospy.wait_for_message(info_topic, CameraInfo, timeout=3)
            except rospy.exceptions.ROSException:
                rospy.sleep(1)
                continue
            break

        fprint("Camera info found!", msg_color="green")
        self.camera_model = PinholeCameraModel()
        self.camera_model.fromCameraInfo(camera_info_msg)

        self.debug = DebugImage("/colorama/debug", prd=.5)

        # These are color mappings from Hue values [0, 360]
        self.color_map = {'red': np.radians(0), 'yellow': np.radians(60),
                          'green': np.radians(120), 'blue': np.radians(200)}

        # RGB map for database setting
        self.database_color_map = {'red': (255, 0, 0), 'yellow': (255, 255, 0), 'green': (0, 255, 0), 'blue': (0, 0, 255)}

        # ========= Some tunable parameters ===================================
        self.update_time = 1  # s

        # Observation parameters
        self.saturation_reject = 20 # Reject color obs with below this saturation
        self.value_reject = 50      # Reject color obs with below this value
        self.height_remove = 0.4    # Remove points that are this percent down on the object (%)
                                    # 1 keeps all, .4 removes the bottom 40%
        # Update parameters
        self.history_length = 100   # How many of each color to keep
        self.min_obs = 5            # Need atleast this many observations before making a determination
        self.conf_reject = .5       # When to reject an observation based on it's confidence

        # Confidence weights
        self.v_factor = 0.6         # Favor value values close to the nominal value
        self.v_u = 200              # Mean of norm for variance error
        self.v_sig = 60             # Sigma of norm for variance error
        self.dist_factor = 0.3      # Favor being closer to the totem
        self.dist_sig = 30          # Sigma of distance (m)
        self.q_factor = 0           # Favor not looking into the sun
        self.q_sig = 1.2            # Sigma of norm for quaternion error (rads)
        
        # Maps id => observations
        self.colored = {}

        rospy.Timer(ros_t(self.update_time), self.do_observe)

    def _compute_average_angle(self, angles, weights):
        """
        Returns weighted average of angles.
        Tends to break down with too many angles 180 degrees of each other - try not to do that.
        """
        angles = np.array(angles)
        if np.linalg.norm(weights) == 0:
            return None

        w = weights / np.linalg.norm(weights)
        s = np.sum(w * np.sin(angles))
        c = np.sum(w * np.cos(angles))
        avg_angle = np.arctan2(s, c)
        return avg_angle
   
    def _get_quaternion_error(self, q, target_q):
        """
        Returns an angluar differnce between q and target_q in radians
        """
        dq = trns.quaternion_multiply(np.array(target_q), trns.quaternion_inverse(np.array(q)))
        return 2 * np.arccos(dq[3])

    def _get_closest_color(self, hue_angle):
        """
        Returns a pair of the most likely color and the radian error associated with that prediction
            `hue_angle` := The radian value associated with hue [0, 2*pi] 
        Colors are found from `self.color_map`
        """
        c = np.cos(hue_angle)
        s = np.sin(hue_angle)
        error = np.inf
        likely_color = 'undef'
        for color, h_val in self.color_map.iteritems():
            cg = np.cos(h_val)
            sg = np.sin(h_val)
            # We need a signed error for some math later on so no absolute value
            this_error = np.arctan2(sg*c - cg*s, cg*c + sg*s)
            if np.abs(this_error) < np.abs(error):
                error = this_error
                likely_color = color

        fprint("Likely color: {} with an hue error of {} rads.".format(likely_color, np.round(error, 3)))
        return [likely_color, error] 

    def do_estimate(self, totem_id):
        """Calculates best color estimate for a given totem id"""
        fprint("DOING ESTIMATE", msg_color='blue') 
        if totem_id not in self.colored:
            fprint("Totem ID {} not found!".format(totem_id), msg_color='red')
            return None
       
        t_color = self.colored[totem_id]
        
        if len(t_color) < self.min_obs:
            fprint("Only {} observations. {} required.".format(len(t_color), self.min_obs), msg_color='red')
            return None

        kwargs = {'v_u': self.v_u, 'v_sig': self.v_sig, 'dist_sig': self.dist_sig, 
                  'q_factor': self.q_factor, 'q_sig': self.q_sig}

        w, weights = t_color.compute_confidence([self.v_factor, self.dist_factor, self.q_factor], True, **kwargs)
        fprint("CONF: {}".format(w))
        if np.mean(w) < self.conf_reject:
            return None
        
        hue_angles = np.radians(np.array(t_color.hues) * 2) 
        angle = self._compute_average_angle(hue_angles, w)
        color = self._get_closest_color(angle)
        
        msg = t_color.as_message 
        msg.id = totem_id
        msg.confidence = w
        msg.labels = ["value_errs", "dists", "q_diffs"]
        msg.weights = weights
        msg.color = colors[0]
        msg.est_hues = angle * 2
        msg.hues = np.array(t_color.hues) * 2
        self.status_pub.publish(msg)

        fprint("Color: {}".format(color[0]))
        return color
    
    def got_request(self, req):
        # Threading blah blah something unsafe
        colored_ids = [_id for _id, color_err in self.colored.iteritems() if self.valid_color(_id) == req.color]
        
        fprint("Colored IDs: {}".format(colored_ids), msg_color='blue')
        print '\n' * 50
        if len(colored_ids) == 0:
            return ColorRequestResponse(found=False)
        
        return ColorRequestResponse(found=True, ids=colored_ids)

    def do_observe(self, *args):
        resp = self.make_request(name='totem')
        
        # If atleast one totem was found start observing
        if resp.found:
            # Time of the databse request
            time_of_marker = resp.objects[0].header.stamp# - ros_t(1)
            fprint("Looking for image at {}".format(time_of_marker.to_sec()), msg_color='yellow')
            image_holder = self.image_history.get_around_time(time_of_marker)
            if not image_holder.contains_valid_image:
                t = self.image_history.newest_image.time
                if t is None:
                    fprint("No images found.")
                    return
                
                fprint("No valid image found for t={} ({}) dt: {}".format(time_of_marker.to_sec(), t.to_sec(), (rospy.Time.now() - t).to_sec()), msg_color='red')
                return
            header = navigator_tools.make_header(frame='/enu', stamp=image_holder.time)
            image = image_holder.image
            self.debug.image = np.copy(image)

            cam_tf = self.camera_model.tfFrame()
            try:
                fprint("Getting transform between /enu and {}...".format(cam_tf))
                self.tf_listener.waitForTransform("/enu", cam_tf, time_of_marker, ros_t(1))
                t_mat44 = self.tf_listener.asMatrix(cam_tf, header)
            except tf.ExtrapolationException as e:
                fprint("TF error found and excepted: {}".format(e))
                return

            for obj in resp.objects:
                if len(obj.points) <= 1:
                    fprint("No points in object")
                    continue

                fprint("{} {}".format(obj.id, "=" * 50))
                
                # Get object position in px coordinates to determine if it's in frame
                object_cam = t_mat44.dot(np.append(p2np(obj.position), 1))
                object_px = map(int, self.camera_model.project3dToPixel(object_cam[:3]))
                if not self._object_in_frame(object_cam):
                    fprint("Object not in frame")
                    continue
                
                # Get enu points associated with this totem and remove ones that are too low
                points_np = np.array(map(p2np, obj.points))
                height = np.max(points_np[:, 2]) - np.min(points_np[:, 2])
                if height < .1:
                    # If the height of the object is too small, skip (units are meters)
                    fprint("Object too small")
                    continue

                threshold = np.min(points_np[:, 2]) + self.height_remove * height  
                points_np = points_np[points_np[:, 2] > threshold]
                
                # Shove ones in there to make homogenous points to get points in image frame
                points_np_homo = np.hstack((points_np, np.ones((points_np.shape[0], 1)))).T
                points_cam = t_mat44.dot(points_np_homo).T
                points_px = map(self.camera_model.project3dToPixel, points_cam[:, :3])
                
                [cv2.circle(self.debug.image, tuple(map(int, p)), 2, (255, 255, 255), -1) for p in points_px]
                
                # Get color information from the points
                roi = self._get_ROI_from_points(points_px)
                h, s, v = self._get_hsv_from_ROI(roi, image)

                # Compute parameters that go into the confidence
                boat_q = self.odom[1]
                target_q = self._get_solar_angle()
                q_err = self._get_quaternion_error(boat_q, target_q)
                
                dist = np.linalg.norm(self.odom[0] - p2np(obj.position))

                fprint("H: {}, S: {}, V: {}".format(h, s, v))
                fprint("q_err: {}, dist: {}".format(q_err, dist))

                # Add to database and setup debug image
                if s < self.saturation_reject or v < self.value_reject:
                    err_msg = "The colors aren't expressive enough s: {} ({}) v: {} ({}). Rejecting."
                    fprint(err_msg.format(s, self.saturation_reject, v, self.value_reject), msg_color='red')

                else:
                    if obj.id not in self.colored:
                       self.colored[obj.id] = Observation() 
                    
                    # Add this observation in
                    self.colored[obj.id] += h, v, dist, q_err
                    print self.colored[obj.id]

                rgb = (0, 0, 0)
                color = self.do_estimate(obj.id)
                # Do we have a valid color estimate
                if color:
                    fprint("COLOR IS VALID", msg_color='green')
                    rgb = self.database_color_map[color[0]]
                    
                    cmd = '{name}={rgb[0]},{rgb[1]},{rgb[2]},{_id}'
                    self.make_request(cmd=cmd.format(name=obj.name,_id=obj.id, rgb=rgb))

                bgr = rgb[::-1]
                cv2.circle(self.debug.image, tuple(object_px), 10, bgr, -1)
                font = cv2.FONT_HERSHEY_SIMPLEX
                cv2.putText(self.debug.image, str(obj.id), tuple(object_px), font, 1, bgr, 2)

    def _get_solar_angle(self):
        return [0, 0, 0, 1]

    def _get_ROI_from_points(self, image_points):
        cnt = np.array([image_points]).astype(np.float32)
        rect = cv2.minAreaRect(cnt)
        box = cv2.cv.BoxPoints(rect)
        box = np.int0(box)
        fprint("Drawing rectangle")
        cv2.drawContours(self.debug.image, [box], 0, (0, 0, 255), 2)
        return box

    def _get_hsv_from_ROI(self, roi, img):
        mask = np.zeros(img.shape[:2], np.uint8)
        cv2.drawContours(mask, [roi], 0, 255, -1)
        bgr = cv2.mean(img, mask)
        # We have to treat that bgr value as a 3d array to get cvtColor to work
        bgr = np.array([[bgr]])[:, :3].astype(np.uint8)
        h, s, v = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)[0, 0]

        return h, s, v
        
        # Now check that s and v are in a good range
        if s < self.saturation_reject or v < self.value_reject:
            err_msg = "The colors aren't expressive enough s: {} ({}) v: {} ({}). Rejecting."
            fprint(err_msg.format(s, self.saturation_reject, v, self.value_reject), msg_color='red')
            return None

        # Compute hue error in SO2
        hue_angle = np.radians(h * 2) 
        
        # Display the current values
        font = cv2.FONT_HERSHEY_SIMPLEX
        cv2.putText(self.debug.image, "h_val: {}".format(np.degrees(hue_angle)), 
                    tuple(roi[1]), font, 1, (255, 255, 255), 2)

        likely_color, error = self.get_closest_color(hue_angle)

        if error > self.hue_error_reject:
            fprint("Closest color was {} with an error of {} rads (> {}). Rejecting.".format(likely_color, np.round(error, 3), 
                                                                                            self.hue_error_reject), msg_color='red')
            return None

        fprint("Likely color: {} with an hue error of {} rads.".format(likely_color, np.round(error, 3)))
        return [likely_color, error]

    def _object_in_frame(self, object_point):
        """
        Returns if the object is in frame given the centroid of the object.
        `object_point` should be in the camera_model's frame.
        """
        if object_point[2] < 0:
            return False

        px = np.array(self.camera_model.project3dToPixel(object_point))
        resoultion = self.camera_model.fullResolution()
        return not (np.any([0, 0] > px) or np.any(px > resoultion))
class ceilingLocalizer():
	def __init__(self, image_topic, camera_topic, robot_depth, robot_frame, pattern):

		# A subscriber to the image topic
		rospy.Subscriber(image_topic, Image, self.image_callback)

		# To get which frame is the camera frame
		rospy.Subscriber(camera_topic, CameraInfo, self.camera_callback)

		# To make the pixel to vector projection
		self.camModel = PinholeCameraModel()

		# How far is the robot plane from the camera?
		self.robot_depth = robot_depth

		# What is the name of the frame we should publish?
		self.robot_frame = robot_frame

		# Pattern to match
		self.image_orig = cv2.imread(pattern, 1)
		image_bin = cv2.cvtColor(self.image_orig, cv2.COLOR_BGR2GRAY)
		ret, image_bin = cv2.threshold(image_bin, 127,255,cv2.THRESH_BINARY_INV)
		contours, hierarchy = cv2.findContours(image_bin, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
		sorted_contours = sorted(contours, key = cv2.contourArea, reverse = True)
		# Our pattern is the second contour because for some reason its finding the edges of our image as a contour
		self.pattern = sorted_contours[1]
		self.pattern_area = 200

		# To convert images from ROS to CV formats
		self.bridge = CvBridge()

		# Publish frames to tf
		self.broadcaster = tf.TransformBroadcaster()

		# Publisher for edited video
		self.video = rospy.Publisher('ceilingMarker', Image)

	def camera_callback(self, cameraInfo):
		self.cameraInfo = cameraInfo
		self.camModel.fromCameraInfo(cameraInfo)
		self.cameraFrame = cameraInfo.header.frame_id

	def image_callback(self, image_in):
		image_cv = self.bridge.imgmsg_to_cv(image_in, 'bgr8')
		image_cv2 = numpy.array(image_cv, dtype=numpy.uint8)
		#image_cv2 = cv2.GaussianBlur(image_cv2,(5,5), 100, 100)

		# Convert the image from BGR to HSV
		image_hsv_cv2 = cv2.cvtColor(image_cv2, cv2.COLOR_BGR2HSV)
		# Convert the image to grayscale
		image_gray = cv2.cvtColor(image_cv2, cv2.COLOR_BGR2GRAY)
		# Threshold grayscale image
		flag, thresh = cv2.threshold(image_gray, 230, 255, cv2.THRESH_BINARY)

		# Make a copy for contour detection
		thresh_copy = numpy.copy(thresh)

		contours, hierarchy = cv2.findContours(thresh_copy, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
		# Sort contours by how much they match our pattern
		potential_markers = self.find_potential_markers(contours)

		# Ranges in hsv
		lower_red_hsv = numpy.array([0,50,50])
		upper_red_hsv = numpy.array([10,255,255])
		lower_blue_hsv = numpy.array([105,50,50])
		upper_blue_hsv = numpy.array([120,255,255])
		lower_red_hsv2 = numpy.array([160,50,50])
		upper_red_hsv2 = numpy.array([179,255,255])
		# Make masks from our ranges
		mask_red_hsv = numpy.bitwise_or(cv2.inRange(image_hsv_cv2, lower_red_hsv, upper_red_hsv), cv2.inRange(image_hsv_cv2, lower_red_hsv2, upper_red_hsv2))
		mask_blue_hsv = cv2.inRange(image_hsv_cv2, lower_blue_hsv, upper_blue_hsv)

		# Front should be red, rear should be blue
		front_mask = mask_red_hsv
		rear_mask = mask_blue_hsv


		
		if len(potential_markers) >= 2:
			# Set one marker as the front, the other the rear
			if self.isFrontDominant(potential_markers[0], front_mask, rear_mask):
				front_marker = potential_markers[0]
				rear_marker = potential_markers[1]
			else
				front_marker = potential_markers[1]
				rear_marker = potential_markers[0]


			# Find the centerpoints
			[frontx,fronty] = self.find_center(front_marker)
			[backx,backy] = self.find_center(rear_marker)
			# Find the midpoint
			midx = (frontx+backx)/2
			midy = (fronty+backy)/2

			[vx,vy,vz] = self.camModel.projectPixelTo3dRay((midx,midy))
			marker_depth = self.robot_depth / vz

			# Position of the robot relative to ceiling kinect
			x = marker_depth * vx
			y = marker_depth * vy
			z = self.robot_depth

			# Orientation of robot
			yaw = math.atan2((fronty-backy), (frontx-backx))
			quaternion = tf.transformations.quaternion_from_euler(0.0,0.0,yaw, axes='sxyz')
			# Broadcast the frame to tf
			self.broadcaster.sendTransform((x,y,z), quaternion, rospy.Time.now(), self.robot_frame, self.cameraFrame)

			self.draw_orientation(image_cv2, (frontx, fronty), (rearx, reary))

		else:
			rospy.loginfo("No marker found")



		cv2.imshow('image', image_cv2)
		cv2.imshow('image_gray', image_gray)
		cv2.imshow('pattern', self.image_orig)
		cv2.imshow('image_corners', image_corners)
		cv2.imshow('thresh', thresh)
		cv2.waitKey(3)

		# Convert back to ROS Image msg
		image_cv = cv.fromarray(image_cv2)
		image_out = self.bridge.cv_to_imgmsg(image_cv, 'bgr8') 
		self.video.publish(image_out)

	def isFrontDominant(self, contour, front_mask, rear_mask):
		# Is the contour more red or more blue?
		x,y,w,h = cv2.boundingRect(contour)
		x1 = x - w/2
		y1 = y - h/2
		x2 = x + w/2
		y2 = y + h/2

		cropped_front = front_mask[y1:y2, x1:x2]
		cropped_rear = rear_mask[y1:y2, x1:x2]
		front_ratio = numpy.count_nonzero(cropped_front)/cropped_front.size
		rear_ratio = numpy.count_nonzero(cropped_rear)/cropped_rear.size

		if front_ratio >= rear_ratio:
			return True
		else
			return False

	def find_potential_markers(self, contours):
		# Get rid of small contours and order by match strength
		potential_markers = []
		for contour in contours:
			if cv2.contourArea(contour) > self.pattern_area:
				potential_markers.append(contour)
		sorted_markers = sorted(potential_markers, key=self.match_pattern)
		return sorted_markers

	def match_pattern(self, contour):
		# Match the contour against our pattern
		match = cv2.matchShapes(contour,self.pattern, 1, 0.0)
		return match

	def draw_orientation(self, image, front, back):
		# Draw a line across the whole image for debug
		slope = float(front[1] - back[1]) / float(front[0] - back[0])
		leftmosty = int((-front[0]*slope) + front[1])
		rightmosty = int(((image.shape[1]-front[0])*slope)+front[1])
		cv2.line(image, (image.shape[1]-1, rightmosty), (0, leftmosty), (0,255,0), 3)

	def find_center(self, contour):
		moments = cv2.moments(contour, True)
		x = int(moments['m10']/moments['m00'])
		y = int(moments['m01']/moments['m00'])
		return x,y
class FaceDetector():
    """ Measures face location, compiles important info. """
    def __init__(self, info_path, image_topic):
        # Create depth model for Asus Xtion camera
        bag = rosbag.Bag(info_path)
        infos = bag.read_messages(topics='/camera/rgb/camera_info')
        info = infos.next()[1]
        self.model = PinholeCameraModel()
        self.model.fromCameraInfo(info)

        # Ready the face detector
        self.classifier = cv2.CascadeClassifier(os.environ['ROS_ROOT'] + '/../OpenCV/haarcascades/haarcascade_frontalface_alt.xml')
        self.have_face = False

        # Subscribe to images
        self.have_image = False
        self.have_t_ref = False
        self.bridge = CvBridge()
        self.sub = rospy.Subscriber(image_topic, Image, self.handle_image)

    def handle_image(self, imgmsg):
        if not self.have_t_ref:
            self.t_ref = imgmsg.header.stamp.to_sec()
            self.t_old = 0.0
            self.have_t_ref = True
        t_new = imgmsg.header.stamp.to_sec() - self.t_ref
        dt = t_new - self.t_old
        self.t_old = t_new
            
        image = self.bridge.imgmsg_to_cv2(imgmsg)
        if not self.have_image:
            self.have_image = True
        success = self.detect_face(image, dt)

    def detect_face(self, image, dt):
        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
        faces = self.classifier.detectMultiScale(gray, 1.15, 1)

        if len(faces) > 0:
            (x,y,w,h) = faces[0]
            self.face = [x, y, w, h]
            self.face_size = w
            cv2.rectangle(image, (x,y), (x+w,y+h), (0,255,0), 1)  # plot detected face
            if not self.have_face:
                self.have_face = True
        else:
            if self.have_face:
                x,y,w,h = self.expand_filter(1, dt)
                cv2.rectangle(image, (x,y), (x+w,y+h), (255,0,255), 3)  # plot expanded filter area
                self.face = [x, y, w, h]

        cv2.imshow('Image with Face', image)
        cv2.waitKey(1)

    def expand_filter(self, ci, dt):
        """ Input: confidence interval N as in N*SD or N-sigma. """
        (x, y, w, h) = self.face  # break it down
        mean_face = 0.1875  # m
        mean_speed = 1.626  # m/s
        sd_speed = 0.201   # m/s
        meters_to_pixels = self.face_size / mean_face
        print meters_to_pixels
        mean_speed *= meters_to_pixels  # convert
        sd_speed *= meters_to_pixels  # convert
        expansion = (mean_speed + ci * sd_speed) * dt
        print mean_speed, sd_speed
        print dt, expansion
        print ''
        x -= expansion
        y -= expansion
        w += 2*expansion
        h += 2*expansion
        
        if x < 0: 
            x = 0
        if y < 0: 
            y = 0
        if x + w > 640: 
            w = 640 - x
        if y + h > 480:
            h = 480 - y

        face = [x,y,w,h]
        face = [int(el) for el in face]  # integer-ize
        return face