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 save_camera_data(bag, kitti_type, kitti, util, bridge, camera, camera_frame_id, topic_name, initial_time): print(f"Exporting camera {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([int(x) for x in util[f'S_rect_{camera_pad}']]) calib.distortion_model = 'plumb_bob' calib.k = util[f'K_{camera_pad}'] calib.r = util[f'R_rect_{camera_pad}'] calib.d = [float(x) for x in util[f'D_{camera_pad}']] calib.p = util[f'P_rect_{camera_pad}'] elif kitti_type.find("odom") != -1: camera_pad = '{0:01d}'.format(camera) image_path = os.path.join(kitti.sequence_path, f'image_{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[f'P{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 = Time(nanoseconds=int(float(datetime.strftime(dt, "%s.%f")) * 1e+9)).to_msg() topic_ext = "/image_raw" elif kitti_type.find("odom") != -1: image_message.header.stamp = Time(seconds=dt).to_msg() topic_ext = "/image_rect" calib.header.stamp = image_message.header.stamp camera_info_topic = create_topic(topic_name + '/camera_info', "sensor_msgs/msg/CameraInfo", "cdr") bag.create_topic(camera_info_topic) camera_topic = create_topic(topic_name + topic_ext, "sensor_msgs/msg/Image", "cdr") bag.create_topic(camera_topic) bag.write((topic_name + topic_ext, serialize_message(image_message), timestamp_to_nanoseconds_from_epoch(image_message.header.stamp))) bag.write((topic_name + '/camera_info', serialize_message(calib), timestamp_to_nanoseconds_from_epoch(calib.header.stamp)))
def msg_set(self): """prepares CameraInfo msg""" msg = CameraInfo() msg.header.frame_id = self.name msg.header.stamp = self.get_clock().now().to_msg() msg.height = self.height msg.width = self.width msg.distortion_model = self.dist_model msg.d = self.cam_info["distortion_coefficients"]["data"] msg.k = self.cam_info["camera_matrix"]["data"] return msg
def yaml_to_camera_info(self, yaml_file): with open(yaml_file, "r") as f: calib_data = yaml.load(f) 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 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: rclpy.logging._root_logger.log( "[" + cname + "] does not match name " + calib['camera_name'] + " in file " + filename, rclpy.logging.LoggingSeverity.WARN) #print("[" + 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.d = calib['distortion_coefficients']['data'] #ci.K = calib['camera_matrix']['data'] ci.k = calib['camera_matrix']['data'] #ci.R = calib['rectification_matrix']['data'] ci.r = calib['rectification_matrix']['data'] #ci.P = calib['projection_matrix']['data'] ci.p = calib['projection_matrix']['data'] except IOError: # OK if file did not exist pass return ci
def __init__(self, node, device_key, wb_device, params=None): super().__init__(node, device_key, wb_device, params) self._camera_info_publisher = None self._image_publisher = None # Create topics if not self._disable: self._image_publisher = self._node.create_publisher( Image, self._topic_name + '/image_raw', 1) self._camera_info_publisher = self._node.create_publisher( CameraInfo, self._topic_name + '/camera_info', QoSProfile( depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL, history=HistoryPolicy.KEEP_LAST, )) # CameraInfo data msg = CameraInfo() msg.header.stamp = Time( seconds=self._node.robot.getTime()).to_msg() msg.height = self._wb_device.getHeight() msg.width = self._wb_device.getWidth() msg.distortion_model = 'plumb_bob' msg.d = [0.0, 0.0, 0.0, 0.0, 0.0] msg.k = [ self._wb_device.getFocalLength(), 0.0, self._wb_device.getWidth() / 2, 0.0, self._wb_device.getFocalLength(), self._wb_device.getHeight() / 2, 0.0, 0.0, 1.0 ] msg.p = [ self._wb_device.getFocalLength(), 0.0, self._wb_device.getWidth() / 2, 0.0, 0.0, self._wb_device.getFocalLength(), self._wb_device.getHeight() / 2, 0.0, 0.0, 0.0, 1.0, 0.0 ] self._camera_info_publisher.publish(msg) # Load parameters camera_period_param = node.declare_parameter( wb_device.getName() + '_period', self._timestep) self._camera_period = camera_period_param.value
def load_camera_info(self, filename): import yaml try: f = open(filename, 'r') except IOError as err: print("Could not open file, IO Error({0})".format(err)) return calib_data = yaml.safe_load(f) cam_info = CameraInfo() # in ROS2 all the cameraInfo attributes are lower case 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 get_camera_info(self, time): """ From https://github.com/FurqanHabibi/cozmo_driver_ros2/blob/master/camera_info_manager.py :param time: :return: """ ci = CameraInfo() # fill in CameraInfo fields ci.width = self.calib["image_width"] ci.height = self.calib["image_height"] ci.distortion_model = self.calib["distortion_model"] ci.d = self.calib["distortion_coefficients"]["data"] ci.k = self.calib["camera_matrix"]["data"] ci.r = self.calib["rectification_matrix"]["data"] ci.p = self.calib["projection_matrix"]["data"] ci.header.stamp = time ci.header.frame_id = self.frame_id_ return ci
def construct_info(header: Header, info: SimCameraInfo, height: int, width: int) -> CameraInfo: msg = CameraInfo() Tx = 0.0 # Assumed for now since we are not using stereo hfov = np.deg2rad(info.fov) # https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/issues/86 f = width / (2 * np.tan(0.5 * hfov)) Fx = Fy = f cx = width / 2 cy = height / 2 K = np.array([ [Fx, 0.0, cx], [0.0, Fy, cy], [0.0, 0.0, 1 ] ]).flatten() R = np.array([ [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0] ]).flatten() P = np.array([ [Fx, 0.0, cx, Tx ], [0.0, Fy, cy, 0.0], [0.0, 0.0, 1.0, 0.0] ]).flatten() msg.header = header msg.height = height msg.width = width msg.k = K msg.r = R msg.p = P msg.binning_x = 0 msg.binning_y = 0 return msg