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 _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 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
