def run(self): # left_cam_info = self.yaml_to_camera_info(self.left_yaml_file) # right_cam_info = self.yaml_to_camera_info(self.right_yaml_file) left_cam_info = CameraInfo() left_cam_info.width = 640 left_cam_info.height = 480 left_cam_info.K = [742.858255, 0.000000, 342.003337, 0.000000, 752.915532, 205.211518, 0.000000, 0.000000, 1.000000] left_cam_info.D = [0.098441, 0.046414, -0.039316, 0.011202, 0.000000] left_cam_info.R = [0.983638, 0.047847, -0.173686, -0.048987, 0.998797, -0.002280, 0.173368, 0.010751, 0.984798] left_cam_info.P = [905.027641, 0.000000, 500.770557, 0.000000, 0.000000, 905.027641, 180.388927, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] left_cam_info.distortion_model = 'plumb_bob' right_cam_info = CameraInfo() right_cam_info.width = 640 right_cam_info.height = 480 right_cam_info.K = [747.026840, 0.000000, 356.331849, 0.000000, 757.336986, 191.248883, 0.000000, 0.000000, 1.000000] right_cam_info.D = [0.127580, -0.050428, -0.035857, 0.018986, 0.000000] right_cam_info.R = [0.987138, 0.047749, -0.152571, -0.046746, 0.998855, 0.010153, 0.152881, -0.002890, 0.988240] right_cam_info.P = [905.027641, 0.000000, 500.770557, -42.839515, 0.000000, 905.027641, 180.388927, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] right_cam_info.distortion_model = 'plumb_bob' rate = rospy.Rate(30) while not rospy.is_shutdown(): ret,frame=self.cam.read() if not ret: print('[ERROR]: frame error') break expand_frame=cv2.resize(frame,None,fx=2,fy=1) left_image = expand_frame[0:480,0:640] right_image = expand_frame[0:480,640:1280] self.msg_header.frame_id = 'stereo_image' self.msg_header.stamp = rospy.Time.now() left_cam_info.header = self.msg_header right_cam_info.header = self.msg_header self.left_image_info_pub.publish(left_cam_info) self.right_image_info_pub.publish(right_cam_info) # self.pub_image(self.left_image_pub, left_image, self.msg_header ) # self.pub_image(self.right_image_pub, right_image, self.msg_header ) try: thread.start_new_thread( self.pub_image, (self.left_image_pub, left_image, self.msg_header, )) thread.start_new_thread( self.pub_image, (self.right_image_pub, right_image, self.msg_header, )) except: print("[ERROR]: unable to start thread") rate.sleep()
def __init__(self): rospy.init_node('image_converter', anonymous=True) self.filtered_face_locations = rospy.get_param('camera_topic') self.image_pub = rospy.Publisher(self.filtered_face_locations, Image) self.image_info = rospy.Publisher('camera_info', CameraInfo) self.bridge = CvBridge() cap = cv2.VideoCapture('/home/icog-labs/Videos/video.mp4') print cap.get(5) path = rospkg.RosPack().get_path("robots_config") path = path + "/robot/camera.yaml" stream = file(path, 'r') calib_data = yaml.load(stream) cam_info = CameraInfo() cam_info.width = calib_data['image_width'] cam_info.height = calib_data['image_height'] cam_info.K = calib_data['camera_matrix']['data'] cam_info.D = calib_data['distortion_coefficients']['data'] cam_info.R = calib_data['rectification_matrix']['data'] cam_info.P = calib_data['projection_matrix']['data'] cam_info.distortion_model = calib_data['distortion_model'] while (not rospy.is_shutdown()) and (cap.isOpened()): self.keystroke = cv2.waitKey(1000 / 30) ret, frame = cap.read() gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # cv2.imshow('frame', gray) self.image_pub.publish(self.bridge.cv2_to_imgmsg(gray, "mono8")) self.image_info.publish(cam_info) if cv2.waitKey(1) & 0xFF == ord('q'): break
def publishloopCallback(self, event): # fpv_cam的参数 msg = CameraInfo() msg.K = [277.191356, 0.0, 320.5, 0.0, 277.191356, 240.5, 0.0, 0.0, 1.0] msg.height = 480 msg.width = 640 self.gridPub_.publish(msg)
def publish( self ): # Get the image. image = self.__videoDeviceProxy.getImageRemote( self.__videoDeviceProxyName ); # Create Image message. ImageMessage = Image(); ImageMessage.header.stamp.secs = image[ 5 ]; ImageMessage.width = image[ 0 ]; ImageMessage.height = image[ 1 ]; ImageMessage.step = image[ 2 ] * image[ 0 ]; ImageMessage.is_bigendian = False; ImageMessage.encoding = 'bgr8'; ImageMessage.data = image[ 6 ]; self.__imagePublisher.publish( ImageMessage ); # Create CameraInfo message. # Data from the calibration phase is hard coded for now. CameraInfoMessage = CameraInfo(); CameraInfoMessage.header.stamp.secs = image[ 5 ]; CameraInfoMessage.width = image[ 0 ]; CameraInfoMessage.height = image[ 1 ]; CameraInfoMessage.D = [ -0.0769218451517258, 0.16183180613612602, 0.0011626049774280595, 0.0018733894100460534, 0.0 ]; CameraInfoMessage.K = [ 581.090096189648, 0.0, 341.0926325830606, 0.0, 583.0323248080421, 241.02441593704128, 0.0, 0.0, 1.0 ]; CameraInfoMessage.R = [ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 ]; CameraInfoMessage.P = [ 580.5918359588198, 0.0, 340.76398441334106, 0.0, 0.0, 582.4042541534321, 241.04182225157172, 0.0, 0.0, 0.0, 1.0, 0.0] ; CameraInfoMessage.distortion_model = 'plumb_bob'; #CameraInfoMessage.roi.x_offset = self.__ROIXOffset; #CameraInfoMessage.roi.y_offset = self.__ROIYOffset; #CameraInfoMessage.roi.width = self.__ROIWidth; #CameraInfoMessage.roi.height = self.__ROIHeight; #CameraInfoMessage.roi.do_rectify = self.__ROIDoRectify; self.__cameraInfoPublisher.publish( CameraInfoMessage );
def __init__(self): rospy.init_node('image_publish') name = sys.argv[1] image = cv2.imread(name) #cv2.imshow("im", image) #cv2.waitKey(5) hz = rospy.get_param("~rate", 1) frame_id = rospy.get_param("~frame_id", "map") use_image = rospy.get_param("~use_image", True) rate = rospy.Rate(hz) self.ci_in = None ci_sub = rospy.Subscriber('camera_info_in', CameraInfo, self.camera_info_callback, queue_size=1) if use_image: pub = rospy.Publisher('image', Image, queue_size=1) ci_pub = rospy.Publisher('camera_info', CameraInfo, queue_size=1) msg = Image() msg.header.stamp = rospy.Time.now() msg.header.frame_id = frame_id msg.encoding = 'bgr8' msg.height = image.shape[0] msg.width = image.shape[1] msg.step = image.shape[1] * 3 msg.data = image.tostring() if use_image: pub.publish(msg) ci = CameraInfo() ci.header = msg.header ci.height = msg.height ci.width = msg.width ci.distortion_model ="plumb_bob" # TODO(lucasw) need a way to set these values- have this node # subscribe to an input CameraInfo? ci.D = [0.0, 0.0, 0.0, 0, 0] ci.K = [500.0, 0.0, msg.width/2, 0.0, 500.0, msg.height/2, 0.0, 0.0, 1.0] ci.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] ci.P = [500.0, 0.0, msg.width/2, 0.0, 0.0, 500.0, msg.height/2, 0.0, 0.0, 0.0, 1.0, 0.0] # ci_pub.publish(ci) # TODO(lwalter) only run this is hz is positive, # otherwise wait for input trigger message to publish an image while not rospy.is_shutdown(): if self.ci_in is not None: ci = self.ci_in msg.header.stamp = rospy.Time.now() ci.header = msg.header if use_image: pub.publish(msg) ci_pub.publish(ci) if hz <= 0: rospy.sleep() else: rate.sleep()
def yaml_to_CameraInfo(calib_yaml): """ Parse a yaml file containing camera calibration data (as produced by rosrun camera_calibration cameracalibrator.py) into a sensor_msgs/CameraInfo msg. Parameters ---------- yaml_fname : str Path to yaml file containing camera calibration data Returns ------- camera_info_msg : sensor_msgs.msg.CameraInfo A sensor_msgs.msg.CameraInfo message containing the camera calibration data """ # Load data from file calib_data = yaml.load(calib_yaml) # Parse camera_info_msg = CameraInfo() camera_info_msg.width = calib_data["image_width"] camera_info_msg.height = calib_data["image_height"] camera_info_msg.K = calib_data["camera_matrix"]["data"] camera_info_msg.D = calib_data["distortion_coefficients"]["data"] camera_info_msg.R = calib_data["rectification_matrix"]["data"] camera_info_msg.P = calib_data["projection_matrix"]["data"] camera_info_msg.distortion_model = calib_data["distortion_model"] return camera_info_msg
def yaml_to_CameraInfo(yaml_fname): """ Parse a yaml file containing camera calibration data (as produced by rosrun camera_calibration cameracalibrator.py) into a sensor_msgs/CameraInfo msg. Parameters ---------- yaml_fname : str Path to yaml file containing camera calibration data Returns ------- camera_info_msg : sensor_msgs.msg.CameraInfo A sensor_msgs.msg.CameraInfo message containing the camera calibration data """ # Load data from file with open(yaml_fname, "r") as file_handle: calib_data = yaml.load(file_handle) # Parse camera_info_msg = CameraInfo() camera_info_msg.width = calib_data["image_width"] camera_info_msg.height = calib_data["image_height"] camera_info_msg.K = calib_data["camera_matrix"]["data"] camera_info_msg.D = calib_data["distortion_coefficients"]["data"] camera_info_msg.R = calib_data["rectification_matrix"]["data"] camera_info_msg.P = calib_data["projection_matrix"]["data"] camera_info_msg.distortion_model = calib_data["distortion_model"] return camera_info_msg
def save_camera_data(bag, kitti, camera, timestamps): print("Exporting camera {}".format(camera.nr)) camera_info = CameraInfo() camera_info.header.frame_id = rectified_camera_frame_id camera_info.K = list( getattr(kitti.calib, 'K_cam{}'.format(camera.nr)).flat) camera_info.P = list( getattr(kitti.calib, 'P_rect_{}0'.format(camera.nr)).flat) # We do not include the D and R parameters from the calibration data since the images are # already undistorted and rectified to the camera #0 frame. camera_info.R = list(np.eye(3).flat) cv_bridge = CvBridge() image_paths = getattr(kitti, 'cam{}_files'.format(camera.nr)) for timestamp, image_path in tqdm(list(zip(timestamps, image_paths))): cv_image = cv2.imread(image_path, cv2.IMREAD_UNCHANGED) camera_info.height, camera_info.width = cv_image.shape[:2] encoding = 'bgr8' if camera.is_rgb else 'mono8' image_message = cv_bridge.cv2_to_imgmsg(cv_image, encoding=encoding) image_message.header.frame_id = rectified_camera_frame_id t = to_rostime(timestamp) image_message.header.stamp = t camera_info.header.stamp = t # Follow the naming conventions from # http://docs.ros.org/melodic/api/sensor_msgs/html/msg/CameraInfo.html image_topic_ext = '/image_rect_color' if camera.is_rgb else '/image_rect' bag.write(camera.topic_id + image_topic_ext, image_message, t=t) bag.write(camera.topic_id + '/camera_info', camera_info, t=t)
def genRosCameraInfo(K, P, resolution): ''' 默认参数 header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: '' height: 0 width: 0 distortion_model: '' D: [] K: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] R: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] P: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] binning_x: 0 binning_y: 0 roi: x_offset: 0 y_offset: 0 height: 0 width: 0 do_rectify: False ''' caminfo = CameraInfo() caminfo.width = resolution[0] caminfo.height = resolution[1] caminfo.distortion_model = "plumb_bob" caminfo.header.frame_id = "rgbd_camera_link" # caminfo.header.stamp = time_now caminfo.D = [0.0, 0.0, 0.0, 0.0, 0.0] # 畸变系数,无初始值需要指定 caminfo.K = K caminfo.P = P return caminfo
def get_camera_info(pipeline, array): profile = pipeline.get_active_profile() if "color" in array: stream_profile = rs.video_stream_profile( profile.get_stream(rs.stream.color)) stream_intrinsics = stream_profile.get_intrinsics() elif "depth" in array: stream_profile = rs.video_stream_profile( profile.get_stream(rs.stream.depth)) stream_intrinsics = stream_profile.get_intrinsics() camera_info = CameraInfo() camera_info.width = stream_intrinsics.width camera_info.height = stream_intrinsics.height camera_info.distortion_model = 'plumb_bob' cx = stream_intrinsics.ppx cy = stream_intrinsics.ppy fx = stream_intrinsics.fx fy = stream_intrinsics.fy camera_info.K = [fx, 0, cx, 0, fy, cy, 0, 0, 1] camera_info.D = [0, 0, 0, 0, 0] camera_info.R = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0] camera_info.P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1.0, 0] return camera_info
def test_CameraFOV(self): msg = CameraInfo() msg.height = 1024 msg.width = 1280 msg.distortion_model = 'plumb_bob' msg.D = [ -0.5506515572367885, 0.16918149333674903, -0.0005494252446900035, -0.003574460971943457, 0.08824797068343779 ] msg.K = [ 1547.3611792786492, 0.0, 645.7946620597459, 0.0, 1546.5965535476455, 512.489834878375, 0.0, 0.0, 1.0 ] msg.R = [ 0.9976063902119301, 0.0008462845042432227, 0.06914314145929477, -0.0007608300534628908, 0.9999989139538485, -0.0012622316559149822, -0.06914413457374326, 0.0012066041858555048, 0.997605950644034 ] msg.P = [ 1445.628365834274, 0.0, 521.7993656184378, 0.0, 0.0, 1445.628365834274, 514.7422812912976, 0.0, 0.0, 0.0, 1.0, 0.0 ] camfov = ru.camera.CameraFOV(msg, maxdist=2.) corners = camfov.get_corners() vertices, faces = camfov.get_trimesh() self.assertTrue(camfov.contains(corners)) points_inside = [camfov.random_point_inside() for _ in range(100)] self.assertTrue(camfov.contains(points_inside))
def rosmsg(self): """:obj:`sensor_msgs.CamerInfo` : Returns ROS CamerInfo msg """ msg_header = Header() msg_header.frame_id = self._frame msg_roi = RegionOfInterest() msg_roi.x_offset = 0 msg_roi.y_offset = 0 msg_roi.height = 0 msg_roi.width = 0 msg_roi.do_rectify = 0 msg = CameraInfo() msg.header = msg_header msg.height = self._height msg.width = self._width msg.distortion_model = 'plumb_bob' msg.D = [0.0, 0.0, 0.0, 0.0, 0.0] msg.K = [ self._fx, 0.0, self._cx, 0.0, self._fy, self._cy, 0.0, 0.0, 1.0 ] msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] msg.P = [ self._fx, 0.0, self._cx, 0.0, 0.0, self._fx, self._cy, 0.0, 0.0, 0.0, 1.0, 0.0 ] msg.binning_x = 0 msg.binning_y = 0 msg.roi = msg_roi return msg
def publish_camera_intrinsics(self): mtx, dist = self.__calibration_object.get_camera_info() msg_camera_info = CameraInfo() msg_camera_info.K = list(mtx.flatten()) msg_camera_info.D = list(dist.flatten()) return self.__camera_intrinsics_publisher.publish(msg_camera_info)
def publish(self, event): if self.imgmsg is None: return now = rospy.Time.now() # setup ros message and publish with self.lock: self.imgmsg.header.stamp = now self.imgmsg.header.frame_id = self.frame_id self.pub.publish(self.imgmsg) if self.publish_info: info = CameraInfo() info.header.stamp = now info.header.frame_id = self.frame_id info.width = self.imgmsg.width info.height = self.imgmsg.height if self.fovx is not None and self.fovy is not None: fx = self.imgmsg.width / 2.0 / \ np.tan(np.deg2rad(self.fovx / 2.0)) fy = self.imgmsg.height / 2.0 / \ np.tan(np.deg2rad(self.fovy / 2.0)) cx = self.imgmsg.width / 2.0 cy = self.imgmsg.height / 2.0 info.K = np.array([fx, 0, cx, 0, fy, cy, 0, 0, 1.0]) info.P = np.array([fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1, 0]) info.R = [1, 0, 0, 0, 1, 0, 0, 0, 1] self.pub_info.publish(info)
def load_camera_info(self): '''Load camera intrinsics''' filename = (sys.path[0] + "/calibrations/camera_intrinsic/" + self.robot_name + ".yaml") if not os.path.isfile(filename): logger.warn( "no intrinsic calibration parameters for {}, trying default". format(self.robot_name)) filename = (sys.path[0] + "/calibrations/camera_intrinsic/default.yaml") if not os.path.isfile(filename): logger.error("can't find default either, something's wrong") calib_data = yaml_load_file(filename) # logger.info(yaml_dump(calib_data)) cam_info = CameraInfo() cam_info.width = calib_data['image_width'] cam_info.height = calib_data['image_height'] cam_info.K = np.array(calib_data['camera_matrix']['data']).reshape( (3, 3)) cam_info.D = np.array( calib_data['distortion_coefficients']['data']).reshape((1, 5)) cam_info.R = np.array( calib_data['rectification_matrix']['data']).reshape((3, 3)) cam_info.P = np.array(calib_data['projection_matrix']['data']).reshape( (3, 4)) cam_info.distortion_model = calib_data['distortion_model'] logger.info( "Loaded camera calibration parameters for {} from {}".format( self.robot_name, os.path.basename(filename))) return cam_info
def info_480p(): data = CameraInfo() data.height = 480 data.width = 640 data.distortion_model = "plumb_bob" data.D = [-0.297117, 0.070173, -0.000390, 0.005232, 0.000000] data.K = [ 342.399061, 0.000000, 284.222700, 0.000000, 343.988302, 227.555237, 0.000000, 0.000000, 1.000000 ] data.R = [ 1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000 ] data.P = [ 247.555710, 0.000000, 290.282918, 0.000000, 0.000000, 281.358612, 220.776009, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000 ] data.binning_x = 0 data.binning_y = 0 data.roi.x_offset = 0 data.roi.y_offset = 0 data.roi.height = 0 data.roi.width = 0 data.roi.do_rectify = False return data
def info_720p(): data = CameraInfo() data.height = 720 data.width = 1280 data.distortion_model = "plumb_bob" data.D = [-0.266169, 0.056566, 0.003569, 0.002922, 0.000000] data.K = [ 550.515474, 0.000000, 560.486530, 0.000000, 550.947091, 334.377088, 0.000000, 0.000000, 1.000000 ] data.R = [ 1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000 ] data.P = [ 395.931458, 0.000000, 570.158643, 0.000000, 0.000000, 474.049377, 329.846866, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000 ] data.binning_x = 0 data.binning_y = 0 data.roi.x_offset = 0 data.roi.y_offset = 0 data.roi.height = 0 data.roi.width = 0 data.roi.do_rectify = False return data
def get_camera_info(self, yaml_fname): """ Parse a yaml file containing camera calibration data Parameters ---------- yaml_fname : str Path to yaml file containing camera calibration data Returns ------- camera_info_msg : sensor_msgs.msg.CameraInfo A sensor_msgs.msg.CameraInfo message containing the camera calibration data """ # Load data from file with open(yaml_fname, "r") as file_handle: calib_data = yaml.load(file_handle) # Parse camera_info_msg = CameraInfo() camera_info_msg.width = calib_data["image_width"] camera_info_msg.height = calib_data["image_height"] camera_info_msg.K = calib_data["intrinsics"]["data"] camera_info_msg.D = calib_data["distortion_coefficients"]["data"] camera_info_msg.R = calib_data["rectification_matrix"]["data"] camera_info_msg.P = calib_data["projection_matrix"]["data"] camera_info_msg.distortion_model = calib_data["distortion_model"] camera_info_msg.header.frame_id = calib_data["frame_id"] return camera_info_msg
def send_test_messages(self, filename): self.msg_received = False # Publish the camera info TODO make this a field in the annotations file to dictate the source calibration file msg_info = CameraInfo() msg_info.height = 480 msg_info.width = 640 msg_info.distortion_model = "plumb_bob" msg_info.D = [-0.28048157543793056, 0.05674481026365553, -0.000988764087143394, -0.00026869128565781613, 0.0] msg_info.K = [315.128501, 0.0, 323.069638, 0.0, 320.096636, 218.012581, 0.0, 0.0, 1.0] msg_info.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] msg_info.P = [217.48876953125, 0.0, 321.3154072384932, 0.0, 0.0, 250.71084594726562, 202.30416165274983, 0.0, 0.0, 0.0, 1.0, 0.0] msg_info.roi.do_rectify = False # Publish the test image img = cv2.imread(filename) cvb = CvBridge() msg_raw = cvb.cv2_to_imgmsg(img, encoding="bgr8") self.pub_info.publish(msg_info) self.pub_raw.publish(msg_raw) # Wait for the message to be received timeout = rospy.Time.now() + rospy.Duration(10) # Wait at most 5 seconds for the node to reply while not self.msg_received and not rospy.is_shutdown() and rospy.Time.now() < timeout: rospy.sleep(0.1) self.assertLess(rospy.Time.now(), timeout, "Waiting for apriltag detection timed out.")
def __init__(self): rospy.init_node('image_converter', anonymous=True) self.filtered_face_locations = rospy.get_param('camera_topic') self.image_pub = rospy.Publisher(self.filtered_face_locations,Image) self.image_info = rospy.Publisher('camera_info',CameraInfo) self.bridge = CvBridge() cap = cv2.VideoCapture('/home/icog-labs/Videos/video.mp4') print cap.get(5) path = rospkg.RosPack().get_path("robots_config") path = path + "/robot/camera.yaml" stream = file(path, 'r') calib_data = yaml.load(stream) cam_info = CameraInfo() cam_info.width = calib_data['image_width'] cam_info.height = calib_data['image_height'] cam_info.K = calib_data['camera_matrix']['data'] cam_info.D = calib_data['distortion_coefficients']['data'] cam_info.R = calib_data['rectification_matrix']['data'] cam_info.P = calib_data['projection_matrix']['data'] cam_info.distortion_model = calib_data['distortion_model'] while (not rospy.is_shutdown()) and (cap.isOpened()): self.keystroke = cv2.waitKey(1000 / 30) ret, frame = cap.read() gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # cv2.imshow('frame', gray) self.image_pub.publish(self.bridge.cv2_to_imgmsg(gray, "mono8")) self.image_info.publish(cam_info) if cv2.waitKey(1) & 0xFF == ord('q'): break
def camera_info(self): msg_header = Header() msg_header.frame_id = "f450/robot_camera_down" msg_roi = RegionOfInterest() msg_roi.x_offset = 0 msg_roi.y_offset = 0 msg_roi.height = 0 msg_roi.width = 0 msg_roi.do_rectify = 0 msg = CameraInfo() msg.header = msg_header msg.height = 480 msg.width = 640 msg.distortion_model = 'plumb_bob' msg.D = [0.0, 0.0, 0.0, 0.0, 0.0] msg.K = [1.0, 0.0, 320.5, 0.0, 1.0, 240.5, 0.0, 0.0, 1.0] msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] msg.P = [ 1.0, 0.0, 320.5, -0.0, 0.0, 1.0, 240.5, 0.0, 0.0, 0.0, 1.0, 0.0 ] msg.binning_x = 0 msg.binning_y = 0 msg.roi = msg_roi return msg
def _build_camera_info(self): """ Private function to compute camera info camera info doesn't change over time """ camera_info = CameraInfo() # store info without header camera_info.header = self.get_msg_header() camera_info.width = int(self.carla_actor.attributes['image_size_x']) camera_info.height = int(self.carla_actor.attributes['image_size_y']) camera_info.distortion_model = 'plumb_bob' cx = camera_info.width / 2.0 cy = camera_info.height / 2.0 fx = camera_info.width / ( 2.0 * math.tan(float(self.carla_actor.attributes['fov']) * math.pi / 360.0)) fy = fx if ROS_VERSION == 1: camera_info.K = [fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0] camera_info.D = [0.0, 0.0, 0.0, 0.0, 0.0] camera_info.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] camera_info.P = [fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0] elif ROS_VERSION == 2: # pylint: disable=assigning-non-slot camera_info.k = [fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0] camera_info.d = [0.0, 0.0, 0.0, 0.0, 0.0] camera_info.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] camera_info.p = [fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0] self._camera_info = camera_info
def callback(data): print(rospy.get_name(), "I heard %s" % str(data.data)) img_raveled = data.data[0:-2] img_size = data.data[-2:].astype(int) img = np.float32(np.reshape(img_raveled, (img_size[0], img_size[1]))) #img = np.float32((np.reshape(data.data, (DEPTH_IMG_WIDTH, DEPTH_IMG_HEIGHT)))) h = std_msgs.msg.Header() h.stamp = rospy.Time.now() image_message = CvBridge().cv2_to_imgmsg(img, encoding="passthrough") image_message.header = h pub.publish(image_message) camera_info_msg = CameraInfo() camera_info_msg.header = h fx, fy = DEPTH_IMG_WIDTH / 2, DEPTH_IMG_HEIGHT / 2 cx, cy = DEPTH_IMG_WIDTH / 2, DEPTH_IMG_HEIGHT / 2 camera_info_msg.width = DEPTH_IMG_WIDTH camera_info_msg.height = DEPTH_IMG_HEIGHT camera_info_msg.distortion_model = "plumb_bob" camera_info_msg.K = np.float32([fx, 0, cx, 0, fy, cy, 0, 0, 1]) camera_info_msg.D = np.float32([0, 0, 0, 0, 0]) camera_info_msg.P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1, 0] camera_info_pub.publish(camera_info_msg)
def yaml_to_CameraInfo(): rospy.init_node("camera_info_publisher", anonymous=True) info_publisher = rospy.Publisher("camera_info", CameraInfo, queue_size=10) # yaml_publisher = rospy.Publisher("yaml_filename", String, queue_size=10) rate = rospy.Rate(10) # 10hz yaml_fname = rospy.get_param('~camera_yaml') # yaml_fname = rospy.get_param('camera_yaml') with open(yaml_fname, "r") as file_handle: calib_data = yaml.load(file_handle) # Parse camera_info_msg = CameraInfo() camera_info_msg.width = calib_data["image_width"] camera_info_msg.height = calib_data["image_height"] camera_info_msg.K = calib_data["camera_matrix"]["data"] camera_info_msg.D = calib_data["distortion_coefficients"]["data"] camera_info_msg.R = calib_data["rectification_matrix"]["data"] camera_info_msg.P = calib_data["projection_matrix"]["data"] camera_info_msg.distortion_model = calib_data["distortion_model"] while not rospy.is_shutdown(): # yaml_publisher.publish(yaml_fname) info_publisher.publish(camera_info_msg) rate.sleep()
def __init__(self): # initialize this code as a ROS node named stream_to_ROS_node: rospy.init_node("stream_to_ROS_node", anonymous=True) # create publisher to publish frames from the video stream: self.image_pub = rospy.Publisher("/balrog_camera/image_raw", Image, queue_size = 1) # create publisher to publish camera info (calibration params): self.camera_info_pub = rospy.Publisher("/balrog_camera/camera_info", CameraInfo, queue_size = 1) # initialize cv_bridge for conversion between openCV and ROS images: self.cv_bridge = CvBridge() # the most recently read camera frame: self.latest_frame = None # start a thread constantly reading frames from the RPI video stream: self.thread_video = Thread(target = self.video_thread) self.thread_video.start() # camera info and calibration parameters: camera_info_msg = CameraInfo() camera_info_msg.height = 360 camera_info_msg.width = 640 camera_info_msg.distortion_model = "plumb_bob" camera_info_msg.D = [1.6541397736403166e-01, -3.1762679367786140e-01, 0.0, 0.0, -1.4358526468495059e-01] # ([k1, k2, t1, t2, k3]) camera_info_msg.K = [4.8488441935486514e+02, 0, 320, 0, 4.8488441935486514e+02, 180, 0, 0, 1] # ([fx, 0, cx, 0, fy, cy, 0, 0, 1]) camera_info_msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] self.camera_info_msg = camera_info_msg
def publishing(pub_image, pub_camera, image, type_of_camera): if type_of_camera is 1: image.convert(carla.ColorConverter.Depth) elif type_of_camera is 2: image.convert(carla.ColorConverter.CityScapesPalette) array = np.frombuffer(image.raw_data, dtype=np.uint8) array = np.reshape(array, (image.height, image.width, 4)) array = array[:, :, :3] array = array[:, :, ::-1] img = Image() infomsg = CameraInfo() img.header.stamp = rospy.Time.now() img.header.frame_id = 'base' img.height = infomsg.height = image.height img.width = infomsg.width = image.width img.encoding = "rgb8" img.step = img.width * 3 * 1 st1 = array.tostring() img.data = st1 cx = infomsg.width / 2.0 cy = infomsg.height / 2.0 fx = fy = infomsg.width / (2.0 * math.tan(image.fov * math.pi / 360.0)) infomsg.K = [fx, 0, cx, 0, fy, cy, 0, 0, 1] infomsg.P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1, 0] infomsg.R = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0] infomsg.D = [0, 0, 0, 0, 0] infomsg.binning_x = 0 infomsg.binning_y = 0 infomsg.distortion_model = "plumb_bob" infomsg.header = img.header pub_image.publish(img) pub_camera.publish(infomsg)
def load_camera_info_3(robot): # Load camera information filename = (os.environ['DUCKIEFLEET_ROOT'] + "/calibrations/camera_intrinsic/" + robot + ".yaml") if not os.path.isfile(filename): dtu.logger.warn( "no intrinsic calibration parameters for {}, trying default". format(robot)) filename = (os.environ['DUCKIEFLEET_ROOT'] + "/calibrations/camera_intrinsic/default.yaml") if not os.path.isfile(filename): dtu.logger.error("can't find default either, something's wrong") calib_data = dtu.yaml_wrap.yaml_load_file(filename) cam_info = CameraInfo() cam_info.width = calib_data['image_width'] cam_info.height = calib_data['image_height'] cam_info.K = calib_data['camera_matrix']['data'] cam_info.D = calib_data['distortion_coefficients']['data'] cam_info.R = calib_data['rectification_matrix']['data'] cam_info.P = calib_data['projection_matrix']['data'] cam_info.distortion_model = calib_data['distortion_model'] dtu.logger.info( "Loaded camera calibration parameters for {} from {}".format( robot, os.path.basename(filename))) return cam_info
def make_camera_msg(cam, rgb_time_sec, rgb_time_nsec, test_image): camera_info_msg = CameraInfo() camera_info_msg.header.seq = test_image if rgb_time_nsec + 20000000 > 999999999: camera_info_msg.header.stamp.secs = rgb_time_sec + 1 camera_info_msg.header.stamp.nsecs = rgb_time_nsec + 20000000 - 100000000 else: camera_info_msg.header.stamp.secs = rgb_time_sec camera_info_msg.header.stamp.nsecs = rgb_time_nsec + 20000000 # camera_info_msg.header.stamp.secs = rgb_time_sec # camera_info_msg.header.stamp.nsecs = rgb_time_nsec + 20000000 camera_info_msg.header.frame_id = "/openni_rgb_optical_frame" camera_info_msg.distortion_model = "plumb_bob" width, height = cam[0], cam[1] fx, fy = cam[2], cam[3] cx, cy = cam[4], cam[5] camera_info_msg.width = width camera_info_msg.height = height camera_info_msg.K = [fx, 0, cx, 0, fy, cy, 0, 0, 1] camera_info_msg.D = [0, 0, 0, 0, 0] camera_info_msg.R = [1, 0, 0, 0, 1, 0, 0, 0, 1] camera_info_msg.P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1, 0] return camera_info_msg
def run(): rospy.init_node('publish_constant', anonymous=True) pub = rospy.Publisher(args.topic, CameraInfo, queue_size=10) rate = rospy.Rate(30) filename = args.camera_info with open(filename, 'r') as f: camera_data = yaml.load(f) camera_info = CameraInfo() camera_info.height = camera_data['height'] camera_info.width = camera_data['width'] camera_info.K = camera_data['K'] camera_info.D = camera_data['D'] camera_info.R = camera_data['R'] camera_info.P = camera_data['P'] camera_info.distortion_model = camera_data['distortion_model'] camera_info.header.frame_id = args.frame # Setting this to zero means 'take the newest camera pose if TF'. camera_info.header.stamp = rospy.Time(0) while not rospy.is_shutdown(): pub.publish(camera_info) rate.sleep()
def save_camera_data(bag, kitti_type, kitti, util, bridge, camera, camera_frame_id, topic, initial_time): print("Exporting camera {}".format(camera)) if kitti_type.find("raw") != -1: camera_pad = '{0:02d}'.format(camera) image_dir = os.path.join(kitti.data_path, 'image_{}'.format(camera_pad)) image_path = os.path.join(image_dir, 'data') image_filenames = sorted(os.listdir(image_path)) with open(os.path.join(image_dir, 'timestamps.txt')) as f: image_datetimes = map( lambda x: datetime.strptime(x[:-4], '%Y-%m-%d %H:%M:%S.%f'), f.readlines()) calib = CameraInfo() calib.header.frame_id = camera_frame_id calib.width, calib.height = tuple( util['S_rect_{}'.format(camera_pad)].tolist()) calib.distortion_model = 'plumb_bob' calib.K = util['K_{}'.format(camera_pad)] calib.R = util['R_rect_{}'.format(camera_pad)] calib.D = util['D_{}'.format(camera_pad)] calib.P = util['P_rect_{}'.format(camera_pad)] elif kitti_type.find("odom") != -1: camera_pad = '{0:01d}'.format(camera) image_path = os.path.join(kitti.sequence_path, 'image_{}'.format(camera_pad)) image_filenames = sorted(os.listdir(image_path)) image_datetimes = map(lambda x: initial_time + x.total_seconds(), kitti.timestamps) calib = CameraInfo() calib.header.frame_id = camera_frame_id calib.P = util['P{}'.format(camera_pad)] iterable = zip(image_datetimes, image_filenames) bar = progressbar.ProgressBar() for dt, filename in bar(iterable): image_filename = os.path.join(image_path, filename) cv_image = cv2.imread(image_filename) calib.height, calib.width = cv_image.shape[:2] if camera in (0, 1): cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) encoding = "mono8" if camera in (0, 1) else "bgr8" image_message = bridge.cv2_to_imgmsg(cv_image, encoding=encoding) image_message.header.frame_id = camera_frame_id if kitti_type.find("raw") != -1: image_message.header.stamp = rospy.Time.from_sec( float(datetime.strftime(dt, "%s.%f"))) topic_ext = "/image_raw" elif kitti_type.find("odom") != -1: image_message.header.stamp = rospy.Time.from_sec(dt) topic_ext = "/image_rect" calib.header.stamp = image_message.header.stamp bag.write(topic + topic_ext, image_message, t=image_message.header.stamp) bag.write(topic + '/camera_info', calib, t=calib.header.stamp)
def __callback_get_calibration_object(self, _): if not self.__calibration_object.is_set(): return GetCalibrationCamResponse(False, CameraInfo()) msg_camera_info = CameraInfo() mtx, dist = self.__calibration_object.get_camera_info() msg_camera_info.K = list(mtx.flatten()) msg_camera_info.D = list(dist.flatten()) return GetCalibrationCamResponse(True, msg_camera_info)
def param2Msg(K, RT, width, height): param = CameraInfo() param.height = height param.width = width param.K = K param.R = RT return param
def default(self, ci="unused"): if not self.component_instance.capturing: return # press [Space] key to enable capturing image_local = self.data["image"] image = Image() image.header = self.get_ros_header() image.header.frame_id += "/base_image" image.height = self.component_instance.image_height image.width = self.component_instance.image_width image.encoding = "rgba8" image.step = image.width * 4 # VideoTexture.ImageRender implements the buffer interface image.data = bytes(image_local) # fill this 3 parameters to get correcty image with stereo camera Tx = 0 Ty = 0 R = [1, 0, 0, 0, 1, 0, 0, 0, 1] intrinsic = self.data["intrinsic_matrix"] camera_info = CameraInfo() camera_info.header = image.header camera_info.height = image.height camera_info.width = image.width camera_info.distortion_model = "plumb_bob" camera_info.K = [ intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], intrinsic[2][0], intrinsic[2][1], intrinsic[2][2], ] camera_info.R = R camera_info.P = [ intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], Tx, intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], Ty, intrinsic[2][0], intrinsic[2][1], intrinsic[2][2], 0, ] self.publish(image) self.topic_camera_info.publish(camera_info)
def sim(): global t, pose, camInfoMsg global centerPub, targetVelPub, camVelPub, camInfoPub, br, tfl global switchTimer, imageTimer, estimatorOn rospy.init_node("sim") estimatorOn = True centerPub = rospy.Publisher("markerCenters",Center,queue_size=10) targetVelPub = rospy.Publisher("ugv0/body_vel",TwistStamped,queue_size=10) camVelPub = rospy.Publisher("image/body_vel",TwistStamped,queue_size=10) cameraName = rospy.get_param(rospy.get_name()+"/camera","camera") camInfoPub = rospy.Publisher(cameraName+"/camera_info",CameraInfo,queue_size=1) br = tf.TransformBroadcaster() tfl = tf.TransformListener() # Camera parameters camInfoMsg = CameraInfo() camInfoMsg.D = distCoeffs.tolist() camInfoMsg.K = camMat.reshape((-1)).tolist() # Wait for node to get cam info while (camVelPub.get_num_connections() == 0) and (not rospy.is_shutdown()): # publish camera parameters camInfoPub.publish(camInfoMsg) rospy.sleep(0.5) # Publishers rospy.Timer(rospy.Duration(1.0/velRate),velPubCB) imageTimer = rospy.Timer(rospy.Duration(1.0/frameRate),imagePubCB) rospy.Timer(rospy.Duration(0.5),camInfoPubCB) switchTimer = rospy.Timer(rospy.Duration(11.0),switchCB,oneshot=True) # Initial conditions startTime = rospy.get_time() camPos = np.array([0,-1,1.5]) camOrient = np.array([-1*np.sqrt(2)/2,0,0,np.sqrt(2)/2]) targetPos = np.array([0,1,0]) targetOrient = np.array([0,0,0,1]) pose = np.concatenate((camPos,camOrient,targetPos,targetOrient)) r = rospy.Rate(intRate) h = 1.0/intRate while not rospy.is_shutdown(): t = np.array(rospy.get_time() - startTime) poseDot = poseDyn(t,pose) pose = pose + poseDot*h # Send Transform camPos = pose[0:3] camOrient = pose[3:7] targetPos = pose[7:10] targetOrient = pose[10:] br.sendTransform(targetPos,targetOrient,rospy.Time.now(),"ugv0","world") br.sendTransform(camPos,camOrient,rospy.Time.now(),"image","world") r.sleep()
def fill_camera_info(calib): ci = CameraInfo() ci.width = int(calib['image_width']) ci.height = int(calib['image_height']) ci.distortion_model = calib['distortion_model'] ci.D = calib['distortion_coefficients']['data'] ci.K = calib['camera_matrix']['data'] ci.R = calib['rectification_matrix']['data'] ci.P = calib['projection_matrix']['data'] return ci
def publish(self, width, height, intrinsic_matrix): cam_info = CameraInfo() cam_info.height = height cam_info.width = width K = list() for i in range(3): for j in range(3): K.append(intrinsic_matrix[i][j]) cam_info.K = K self.pub.publish(cam_info)
def callback(msg): info = CameraInfo() info.header = msg.header info.height = data['image_height'] info.width = data['image_width'] info.distortion_model = data['distortion_model'] info.D = data['distortion_coefficients']['data'] info.K = data['camera_matrix']['data'] info.P = data['projection_matrix']['data'] info.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] info_pub.publish(info)
def talker(): pub = rospy.Publisher('camera_info', CameraInfo, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): data = CameraInfo() #[[ 820.29938083 0. 408.73210359] #[ 0. 827.01247867 237.04151422] #[ 0. 0. 1. ]] data.K = (820,0.,408.73210359,0.,827.01247867,827.01247867,0.,0.,1.) pub.publish(data) rate.sleep()
def createMsg(self): msg = CameraInfo() msg.height = 480 msg.width = 640 msg.distortion_model = "plumb_bob" msg.D = [0.08199114285264993, -0.04549390835713936, -0.00040960290587863145, 0.0009833748346549968, 0.0] msg.K = [723.5609128875188, 0.0, 315.9845354772031, 0.0, 732.2615109685506, 242.26660681756633,\ 0.0, 0.0, 1.0] msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] msg.P = [737.1736450195312, 0.0, 316.0324931112791, 0.0, 0.0, 746.1573486328125, 241.59042622688867,\ 0.0, 0.0, 0.0, 1.0, 0.0] self.msg = msg
def parse_yaml(self, filename): stream = file(filename, "r") calib_data = yaml.load(stream) cam_info = CameraInfo() cam_info.width = calib_data["image_width"] cam_info.height = calib_data["image_height"] cam_info.K = calib_data["camera_matrix"]["data"] cam_info.D = calib_data["distortion_coefficients"]["data"] cam_info.R = calib_data["rectification_matrix"]["data"] cam_info.P = calib_data["projection_matrix"]["data"] cam_info.distortion_model = calib_data["distortion_model"] return cam_info
def parse_yaml(filename): stream = file(filename, 'r') calib_data = yaml.load(stream) cam_info = CameraInfo() cam_info.width = calib_data['image_width'] cam_info.height = calib_data['image_height'] cam_info.K = calib_data['camera_matrix']['data'] cam_info.D = calib_data['distortion_coefficients']['data'] cam_info.R = calib_data['rectification_matrix']['data'] cam_info.P = calib_data['projection_matrix']['data'] cam_info.distortion_model = calib_data['distortion_model'] return cam_info
def load_cam_info(calib_file): cam_info = CameraInfo() with open(calib_file,'r') as cam_calib_file : cam_calib = yaml.load(cam_calib_file) cam_info.height = cam_calib['image_height'] cam_info.width = cam_calib['image_width'] cam_info.K = cam_calib['camera_matrix']['data'] cam_info.D = cam_calib['distortion_coefficients']['data'] cam_info.R = cam_calib['rectification_matrix']['data'] cam_info.P = cam_calib['projection_matrix']['data'] cam_info.distortion_model = cam_calib['distortion_model'] return cam_info
def yaml_to_CameraInfo(yaml_fname): with open(yaml_fname, "r") as file_handle: calib_data = yaml.load(file_handle) camera_info_msg = CameraInfo() camera_info_msg.width = calib_data["image_width"] camera_info_msg.height = calib_data["image_height"] camera_info_msg.K = calib_data["camera_matrix"]["data"] camera_info_msg.D = calib_data["distortion_coefficients"]["data"] camera_info_msg.R = calib_data["rectification_matrix"]["data"] camera_info_msg.P = calib_data["projection_matrix"]["data"] camera_info_msg.distortion_model = calib_data["distortion_model"] return camera_info_msg
def get_cam_info(self, cam_name): cam_name = "calibrations/" + cam_name + ".yaml" stream = file(os.path.join(os.path.dirname(__file__), cam_name), 'r') calib_data = yaml.load(stream) cam_info = CameraInfo() cam_info.width = calib_data['image_width'] cam_info.height = calib_data['image_height'] cam_info.K = calib_data['camera_matrix']['data'] cam_info.D = calib_data['distortion_coefficients']['data'] cam_info.R = calib_data['rectification_matrix']['data'] cam_info.P = calib_data['projection_matrix']['data'] cam_info.distortion_model = calib_data['distortion_model'] return cam_info
def toCameraInfo(self): msg = CameraInfo() (msg.width, msg.height) = self.size if self.D.size > 5: msg.distortion_model = "rational_polynomial" else: msg.distortion_model = "plumb_bob" msg.D = numpy.ravel(self.D).copy().tolist() msg.K = numpy.ravel(self.K).copy().tolist() msg.R = numpy.ravel(self.R).copy().tolist() msg.P = numpy.ravel(self.P).copy().tolist() return msg
def GetCameraInfo(width, height): cam_info = CameraInfo() cam_info.width = width cam_info.height = height cam_info.distortion_model = model #cam_info.D = [0.0]*5 #cam_info.K = [0.0]*9 #cam_info.R = [0.0]*9 #cam_info.P = [0.0]*12 cam_info.D = D cam_info.K = K cam_info.R = R cam_info.P = P cam_info.binning_x = 0 cam_info.binning_y = 0 return cam_info
def pickle_to_info(info_pickle): info = CameraInfo() info.header.stamp = rospy.Time() info.header.seq = info_pickle.header.seq info.header.frame_id = info_pickle.header.frame_id info.roi.x_offset = info_pickle.roi.x_offset info.roi.y_offset = info_pickle.roi.y_offset info.roi.height = info_pickle.roi.height info.roi.width = info_pickle.roi.width info.height = info_pickle.height info.width = info_pickle.width info.D = info_pickle.D info.K = info_pickle.K info.R = info_pickle.R info.P = info_pickle.P return info
def sim(): global t, pose, camInfoMsg global centerPub, velPub, camInfoPub, br, tfl rospy.init_node("sim") centerPub = rospy.Publisher("markerCenters",Center,queue_size=10) velPub = rospy.Publisher("image/local_vel",TwistStamped,queue_size=10) cameraName = rospy.get_param(rospy.get_name()+"/camera","camera") camInfoPub = rospy.Publisher(cameraName+"/camera_info",CameraInfo,queue_size=1) br = tf.TransformBroadcaster() tfl = tf.TransformListener() # Camera parameters camInfoMsg = CameraInfo() camInfoMsg.D = distCoeffs.tolist() camInfoMsg.K = camMat.reshape((-1)).tolist() # Wait for node to get cam info while (velPub.get_num_connections() == 0) and (not rospy.is_shutdown()): # publish camera parameters camInfoPub.publish(camInfoMsg) rospy.sleep(0.5) # Publishers rospy.Timer(rospy.Duration(1.0/velRate),velPubCB) rospy.Timer(rospy.Duration(1.0/frameRate),imagePubCB) rospy.Timer(rospy.Duration(0.5),camInfoPubCB) # Initial conditions startTime = rospy.get_time() camPos = np.array([0,-1,1.5]) camOrient = np.array([-1*np.sqrt(2)/2,0,0,np.sqrt(2)/2]) targetPos = np.array([-1.5,1.5,0]) targetOrient = np.array([0,0,0,1]) pose = np.concatenate((camPos,camOrient,targetPos,targetOrient)) r = rospy.Rate(intRate) h = 1.0/intRate while not rospy.is_shutdown(): t = np.array(rospy.get_time() - startTime) poseDot = poseDyn(t,pose) pose = pose + poseDot*h r.sleep()
def default(self, ci='unused'): if not self.component_instance.capturing: return # press [Space] key to enable capturing image_local = self.data['image'] image = Image() image.header = self.get_ros_header() image.height = self.component_instance.image_height image.width = self.component_instance.image_width image.encoding = self.encoding image.step = image.width * 4 # VideoTexture.ImageRender implements the buffer interface image.data = bytes(image_local) # fill this 3 parameters to get correcty image with stereo camera Tx = 0 Ty = 0 R = [1, 0, 0, 0, 1, 0, 0, 0, 1] intrinsic = self.data['intrinsic_matrix'] camera_info = CameraInfo() camera_info.header = image.header camera_info.height = image.height camera_info.width = image.width camera_info.distortion_model = 'plumb_bob' camera_info.D = [0] camera_info.K = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], intrinsic[2][0], intrinsic[2][1], intrinsic[2][2]] camera_info.R = R camera_info.P = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], Tx, intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], Ty, intrinsic[2][0], intrinsic[2][1], intrinsic[2][2], 0] if self.pub_tf: self.publish_with_robot_transform(image) else: self.publish(image) self.topic_camera_info.publish(camera_info)
def parse_yaml(filename): stream = file(filename, "r") calib_data = yaml.load(stream) cam_info = CameraInfo() cam_info.width = calib_data["image_width"] cam_info.height = calib_data["image_height"] cam_info.K = calib_data["camera_matrix"]["data"] cam_info.D = calib_data["distortion_coefficients"]["data"] cam_info.R = calib_data["rectification_matrix"]["data"] cam_info.P = calib_data["projection_matrix"]["data"] cam_info.distortion_model = calib_data["distortion_model"] cam_info.binning_x = calib_data["binning_x"] cam_info.binning_y = calib_data["binning_y"] cam_info.roi.x_offset = calib_data["roi"]["x_offset"] cam_info.roi.y_offset = calib_data["roi"]["y_offset"] cam_info.roi.height = calib_data["roi"]["height"] cam_info.roi.width = calib_data["roi"]["width"] cam_info.roi.do_rectify = calib_data["roi"]["do_rectify"] return cam_info
def fetch_image(self, cam): cam.simulate() if not cam.pixels: return None, None cv_img = cv.CreateImageHeader((cam.width , cam.height), cv.IPL_DEPTH_8U, 3) cv.SetData(cv_img, cam.pixels, cam.width*3) cv.ConvertImage(cv_img, cv_img, cv.CV_CVTIMG_FLIP) im = self.bridge.cv_to_imgmsg(cv_img, "rgb8") caminfo = CameraInfo() caminfo.header = im.header caminfo.height = cam.height caminfo.width = cam.width caminfo.D = 5*[0.] caminfo.K = sum([list(r) for r in cam.K],[]) caminfo.P = sum([list(r) for r in cam.P],[]) caminfo.R = sum([list(r) for r in cam.R],[]) return im, caminfo
def parse_yaml(filename): stream = file(filename, 'r') calib_data = yaml.load(stream) cam_info = CameraInfo() cam_info.width = calib_data['image_width'] cam_info.height = calib_data['image_height'] cam_info.K = calib_data['camera_matrix']['data'] cam_info.D = calib_data['distortion_coefficients']['data'] cam_info.R = calib_data['rectification_matrix']['data'] cam_info.P = calib_data['projection_matrix']['data'] cam_info.distortion_model = calib_data['distortion_model'] cam_info.binning_x = calib_data['binning_x'] cam_info.binning_y = calib_data['binning_y'] cam_info.roi.x_offset = calib_data['roi']['x_offset'] cam_info.roi.y_offset = calib_data['roi']['y_offset'] cam_info.roi.height = calib_data['roi']['height'] cam_info.roi.width = calib_data['roi']['width'] cam_info.roi.do_rectify = calib_data['roi']['do_rectify'] return cam_info
def build_camera_info(self): """ computing camera info camera info doesn't change over time """ camera_info = CameraInfo() camera_info.header.frame_id = self.name camera_info.width = self.carla_object.ImageSizeX camera_info.height = self.carla_object.ImageSizeY camera_info.distortion_model = 'plumb_bob' cx = self.carla_object.ImageSizeX / 2.0 cy = self.carla_object.ImageSizeY / 2.0 fx = self.carla_object.ImageSizeX / ( 2.0 * math.tan(self.carla_object.FOV * math.pi / 360.0)) fy = fx camera_info.K = [fx, 0, cx, 0, fy, cy, 0, 0, 1] camera_info.D = [0, 0, 0, 0, 0] camera_info.R = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0] camera_info.P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1.0, 0] self._camera_info = camera_info
def test(): rospy.wait_for_service(service_name) # build theoretical calibration for Unibrain Fire-i camera in # 640x480 mode cinfo = CameraInfo() cinfo.width = 1360 cinfo.height = 1024 #cx = (cinfo.width - 1.0)/2.0 #cy = (cinfo.height - 1.0)/2.0 #fx = fy = 0.0043 # Unibrain Fire-i focal length #fx = 1046.16197 #fy = 1043.87094 #cx = 695.62128 #cy = 544.32183 #k1 = -0.23268 #k2 = 0.08580 #p1 = 0.00098 #p2 = -0.00022 #cinfo.K = [fx, 0., cx, 0., fy, cy, 0., 0., 1.] #cinfo.R = [1., 0., 0., 0., 1., 0., 0., 0., 1.] #cinfo.P = [fx, 0., cx, 0., 0., fy, cy, 0., 0., 0., 1., 0.] cinfo.K = [430.21554970319971, 0.0, 306.6913434743704, 0.0, 430.53169252696676, 227.22480030078816, 0.0, 0.0, 1.0] cinfo.D = [-0.33758562758914146, 0.11161239414304096, -0.00021819272592442094, -3.029195446330518e-05, 0] #cinfo.K = [4827.94, 0, 1223.5, 0, 4835.62, 1024.5, 0, 0, 1] #cinfo.D = [-0.41527, 0.31874, -0.00197, 0.00071, 0] #cinfo.R = [1, 0, 0, 0, 1, 0, 0, 0, 1] #cinfo.P = [4827.94, 0, 1223.5, 0, 0, 4835.62, 1024.5, 0, 0, 0, 1, 0] try: set_camera_info = rospy.ServiceProxy(service_name, SetCameraInfo) response = set_camera_info(cinfo) rospy.loginfo("set_camera_info: success=" + str(response.success) + ", status=" + str(response.status_message)) return response.success except rospy.ServiceException, e: print "Service call failed: %s"%e
def get_camera_info(hard_coded=True): if hard_coded: cx = 319.5 cy = 239.5 fx = 525.5 fy = 525.5 return (cx, cy, fx, fy) #if we are using a different camera, then #we can listen to the ros camera info topic for that device #and get our values here. else: import image_geometry from sensor_msgs.msg import CameraInfo cam_info = CameraInfo() cam_info.height = 480 cam_info.width = 640 cam_info.distortion_model = "plumb_bob" cam_info.D = [0.0, 0.0, 0.0, 0.0, 0.0] cam_info.K = [525.0, 0.0, 319.5, 0.0, 525.0, 239.5, 0.0, 0.0, 1.0] cam_info.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] cam_info.P = [525.0, 0.0, 319.5, 0.0, 0.0, 525.0, 239.5, 0.0, 0.0, 0.0, 1.0, 0.0] cam_info.binning_x = 0 cam_info.binning_y = 0 cam_info.roi.x_offset = 0 cam_info.roi.y_offset = 0 cam_info.roi.height = 0 cam_info.roi.width = 0 cam_info.roi.do_rectify = False camera_model = image_geometry.PinholeCameraModel() camera_model.fromCameraInfo(cam_info) return camera_model.cx(), camera_model.cy(), camera_model.fx(), camera_model.fy()
def test_with_known_image(self): filename = rospy.get_param("~filename") self.setup() # Setup the node # Publish the camera info msg_info = CameraInfo() msg_info.height = 480 msg_info.width = 640 msg_info.K = [331.026328, 0.0, 319.035097, 0.0, 335.330339, 216.450133, 0.0, 0.0, 1.0] self.pub_info.publish(msg_info) # Publish the test image img = cv2.imread(filename) cvb = CvBridge() msg_raw = cvb.cv2_to_imgmsg(img, encoding="bgr8") self.pub_raw.publish(msg_raw) # Wait for the message to be received timeout = rospy.Time.now() + rospy.Duration(5) # Wait at most 5 seconds for the node to reply while not self.msg_received and not rospy.is_shutdown() and rospy.Time.now() < timeout: rospy.sleep(0.1) self.assertLess(rospy.Time.now(), timeout, "Waiting for apriltag detection timed out.") # Check that an apriltag with id=108 was detected found = 0 for tag in self.msg_tags.detections: if tag.id == 108: found += 1 self.assertAlmostEqual(tag.pose.pose.position.x, 0.0, delta=0.1) # Allow 10 cm of error margin self.assertAlmostEqual(tag.pose.pose.position.y, 0.0, delta=0.1) # Allow 10 cm of error margin self.assertAlmostEqual(tag.pose.pose.position.z, 0.305, delta=0.1) # Allow 10 cm of error margin # Convert the quat to an angle about z rot = tag.pose.pose.orientation ang = tr.euler_from_quaternion((rot.x, rot.y, rot.z, rot.w))[2] # z axis angle only self.assertAlmostEqual(ang, 0, delta=15 * np.pi / 180) # Allow up to 15 degrees of error margin self.assertGreaterEqual(found, 1, "Expected apriltag with id=108 not found.")
def loadCalibrationFile(filename, cname): """ Load calibration data from a file. This function returns a `sensor_msgs/CameraInfo`_ message, based on the filename parameter. An empty or non-existent file is *not* considered an error; a null CameraInfo being provided in that case. :param filename: location of CameraInfo to read :param cname: Camera name. :returns: `sensor_msgs/CameraInfo`_ message containing calibration, if file readable; null calibration message otherwise. :raises: :exc:`IOError` if an existing calibration file is unreadable. """ ci = CameraInfo() try: f = open(filename) calib = yaml.load(f) if calib is not None: if calib['camera_name'] != cname: rospy.logwarn("[" + cname + "] does not match name " + calib['camera_name'] + " in file " + filename) # fill in CameraInfo fields ci.width = calib['image_width'] ci.height = calib['image_height'] ci.distortion_model = calib['distortion_model'] ci.D = calib['distortion_coefficients']['data'] ci.K = calib['camera_matrix']['data'] ci.R = calib['rectification_matrix']['data'] ci.P = calib['projection_matrix']['data'] except IOError: # OK if file did not exist pass return ci
def get_camera_info(self, camera_name, img_name='depth'): camera_info = CameraInfo() file_url = '' try : file_url = rospy.get_param(camera_name+'/driver/'+img_name+'_camera_info_url').replace('file://','') except Exception,e: print e if not os.path.exists(file_url): print 'ERROR: Could not read '+ camera_name+ ' '+img_name +'_camera_info' print ' Calibrate the sensor and try again !' exit(0) return print 'Loading camera '+img_name +'_camera_info for '+camera_name+' at:',file_url with open(file_url, 'r') as f: calib = yaml.safe_load(f.read()) camera_info.K = np.matrix(calib["camera_matrix"]["data"]) camera_info.D = np.array(calib["distortion_coefficients"]["data"]) camera_info.R = np.matrix(calib["rectification_matrix"]["data"]) camera_info.P = np.matrix(calib["projection_matrix"]["data"]) camera_info.height = calib["image_height"] camera_info.width = calib["image_width"] print camera_info return camera_info
def default(self, ci='unused'): """ Publish the data of the Camera as a ROS Image message. """ if not self.component_instance.capturing: return # press [Space] key to enable capturing image_local = self.data['image'] image = Image() image.header = self.get_ros_header() image.header.frame_id += '/base_image' image.height = self.component_instance.image_height image.width = self.component_instance.image_width image.encoding = 'rgba8' image.step = image.width * 4 # VideoTexture.ImageRender implements the buffer interface image.data = bytes(image_local) # sensor_msgs/CameraInfo [ http://ros.org/wiki/rviz/DisplayTypes/Camera ] # fill this 3 parameters to get correcty image with stereo camera Tx = 0 Ty = 0 R = [1, 0, 0, 0, 1, 0, 0, 0, 1] intrinsic = self.data['intrinsic_matrix'] camera_info = CameraInfo() camera_info.header = image.header camera_info.height = image.height camera_info.width = image.width camera_info.distortion_model = 'plumb_bob' camera_info.K = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], intrinsic[2][0], intrinsic[2][1], intrinsic[2][2]] camera_info.R = R camera_info.P = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], Tx, intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], Ty, intrinsic[2][0], intrinsic[2][1], intrinsic[2][2], 0] self.publish(image) self.topic_camera_info.publish(camera_info)