def test_monocular(self): ci = sensor_msgs.msg.CameraInfo() ci.width = 640 ci.height = 480 print ci cam = PinholeCameraModel() cam.fromCameraInfo(ci) #print cam.rectifyPoint((0, 0)) print cam.project3dToPixel((0,0,0))
class ConverterToUV(): def __init__(self, info_topic): self.camera_model = PinholeCameraModel() rospy.Subscriber(info_topic, CameraInfo, self.update_model) def update_model(self, info): self.camera_model.fromCameraInfo(info)
def handle_img_msg(self, img_msg): now = rospy.get_rostime() img = None bridge = cv_bridge.CvBridge() try: img = bridge.imgmsg_to_cv2(img_msg, 'bgr8') except cv_bridge.CvBridgeError as e: rospy.logerr( 'image message to cv conversion failed :' ) rospy.logerr( e ) print( e ) return self.frame_index = self.timestamp_map[img_msg.header.stamp.to_nsec()] f = self.frame_map[self.frame_index][0] obs_centroid = np.array(f.trans) + np.array(self.offset) R = tf.transformations.quaternion_matrix(self.rotation_offset) rotated_centroid = R.dot(list(obs_centroid)+[1]) obs_centroid = rotated_centroid[:3] #self.orient = list(f.rotq) self.marker.header.stamp = now self.marker.pose.position = Point(*list(obs_centroid)) self.marker.pose.orientation = Quaternion(*self.orient) self.obs_marker.pose.position = Point(*list(obs_centroid)) self.obs_marker.pose.orientation = Quaternion(*self.orient) self.add_bbox_lidar() md = self.metadata dims = np.array([md['l'], md['w'], md['h']]) outputName = '/image_bbox' if obs_centroid is None: rospy.loginfo("Couldn't find obstacle centroid") imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8')) return # print centroid info rospy.loginfo(str(obs_centroid)) # case when obstacle is not in camera frame if obs_centroid[0]<2.5 : self.imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8')) return # get bbox R = tf.transformations.quaternion_matrix(self.orient) corners = [0.5*np.array([i,j,k])*dims for i in [-1,1] for j in [-1,1] for k in [-1,1]] corners = [obs_centroid + R.dot(list(c)+[1])[:3] for c in corners] projected_pts = [] cameraModel = PinholeCameraModel() cam_info = load_cam_info(self.calib_file) cameraModel.fromCameraInfo(cam_info) projected_pts = [cameraModel.project3dToPixel(list(pt)+[1]) for pt in corners] projected_pts = np.array(projected_pts) center = np.mean(projected_pts, axis=0) out_img = drawBbox(img, projected_pts) self.imgOutput.publish(bridge.cv2_to_imgmsg(out_img, 'bgr8'))
def __init__(self): cv2.namedWindow('AR_TRACKER') self.bridge = cv_bridge.CvBridge() self.cam_info_sub = rospy.Subscriber('camera/rgb/camera_info', CameraInfo, self.info_cb) # self.pose_sub = rospy.Subscriber('pose', Pose, self.pose_cb) # self.img_sub = rospy.Subscriber('camera/rgb/image_raw/compressed', CompressedImage, self.img_cb, queue_size = 1, buff_size=2**24) self.img_sub = rospy.Subscriber('usb_cam/image_raw/compressed', CompressedImage, self.img_cb, queue_size=1, buff_size=2**24) self.ar_sub = rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_cb) self.pub = rospy.Publisher('ar_location', String, queue_size=5) # self.control_pub = rospy.Publisher('ar_control', Bool, queue_size=5) # self.stop_linear = False # self.straight_ahead = False # self.init = [False, False] # self.initial_theta = None # self.theta = None # self.pos = False self.count = 0 self.axis = np.float32([[0.1, 0, 0], [0, 0.1, 0], [0, 0, -0.1], [0, 0, 0]]).reshape(-1, 3) self.objp = np.zeros((6 * 8, 3), np.float32) self.objp[:, :2] = np.mgrid[0:8, 0:6].T.reshape(-1, 2) # self.criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) self.K = None self.D = None self.markers = [] self.cam_info = None self.pinhole = PinholeCameraModel()
def cb_camera_info(self, msg): # unsubscribe from camera_info self.loginfo( "Camera info message received. Unsubscribing from camera_info topic." ) # noinspection PyBroadException try: self.sub_camera_info.shutdown() except BaseException: pass # --- H, W = msg.height, msg.width # create new camera info self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(msg) # find optimal rectified pinhole camera with self.profiler("/cb/camera_info/get_optimal_new_camera_matrix"): rect_camera_K, _ = cv2.getOptimalNewCameraMatrix( self.camera_model.K, self.camera_model.D, (W, H), self.alpha.value) # create rectification map with self.profiler("/cb/camera_info/init_undistort_rectify_map"): self.mapx, self.mapy = cv2.initUndistortRectifyMap( self.camera_model.K, self.camera_model.D, None, rect_camera_K, (W, H), cv2.CV_32FC1) # pack rectified camera info into a CameraInfo message self.rect_camera_info = CameraInfo( width=W, height=H, K=rect_camera_K.flatten().tolist(), R=np.eye(3).flatten().tolist(), P=np.zeros((3, 4)).flatten().tolist(), )
def __init__(self): """ Constructor. """ # Publishing rate self.rate = rospy.Rate(10) # Set camera info boolean flag self.cameraInfoInitialised = False # PinholeCameraModel object self.pcm = PinholeCameraModel() # Tf listener self.tf_listener = tf.TransformListener() # Constant path self.path = str(Path(os.path.dirname(os.path.abspath(__file__))).parents[0]) # Poses, distance and marker publishers self.poses_pub = rospy.Publisher('poses', Poses, queue_size=5) self.distance_pub = rospy.Publisher('distances', Distances, queue_size=5) self.markers_pub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=5) # Camera info subscriber self.info_sub = rospy.Subscriber("/xtion/rgb/camera_info", CameraInfo, self.setCameraInfo)
def __init__(self): # defaults overwritten by param self.robot_name = "shamrock" self.rectified_input = False # read calibrations self.extrinsics_filename = (get_ros_package_path('duckietown') + "/config/baseline/calibration/camera_extrinsic/" + self.robot_name + ".yaml") if not os.path.isfile(extrinsics_filename): logger.warn("no robot specific extrinsic calibration, trying default") alternate_extrinsics_filename = (get_ros_package_path('duckietown') + "/config/baseline/calibration/camera_extrinsic/default.yaml") if not os.path.isfile(alternate_extrinsics_filename): logger.warn("can't find default either, something's wrong") else: self.H = self.load_homography(alternate_extrinsics_filename) else: self.H = self.load_homography(self.extrinsics_filename) self.H_inv = np.linalg.inv(self.H) intrinsics_filename = (get_ros_package_path('duckietown') + "/config/baseline/calibration/camera_intrinsics/" + self.robot_name + ".yaml") if not os.path.isfile(intrinsics_filename): logger.warn("no robot specific calibration, trying default") intrinsics_filename = (get_ros_package_path('duckietown') + "/config/baseline/calibration/camera_extrinsic/default.yaml") if not os.path.isfile(extrinsics_filename): logger.error("can't find default either, something's wrong") self.ci_ = self.load_camera_info(intrinsics_filename) self.pcm_ = PinholeCameraModel() self.pcm_.fromCameraInfo(self.ci)
def __init__(self): self.enabled = False # Maps ID to running class probabilities self.object_map = {} # Maps ID to mean volume, used to discriminate buoys / black totem self.volume_means = {} self.area_means = {} self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) self.get_params() self.last_panel_points_msg = None self.database_client = rospy.ServiceProxy('/database/requests', ObjectDBQuery) self.sub = Image_Subscriber(self.image_topic, self.img_cb) self.camera_info = self.sub.wait_for_camera_info() self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(self.camera_info) if self.debug: self.image_mux = ImageMux(size=(self.camera_info.height, self.camera_info.width), shape=(1, 2), labels=['Result', 'Mask']) self.debug_pub = Image_Publisher('~debug_image') self.last_objects = None self.last_update_time = rospy.Time.now() self.objects_sub = rospy.Subscriber('/pcodar/objects', PerceptionObjectArray, self.process_objects, queue_size=2) self.enabled_srv = rospy.Service('~set_enabled', SetBool, self.set_enable_srv) if self.is_training: self.enabled = True
def get_camera_model(bag, camera_name): camera_infos = [msg.message for msg in bag.read_messages(camera_name)] camera_info = camera_infos[0] from image_geometry import PinholeCameraModel camera_model = PinholeCameraModel() camera_model.fromCameraInfo(camera_info) return camera_model
def 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
def __init__(self): self.node_name = rospy.get_name() self.bridge = CvBridge() self.config = self.setupParam("~config", "baseline") self.cali_file_name = self.setupParam("~cali_file_name", "default") rospack = rospkg.RosPack() self.cali_file = rospack.get_path('duckietown') + \ "/config/" + self.config + \ "/vehicle_detection/vehicle_filter_node/" + \ self.cali_file_name + ".yaml" if not os.path.isfile(self.cali_file): rospy.logwarn("[%s] Can't find calibration file: %s.\n" % (self.node_name, self.cali_file)) self.loadConfig(self.cali_file) self.sub_corners = rospy.Subscriber("~corners", VehicleCorners, self.cbCorners, queue_size=1) self.pub_pose = rospy.Publisher("~pose", VehiclePose, queue_size=1) self.sub_info = rospy.Subscriber("~camera_info", CameraInfo, self.cbCameraInfo, queue_size=1) self.pcm = PinholeCameraModel() rospy.loginfo("[%s] Initialization completed" % (self.node_name)) self.lock = mutex()
def __init__(self, img_proc=None): super(LandmarkMethodROS, self).__init__(device_id_facedetection=rospy.get_param( "~device_id_facedetection", default="cuda:0")) self.subject_tracker = FaceEncodingTracker() if rospy.get_param( "~use_face_encoding_tracker", default=True) else SequentialTracker() self.bridge = CvBridge() self.__subject_bridge = SubjectListBridge() self.ros_tf_frame = rospy.get_param("~ros_tf_frame", "/kinect2_nonrotated_link") self.tf_broadcaster = TransformBroadcaster() self.tf_prefix = rospy.get_param("~tf_prefix", default="gaze") self.visualise_headpose = rospy.get_param("~visualise_headpose", default=True) self.pose_stabilizers = {} # Introduce scalar stabilizers for pose. try: if img_proc is None: tqdm.write("Wait for camera message") cam_info = rospy.wait_for_message("/camera_info", CameraInfo, timeout=None) self.img_proc = PinholeCameraModel() # noinspection PyTypeChecker self.img_proc.fromCameraInfo(cam_info) else: self.img_proc = img_proc if np.array_equal( self.img_proc.intrinsicMatrix().A, np.array([[0., 0., 0.], [0., 0., 0.], [0., 0., 0.]])): raise Exception( 'Camera matrix is zero-matrix. Did you calibrate' 'the camera and linked to the yaml file in the launch file?' ) tqdm.write("Camera message received") except rospy.ROSException: raise Exception("Could not get camera info") # multiple person images publication self.subject_pub = rospy.Publisher("/subjects/images", MSG_SubjectImagesList, queue_size=3) # multiple person faces publication for visualisation self.subject_faces_pub = rospy.Publisher("/subjects/faces", Image, queue_size=3) Server(ModelSizeConfig, self._dyn_reconfig_callback) # have the subscriber the last thing that's run to avoid conflicts self.color_sub = rospy.Subscriber("/image", Image, self.process_image, buff_size=2**24, queue_size=3)
def __init__(self,resize_width,resize_height): self.resize_width = resize_width self.resize_height = resize_height self.bridge = CvBridge() camera_info = rospy.wait_for_message('/usb_cam/camera_info', CameraInfo, timeout=5) # except(rospy.ROSException), e: # print ("Camera info topic not available") # print (e) # exit() waf = float(resize_width) / camera_info.width haf = float(resize_height) / camera_info.height camera_info.height = resize_height camera_info.width = resize_width # adjust the camera matrix K = camera_info.K camera_info.K = (K[0]*waf, 0., K[2]*waf, 0., K[4]*haf, K[5]*haf, 0., 0., 1.) # adjust the projection matrix P = camera_info.P camera_info.P = (P[0]*waf, 0., P[2]*waf, 0., 0., P[5]*haf, P[6]*haf, 0., 0., 0., 1., 0.) self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(camera_info) print (camera_info)
class Helpers: def __init__(self, camera_info): self.ci = camera_info self.pcm = PinholeCameraModel() self.pcm.fromCameraInfo(self.ci) self._rectify_inited = False self._distort_inited = False def process_image(self, cv_image_raw, interpolation=cv2.INTER_NEAREST): ''' Undistort an image. To be more precise, pass interpolation= cv2.INTER_CUBIC ''' if not self._rectify_inited: self._init_rectify_maps() cv_image_rectified = np.empty_like(cv_image_raw) res = cv2.remap(cv_image_raw, self.mapx, self.mapy, interpolation, cv_image_rectified) return res def _init_rectify_maps(self): W = self.pcm.width H = self.pcm.height mapx = np.ndarray(shape=(H, W, 1), dtype='float32') mapy = np.ndarray(shape=(H, W, 1), dtype='float32') mapx, mapy = cv2.initUndistortRectifyMap(self.pcm.K, self.pcm.D, self.pcm.R, self.pcm.P, (W, H), cv2.CV_32FC1, mapx, mapy) self.mapx = mapx self.mapy = mapy self._rectify_inited = True
def __init__(self, camera_name, base_frame, table_height): """ Initialize the instance :param camera_name: The camera name. One of (head_camera, right_hand_camera) :param base_frame: The frame for the robot base :param table_height: The table height with respect to base_frame """ self.camera_name = camera_name self.base_frame = base_frame self.table_height = table_height self.image_queue = Queue.Queue() self.pinhole_camera_model = PinholeCameraModel() self.tf_listener = tf.TransformListener() camera_info_topic = "/io/internal_camera/{}/camera_info".format( camera_name) camera_info = rospy.wait_for_message(camera_info_topic, CameraInfo) self.pinhole_camera_model.fromCameraInfo(camera_info) cameras = intera_interface.Cameras() cameras.set_callback(camera_name, self.__show_image_callback, rectify_image=True)
def __init__(self, cameraInfoFile): """ Overlay lidar points onto images by setting up the camera model, camera intrinsics, and lidar-camera extrinsics. =============== cameraInfoFile : an opened yaml file that stores camera intrinsics and other params used to initialize the cameraInfo which is used to help cameraModel reproject lidar points to image space. """ self.cameraParams = yaml.load(cameraInfoFile) self.cameraInfo = CameraInfo() self._fillCameraInfo() self.cameraModel = PinholeCameraModel() self.cameraModel.fromCameraInfo(self.cameraInfo) print('Distortion model:', self.cameraInfo.distortion_model) self.bridge = cv_bridge.CvBridge() # Get transformation/extrinsics between lidar (velodyne frame) and camera (world frame) self.tf = tf.TransformListener() # Get the topics' names to subscribe or publish self.inImageName = rospy.resolve_name('image') self.outImageName = rospy.resolve_name('lidar_image') self.lidarName = rospy.remap_name('lidar') # Create subscribers and publishers self.subImage = rospy.Subscriber(self.inImageName, Image, callback=self.imageCallback, queue_size=1) self.lidar = rospy.Subscriber(self.lidarName, PointCloud2, callback=self.lidarCallback, queue_size=1) self.pubImage = rospy.Publisher(self.outImageName, Image, queue_size=1) self.lidarPoints = None
def _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 __init__(self): # Pull constants from config file self.lower = rospy.get_param('~lower_color_threshold', [0, 0, 80]) self.upper = rospy.get_param( '~higher_color_threshold', [200, 200, 250]) self.min_contour_area = rospy.get_param('~min_contour_area', .001) self.max_contour_area = rospy.get_param('~max_contour_area', 400) self.min_trans = rospy.get_param('~min_trans', .05) self.max_velocity = rospy.get_param('~max_velocity', 1) self.timeout = rospy.Duration( rospy.get_param('~timeout_seconds'), 250000) self.min_observations = rospy.get_param('~min_observations', 8) self.camera = rospy.get_param('~camera_topic', '/camera/front/left/image_rect_color') # Instantiate remaining variables and objects self._observations = deque() self._pose_pairs = deque() self._times = deque() self.last_image_time = None self.last_image = None self.tf_listener = tf.TransformListener() self.status = '' self.est = None self.visual_id = 0 self.enabled = False self.bridge = CvBridge() # Image Subscriber and Camera Information self.image_sub = Image_Subscriber(self.camera, self.image_cb) self.camera_info = self.image_sub.wait_for_camera_info() ''' These variables store the camera information required to perform the transformations on the coordinates to move from the subs perspective to our global map perspective. This information is also necessary to perform the least squares intersection which will find the 3D coordinates of the torpedo board based on 2D observations from the Camera. More info on this can be found in sub8_vision_tools. ''' self.camera_info = self.image_sub.wait_for_camera_info() self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(self.camera_info) self.frame_id = self.camera_model.tfFrame() # Ros Services so mission can be toggled and info requested rospy.Service('~enable', SetBool, self.toggle_search) self.multi_obs = MultiObservation(self.camera_model) rospy.Service('~pose', VisionRequest, self.request_board3d) self.image_pub = Image_Publisher("torp_vision/debug") self.point_pub = rospy.Publisher( "torp_vision/points", Point, queue_size=1) self.mask_image_pub = rospy.Publisher( 'torp_vison/mask', Image, queue_size=1) # Debug self.debug = rospy.get_param('~debug', True)
def __init__(self): rospy.init_node('offboard_test', anonymous=True) self.loc = [] rospy.Subscriber('/mavros/local_position/pose', PoseStamped, callback=self.mocap_cb) rospy.Subscriber('/mavros/state', State, callback=self.state_cb) rospy.Subscriber('/dreams/state', String, callback=self.update_state_cb) rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes, callback=self.yolo) rospy.Subscriber('/darknet_ros/detection_image', Image, callback=self.detected_image) rospy.Subscriber('/camera/depth/image_raw', Image, callback=self.depth_image) #self.camera.fromCameraInfo(self.rgb_info()) # Zhiang: use this instead camera_info = rospy.wait_for_message( "/camera/depth/camera_info", CameraInfo) # Zhiang: make sure this is the correct camera #camera_info = rospy.wait_for_message("/camera/rgb/camera_info", CameraInfo) self.pinhole_camera_model = PinholeCameraModel() self.pinhole_camera_model.fromCameraInfo(camera_info) #self.listener = tf.TransformListener() # linked to tf_static self.planner()
def __init__(self): self.node_name = rospy.get_name() self.bridge = CvBridge() self.active = True self.currentImg = None self.publish_duration = rospy.Duration.from_sec(1.0 / 2.0) self.last_stamp = rospy.Time.now() #Subscribers and publishers self.imagesub = rospy.Subscriber("~image", CompressedImage, self.find_marker, buff_size=921600, queue_size=1) self.movepub = rospy.Publisher("~car_cmd", Twist2DStamped, queue_size=10) self.imagepub = rospy.Publisher("~image", Image, queue_size=1) self.infosub = rospy.Subscriber("~camera_info", CameraInfo, self.get_camera_info, queue_size=1) self.camerainfo = PinholeCameraModel() rospy.loginfo("[%s] Initialization completed" % (self.node_name))
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 CamPixelToPointServer: def __init__(self): self.camera_model = PinholeCameraModel() self.bridge = CvBridge() self.model_set = False self.tf_listener = TransformListener() def pixel_to_point(self, cam_pos, out_frame='map'): # type: (np.ndarray) -> PointStamped if not self.model_set: self.camera_model.fromCameraInfo( rospy.wait_for_message('/camera/rgb/camera_info', CameraInfo)) self.model_set = True x, y = int(cam_pos[0]), int(cam_pos[1]) d = self.read_depth_simple(x, y) pos = np.array(self.camera_model.projectPixelTo3dRay((x, y))) * d point = PointStamped() point.header.frame_id = self.camera_model.tfFrame() point.point.x, point.point.y, point.point.z = pos[0], pos[1], pos[2] point = convert_frame(self.tf_listener, point, out_frame) return point def read_depth_simple(self, x, y): # (int, int) -> float image = rospy.wait_for_message('/camera/depth_registered/image_raw', Image) image = self.bridge.imgmsg_to_cv2(image) return image[y, x]
def __init__(self): self.node_name = rospy.get_name() self.bridge = CvBridge() self.config = self.setupParam("~config", "baseline") self.cali_file_name = self.setupParam("~cali_file_name", "default") rospack = rospkg.RosPack() self.cali_file = "/code/catkin_ws/src/dt-core/packages/vehicle_detection/config/vehicle_filter_node/" + \ self.cali_file_name + ".yaml" if not os.path.isfile(self.cali_file): rospy.logwarn("[%s] Can't find calibration file: %s.\n" % (self.node_name, self.cali_file)) self.loadConfig(self.cali_file) self.sub_corners = rospy.Subscriber("~corners", VehicleCorners, self.processCorners, queue_size=1) self.pub_pose = rospy.Publisher("~pose", VehiclePose, queue_size=1) self.sub_info = rospy.Subscriber("~camera_info", CameraInfo, self.processCameraInfo, queue_size=1) self.pub_time_elapsed = rospy.Publisher("~pose_calculation_time", Float32, queue_size=1) self.pcm = PinholeCameraModel() rospy.loginfo("[%s] Initialization completed" % (self.node_name)) self.lock = mutex()
def __init__(self, camera_info, maxdist, transform=None): """ Parameters ---------- camera_info: sensor_msgs/CameraInfo Meta information of the camera. For more details check the `sensor_msgs/CameraInfo <http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html>`_ documentation. maxdist: float Maximum distance the FOV covers in the Z direction of the camera frame. transform: array_like Homogenous transformation of the camera reference frame. If `None` then the identity matrix is used. """ if transform is None: transform = np.eye(4) self.maxdist = maxdist self.transform = np.array(transform) self.cam_model = PinholeCameraModel() self.cam_model.fromCameraInfo(camera_info) # Compute FOV corners delta_x = self.cam_model.getDeltaX(self.cam_model.width/2, self.maxdist) delta_y = self.cam_model.getDeltaY(self.cam_model.height/2, self.maxdist) self.corners = np.zeros((5,3)) self.corners[0,:] = transform[:3,3] idx = 1 for k in itertools.product([-1,1],[-1,1]): point = np.array([0, 0, self.maxdist, 1]) point[:2] = np.array([delta_x, delta_y]) * np.array(k) self.corners[idx,:] = np.dot(transform, point)[:3] idx += 1
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
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) self.publish_tf = publish_tf
def __init__(self): self._imageInputName = rospy.resolve_name('image') print(self._imageInputName) self._velodyneOutputName = rospy.resolve_name('velodyne_rgb_points') print(self._velodyneOutputName) self._cameraName = rospy.resolve_name('camera') print(self._cameraName) self._velodyneName = rospy.resolve_name('velodyne') print(self._velodyneName) self._imageInput = rospy.Subscriber(self._imageInputName, Image, callback=self.imageCallback, queue_size=1) self._camera = rospy.Subscriber(self._cameraName, CameraInfo, callback=self.cameraCallback, queue_size=1) self._velodyne = rospy.Subscriber(self._velodyneName, PointCloud2, callback=self.velodyneCallback, queue_size=1) self._velodyneOutput = rospy.Publisher(self._velodyneOutputName, PointCloud2, queue_size=1) self._bridge = cv_bridge.CvBridge() self._cameraModel = PinholeCameraModel() self._tf = tf.TransformListener()
def __init__(self, camera_info, homography, debug=False): self.pcm = PinholeCameraModel() self.pcm.fromCameraInfo(camera_info) self.H = homography # maps points on ground plane to image plane self.debug = debug self.mapx, self.mapy = self._init_rectification()
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')
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
def _cinfo_cb(self, msg): # create mapx and mapy H, W = msg.height, msg.width # create new camera info self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(msg) # find optimal rectified pinhole camera with self.profiler('/cb/camera_info/get_optimal_new_camera_matrix'): rect_K, _ = cv2.getOptimalNewCameraMatrix( self.camera_model.K, self.camera_model.D, (W, H), self.rectify_alpha ) # store new camera parameters self._camera_parameters = (rect_K[0, 0], rect_K[1, 1], rect_K[0, 2], rect_K[1, 2]) # create rectification map with self.profiler('/cb/camera_info/init_undistort_rectify_map'): self._mapx, self._mapy = cv2.initUndistortRectifyMap( self.camera_model.K, self.camera_model.D, None, rect_K, (W, H), cv2.CV_32FC1 ) # once we got the camera info, we can stop the subscriber self.loginfo('Camera info message received. Unsubscribing from camera_info topic.') # noinspection PyBroadException try: self._cinfo_sub.shutdown() except BaseException: pass
def __init__(self): self.node_name = rospy.get_name() self.bridge = CvBridge() self.distance_between_centers = self.setupParam( 'distance_between_centers', 0.0125) self.max_reproj_pixelerror_pose_estimation = self.setupParam( 'max_reproj_pixelerror_pose_estimation', 5) self.sub_corners = rospy.Subscriber("~corners", VehicleCorners, self.processCorners, queue_size=1) self.pub_pose = rospy.Publisher("~pose", VehiclePose, queue_size=1) self.sub_info = rospy.Subscriber("~camera_info", CameraInfo, self.processCameraInfo, queue_size=1) self.pub_time_elapsed = rospy.Publisher("~pose_calculation_time", Float32, queue_size=1) self.pcm = PinholeCameraModel() rospy.loginfo("[%s] Initialization completed" % (self.node_name)) self.lock = mutex()
def __init__(self): """ Initialize Color Tracking ROS interface. """ rospy.init_node('color_tracker') self.tracker = ColorTracker() self.cv_image = self.processed_image = None # the latest image from the camera self.boxes = [] self.bridge = CvBridge() # used to convert ROS messages to OpenCV self.cameraModel = PinholeCameraModel() self.tf = TransformListener(cache_time=rospy.Duration.from_sec(20)) # parameters ... self._gui = bool(rospy.get_param('~gui', default=False)) # set to _gui:=true for debug display self._rate = float(rospy.get_param('~rate', default=50.0)) # processing rate self._par = float(rospy.get_param('~plate_area', default=0.0338709)) # publishers ... self.debug_pub = rospy.Publisher("tracker/debug", PoseWithCovarianceStamped, queue_size=10) self.roomba_pub = rospy.Publisher("visible_roombas", RoombaList, queue_size = 10) rospy.loginfo("Initializing Color Tracker") if self._gui: cv2.namedWindow('preview_window') cv2.namedWindow('binary') # start listening ... rospy.Subscriber("/ardrone/bottom/image_raw", Image, self.process_image) rospy.Subscriber("/ardrone/bottom/camera_info", CameraInfo, self.on_camera_info)
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
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
class VisionNode(object): ''' Base class to be used unify the interfacing for MIL's computer vision scripts. Handles the bootstrap of image subscription, enable/disable, etc. Provides a callback for new images which is expected to return ''' __metaclass__ = abc.ABCMeta def __init__(self): self._objects_pub = rospy.Publisher("~identified_objects", ObjectsInImage, queue_size=3) self._camera_info = None self.camera_model = None self._enabled = False self._image_sub = Image_Subscriber("image", callback=self._img_cb) if rospy.get_param("~autostart", default=False): self._enable() else: self._disable() self._enable_srv = rospy.Service("~enable", SetBool, self._enable_cb) def _enable_cb(self, req): if req.data and not self._enabled: self._enable() elif not req.data and self._enabled: self._disable() return {'success': True} def _enable(self): if self._camera_info is None: self._camera_info = self._image_sub.wait_for_camera_info() self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(self._camera_info) self._enabled = True rospy.loginfo("Enabled.") def _disable(self): self._enabled = False rospy.loginfo("Disabled.") def _img_cb(self, img): if not self._enabled: return msg = ObjectsInImage() msg.header = self._image_sub.last_image_header msg.objects = self.find_objects(img) if not isinstance(msg.objects, list) or (len(msg.objects) and not isinstance(msg.objects[0], ObjectInImage)): rospy.logwarn("find_objects did not return a list of mil_msgs/ObjectInImage message. Ignoring.") self._objects_pub.publish(msg) @abc.abstractmethod def find_objects(self, img): pass
def handle_img_msg(self, img_msg): img = None bridge = cv_bridge.CvBridge() try: img = bridge.imgmsg_to_cv2(img_msg, 'bgr8') except cv_bridge.CvBridgeError as e: rospy.logerr( 'image message to cv conversion failed :' ) rospy.logerr( e ) print( e ) return #tx, ty, tz, yaw, pitch, roll = [0.00749025, -0.40459941, -0.51372948, # -1.66780896, -1.59875352, -3.05415572] #translation = [tx, ty, tz, 1] #rotationMatrix = tf.transformations.euler_matrix(roll, pitch, yaw) #rotationMatrix[:, 3] = translation md = self.metadata dims = np.array([md['l'], md['w'], md['h']]) outputName = '/image_bbox' imgOutput = rospy.Publisher(outputName, Image, queue_size=1) obs_centroid = self.obs_centroid if self.obs_centroid is None: rospy.loginfo("Couldn't find obstacle centroid") imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8')) return # print centroid info rospy.loginfo(str(obs_centroid)) # case when obstacle is not in camera frame if obs_centroid[0]<2.5 : imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8')) return # get bbox R = tf.transformations.quaternion_matrix(self.orient) corners = [0.5*np.array([i,j,k])*dims for i in [-1,1] for j in [-1,1] for k in [-1,1]] corners = [obs_centroid + R.dot(list(c)+[1])[:3] for c in corners] projected_pts = [] cameraModel = PinholeCameraModel() cam_info = load_cam_info(self.calib_file) cameraModel.fromCameraInfo(cam_info) projected_pts = [cameraModel.project3dToPixel(list(pt)+[1]) for pt in corners] #for pt in corners: # rotated_pt = rotationMatrix.dot(list(pt)+[1]) # projected_pts.append(cameraModel.project3dToPixel(rotated_pt)) projected_pts = np.array(projected_pts) center = np.mean(projected_pts, axis=0) out_img = drawBbox(img, projected_pts) imgOutput.publish(bridge.cv2_to_imgmsg(out_img, 'bgr8'))
class RayTransformer(): def __init__(self, topic): self.model = PinholeCameraModel() rospy.Subscriber(topic, CameraInfo, self.callback_info) def callback_info(self, info): self.model.fromCameraInfo(info) def projectPointCloudToPixels(self, cloud): pixel_cloud = [] for point in cloud.points: pixel = self.model.project3dToPixel((point.x, point.y, point.z)) pixel_cloud.append(pixel) return pixel_cloud
class PointFromPixel(): """ Given a pixel location, find its 3D location in the world """ def __init__(self, topic_camera_info, topic_depth): self.need_camera_info = True self.need_depth_image = True self.model = PinholeCameraModel() rospy.Subscriber(topic_camera_info, CameraInfo, self.callback_camera_info) rospy.Subscriber(topic_depth, Image, self.callback_depth_image) def callback_camera_info(self, info): """ Define Pinhole Camera Model parameters using camera info msg """ if self.need_camera_info: rospy.loginfo('Got camera info!') self.model.fromCameraInfo(info) # define model params self.frame = info.header.frame_id self.need_camera_info = False def callback_depth_image(self, depth_image): """ Get depth at chosen pixel using depth image """ if self.need_depth_image: rospy.loginfo('Got depth image!') self.depth = img.fromstring("F", (depth_image.width, depth_image.height), depth_image.data) self.need_depth_image = False def calculate_3d_point(self, pixel): """ Project ray through chosen pixel, then use pixel depth to get 3d point """ lookup = self.depth.load() depth = lookup[pixel[0], pixel[1]] # lookup pixel in depth image ray = self.model.projectPixelTo3dRay(tuple(pixel)) # get 3d ray of unit length through desired pixel ray_z = [el / ray[2] for el in ray] # normalize the ray so its Z-component equals 1.0 pt = [el * depth for el in ray_z] # multiply the ray by the depth; its Z-component should now equal the depth value point = PointStamped() point.header.frame_id = self.frame point.point.x = pt[0] point.point.y = pt[1] point.point.z = pt[2] return point def ray_plane_intersection(self, pixel, plane): """ Given plane parameters [a, b, c, d] as in ax+by+cz+d=0, finds intersection of 3D ray with the plane. """ ray = self.model.projectPixelTo3dRay(tuple(pixel)) # get 3d ray of unit length through desired pixel scale = -plane[3] / (plane[0]*ray[0] + plane[1]*ray[1] + plane[2]*ray[2]) point = PointStamped() point.header.frame_id = self.frame point.point.x = ray[0] * scale point.point.y = ray[1] * scale point.point.z = ray[2] * scale return point
def __init__(self): # Get information for this particular camera camera_info = get_single('head_camera/depth_registered/camera_info', CameraInfo) print camera_info # Set information for the camera self.cam_model = PinholeCameraModel() self.cam_model.fromCameraInfo(camera_info) # Subscribe to the camera's image topic and depth image topic self.rgb_image_sub = rospy.Subscriber("head_camera/rgb/image_raw", Image, self.on_rgb) self.depth_image_sub = rospy.Subscriber("head_camera/depth_registered/image_raw", Image, self.on_depth) # Publish where the button is in the base link frame self.point_pub = rospy.Publisher('button_point', PointStamped, queue_size=10) # Set up connection to CvBridge self.bridge = CvBridge() # Open up viewing windows cv2.namedWindow('depth') cv2.namedWindow('rgb') # Set up the class variables self.rgb_image = None self.rgb_image_time = None self.depth_image = None self.center = None self.return_point = PointStamped() self.tf_listener = TransformListener()
def __init__(self): self.image_topic = "wall_camera/image_raw" self.image_info_topic = "wall_camera/camera_info" self.point_topic = "/clicked_point" self.frame_id = "/map" self.occupancy self.pixel_x = 350 self.pixel_y = 215 # What we do during shutdown rospy.on_shutdown(self.cleanup) # Create the OpenCV display window for the RGB image self.cv_window_name = self.image_topic cv.NamedWindow(self.cv_window_name, cv.CV_WINDOW_NORMAL) cv.MoveWindow(self.cv_window_name, 25, 75) # Create the cv_bridge object self.bridge = CvBridge() self.image_sub = rospy.Subscriber(self.image_topic, Image, self.image_cb, queue_size = 1) self.image_info_sub = rospy.Subscriber(self.image_info_topic, CameraInfo, self.image_info_cb, queue_size = 1) self.point_sub = rospy.Subscriber(self.point_topic, PointStamped, self.point_cb, queue_size = 4) self.points = [] self.ready_for_image = False self.has_pic = False self.camera_info_msg = None self.tf_listener = tf.TransformListener() # self.display_picture() self.camera_model = PinholeCameraModel()
def __init__(self, stillness_radius, mode_topic, torso_topic, info_topic, image_topic): self.stillness_radius = stillness_radius # Subscribe to mode info and torso location self.need_mode = True rospy.Subscriber(mode_topic, Bool, self.mode_callback) self.need_torso = True rospy.Subscriber(torso_topic, PointStamped, self.torso_callback, queue_size=1) # Camera model stuff self.model = PinholeCameraModel() self.need_camera_info = True rospy.Subscriber(info_topic, CameraInfo, self.info_callback, queue_size=1) # Ready the face detector self.classifier = cv2.CascadeClassifier(os.environ['ROS_ROOT'] + '/../OpenCV/haarcascades/haarcascade_frontalface_alt.xml') # Wait for all those messages to begin arriving while self.need_mode or self.need_torso or self.need_camera_info: rospy.sleep(0.01) # Subscribe to images self.have_image = False self.bridge = CvBridge() rospy.Subscriber(image_topic, Image, self.handle_image, queue_size=1)
def __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
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 run(sub_singleton): global marker_found nh = sub_singleton._node_handle #next_pose = yield nh.get_service_client('/next_search_pose', SearchPose) cam = PinholeCameraModel() # Go to max height to see as much as we can. yield sub_singleton.move.depth(SEARCH_DEPTH).zero_roll_and_pitch().go() yield nh.sleep(1.0) # --------------------------------------- # MOST OF THIS IS FOR THE SEARCH PATTERN. # --------------------------------------- # This is just to get camera info resp = yield sub_singleton.channel_marker.get_2d() cam.fromCameraInfo(resp.camera_info) # intial_pose = sub_singleton.last_pose() # intial_height = yield sub_singleton.get_dvl_range() # radius = calculate_visual_radius(cam, intial_height) # s = SearchPoseRequest() # s.intial_position = (yield intial_pose).pose.pose # s.search_radius = radius # s.reset_search = True # yield next_pose(s) # s.reset_search = False marker_found = False goer = go_to_marker(nh, sub_singleton, cam) # while not marker_found: # # Move in search pattern until we find the marker # resp = yield next_pose(s) # print resp # if resp.area > .8: # continue # print pose_to_numpy(resp.target_pose)[0] # yield sub_singleton.move.set_position(pose_to_numpy(resp.target_pose)[0]).zero_roll_and_pitch().go() # print "Searcher arrived" yield goer
def run(sub_singleton): global marker_found print "Searching" nh = sub_singleton._node_handle next_pose = yield nh.get_service_client('/next_search_pose', SearchPose) cam = PinholeCameraModel() # Go to max height to see as much as we can. yield sub_singleton.move.depth(0.6).zero_roll_and_pitch().go() yield nh.sleep(1.0) # This is just to get camera info resp = yield sub_singleton.channel_marker.get_2d() cam.fromCameraInfo(resp.camera_info) intial_pose = sub_singleton.last_pose() intial_height = yield sub_singleton.get_dvl_range() raidus = calculate_visual_radius(cam, intial_height) s = SearchPoseRequest() s.intial_position = (yield intial_pose).pose.pose s.search_radius = raidus s.reset_search = True yield next_pose(s) s.reset_search = False marker_found = False goer = go_to_marker(nh, sub_singleton, cam) while not marker_found: # Move in search pattern until we find the marker resp = yield next_pose(s) print resp if resp.area > .8: continue print pose_to_numpy(resp.target_pose)[0] yield sub_singleton.move.set_position(pose_to_numpy(resp.target_pose)[0]).zero_roll_and_pitch().go() print "Searcher arrived" yield goer
def random_point_inside_fov(camera_info, maxdist, Tcamera=np.eye(4)): """ Generates a random XYZ point inside the camera field of view @type camera_info: sensor_msgs.CameraInfo @param camera_info: Message with the meta information for a camera @type maxdist: float @param maxdist: distance from the camera ref frame in the z direction @type Tcamera: np.array @param Tcamera: homogeneous transformation for the camera ref frame @rtype: array @return: The random XYZ point """ cam_model = PinholeCameraModel() cam_model.fromCameraInfo(camera_info) z = maxdist*np.random.random() delta_x = cam_model.getDeltaX(cam_model.width/2, z) delta_y = cam_model.getDeltaY(cam_model.height/2, z) point = np.array([0, 0, z, 1]) point[:2] = np.array([delta_x, delta_y]) * (2*np.random.random_sample(2) - 1.) return np.dot(Tcamera, point)[:3]
def get_caminfo(self): cam_info = None try: cam_info = rospy.wait_for_message('camera_info', CameraInfo, 1.0) except rospy.ROSException: rospy.logerr("Could not get camera_info") if cam_info is not None: self.model = PinholeCameraModel() self.model.fromCameraInfo(cam_info)
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 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 __init__(self): self.debug_gui = False self.enabled = False self.cam = None # Constants from launch config file self.debug_ros = rospy.get_param("~debug_ros", True) self.canny_low = rospy.get_param("~canny_low", 100) self.canny_ratio = rospy.get_param("~canny_ratio", 3.0) self.thresh_hue_high = rospy.get_param("~thresh_hue_high", 60) self.thresh_saturation_low = rospy.get_param("~thresh_satuation_low", 100) self.min_contour_area = rospy.get_param("~min_contour_area", 100) self.epsilon_range = rospy.get_param("~epsilon_range", (0.01, 0.1)) self.epsilon_step = rospy.get_param("~epsilon_step", 0.01) self.shape_match_thresh = rospy.get_param("~shape_match_thresh", 0.4) self.min_found_count = rospy.get_param("~min_found_count", 10) self.timeout_seconds = rospy.get_param("~timeout_seconds", 2.0) # Default to scale model of path marker. Please use set_geometry service # to set to correct model of object. length = rospy.get_param("~length", 1.2192) width = rospy.get_param("~width", 0.1524) self.rect_model = RectFinder(length, width) self.do_3D = rospy.get_param("~do_3D", True) camera = rospy.get_param("~image_topic", "/camera/down/left/image_rect_color") self.tf_listener = tf.TransformListener() # Create kalman filter to track 3d position and direction vector for marker in /map frame self.state_size = 5 # X, Y, Z, DY, DX self.filter = cv2.KalmanFilter(self.state_size, self.state_size) self.filter.transitionMatrix = 1. * np.eye(self.state_size, dtype=np.float32) self.filter.measurementMatrix = 1. * np.eye(self.state_size, dtype=np.float32) self.filter.processNoiseCov = 1e-5 * np.eye(self.state_size, dtype=np.float32) self.filter.measurementNoiseCov = 1e-4 * np.eye(self.state_size, dtype=np.float32) self.filter.errorCovPost = 1. * np.eye(self.state_size, dtype=np.float32) self.reset() self.service_set_geometry = rospy.Service('~set_geometry', SetGeometry, self._set_geometry_cb) if self.debug_ros: self.debug_pub = Image_Publisher("~debug_image") self.markerPub = rospy.Publisher('~marker', Marker, queue_size=10) self.service2D = rospy.Service('~2D', VisionRequest2D, self._vision_cb_2D) if self.do_3D: self.service3D = rospy.Service('~pose', VisionRequest, self._vision_cb_3D) self.toggle = rospy.Service('~enable', SetBool, self._enable_cb) self.image_sub = Image_Subscriber(camera, self._img_cb) self.camera_info = self.image_sub.wait_for_camera_info() assert self.camera_info is not None self.cam = PinholeCameraModel() self.cam.fromCameraInfo(self.camera_info)
def 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 __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)
def __init__(self): self.tf_listener = tf.TransformListener() self.enabled = False self.last_image = None self.last_image_time = None self.camera_model = None self.circle_finder = CircleFinder(1.0) # Model radius doesn't matter beacause it's not being used for 3D pose # Various constants for tuning, debugging. See buoy_finder.yaml for more info self.min_observations = rospy.get_param('~min_observations') self.debug_ros = rospy.get_param('~debug/ros', True) self.debug_cv = rospy.get_param('~debug/cv', False) self.min_contour_area = rospy.get_param('~min_contour_area') self.max_circle_error = rospy.get_param('~max_circle_error') self.max_velocity = rospy.get_param('~max_velocity') self.roi_y = rospy.get_param('~roi_y') self.roi_height = rospy.get_param('~roi_height') camera = rospy.get_param('~camera_topic', '/camera/front/right/image_rect_color') self.buoys = {} for color in ['red', 'yellow', 'green']: self.buoys[color] = Buoy(color, debug_cv=self.debug_cv) if self.debug_cv: cv2.waitKey(1) self.debug_images = {} # Keep latest odom message for sanity check self.last_odom = None self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_cb, queue_size=3) self.image_sub = Image_Subscriber(camera, self.image_cb) if self.debug_ros: self.rviz = rviz.RvizVisualizer(topic='~markers') self.mask_pub = Image_Publisher('~mask_image') rospy.Timer(rospy.Duration(1), self.print_status) self.camera_info = self.image_sub.wait_for_camera_info() self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(self.camera_info) self.frame_id = self.camera_model.tfFrame() self.multi_obs = MultiObservation(self.camera_model) rospy.Service('~enable', SetBool, self.toggle_search) rospy.Service('~2D', VisionRequest2D, self.request_buoy) rospy.Service('~pose', VisionRequest, self.request_buoy3d) rospy.loginfo("BUOY FINDER: initialized successfully")
def __init__(self, depthThreshold, image_topic): self.listener = tf.TransformListener() self.image_sub = rospy.Subscriber(image_topic, Image, self.image_callback) self.info_sub = rospy.Subscriber("/camera/rgb/camera_info", CameraInfo, self.camera_callback) self.depth_sub = rospy.Subscriber("/camera/depth/image_raw", Image, self.depth_callback) self.depth_camera_info = rospy.Subscriber("/camera/depth/camera_info", CameraInfo, self.depthcamera_callback) self.marker_pub = rospy.Publisher('pose_markers', PoseMarkers) self.camModel = PinholeCameraModel() #bridge to convert the image to cvMAT. self.bridge = CvBridge() #Do we want to try for occlusion? This isn't working properly so it's disabled via the script and not launch file for the time being. self.doOcclusion = False #the maximum distance between a point's depth in the camera and a marker's depth we wish to allow. self.threshold = depthThreshold
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 = []
def __init__(self): info_topic = camera_root + "/camera_info" image_topic = camera_root + "/image_raw" # Change this to rect on boat self.tf_listener = tf.TransformListener() # Map id => [{color, error}, ...] for determining when a color is good self.colored = {} db_request = rospy.ServiceProxy("/database/requests", ObjectDBQuery) self.make_request = lambda **kwargs: db_request(ObjectDBQueryRequest(**kwargs)) rospy.Service("/vision/buoy_colorizer", VisionRequest, self.got_request) self.image_history = ImageHistory(image_topic) # Wait for camera info, and exit if not found try: fprint("Waiting for camera info on: '{}'".format(info_topic)) camera_info_msg = rospy.wait_for_message(info_topic, CameraInfo, timeout=3) except rospy.exceptions.ROSException: fprint("Camera info not found! Terminating.", msg_color="red") rospy.signal_shutdown("Camera not found!") return fprint("Camera info found!", msg_color="green") self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(camera_info_msg) self.debug = DebugImage("/colorama/debug", prd=.5) # These are color mappings from Hue values [0, 360] self.color_map = {'red': np.radians(0), 'yellow': np.radians(60), 'green': np.radians(120), 'blue': np.radians(240)} # Some tunable parameters self.update_time = .5 # s self.saturation_reject = 50 self.value_reject = 50 self.hue_error_reject = .4 # rads # To decide when to accept color observations self.error_tolerance = .4 # rads self.spottings_req = 4 self.similarity_reject = .1 # If two colors are within this amount of error, reject rospy.Timer(ros_t(self.update_time), self.do_update)