I have a multi camera setup and is attempting to get the extrinsic transformations for each camera. For this I used a class object in python to retrieve the static transform of each camera during initialization as follows:
import rospy
import tf2_ros
import numpy as np
class SynchronisedCamera():
def __init__(self, camera_param, reference_frame):
self.trans = None
self.rot = None
self.reference_frame = reference_frame
self.name = camera_param["name"]
self.tf_buffer = tf2_ros.Buffer()
self.tf2_listener = tf2_ros.TransformListener(self.tf_buffer)
try:
self.camera_frame = camera_param["camera_frame"]
except KeyError:
self.camera_frame = None
rospy.sleep(0.1)
self.get_transform_with_retry(camera_param)
def get_transform_with_retry(self, camera_param):
max_attempts = 3
for attempt in range(max_attempts):
self.tf_buffer.can_transform(self.camera_frame, self.reference_frame, rospy.Duration(2))
try:
# Intrinsic
camera_info = rospy.wait_for_message(camera_param["camera_info"], CameraInfo, timeout=10)
self.K = camera_info.K
if self.orientation % 2 != 0:
fu = camera_info.K[0]
fv = camera_info.K[4]
u0 = camera_info.K[2]
v0 = camera_info.K[5]
self.K = [fv, 0, v0, 0, fu, u0, 0, 0, 1.0]
# Intrinsic
self.intrinsic_matrix = np.asarray(self.K).reshape(3, 3)
# Extrinsic
# Look up the transform from 'source_frame' to 'target_frame'
transform = self.tf_buffer.lookup_transform(self.camera_frame, self.reference_frame, rospy.Time(0),
rospy.Duration(1))
trans = [transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z]
rot = [transform.transform.rotation.x, transform.transform.rotation.y, transform.transform.rotation.z,
transform.transform.rotation.w]
self.extrinsic_matrix = self.fromTranslationRotation(trans, rot)
except rospy.ROSException as e:
rospy.logwarn(e)
def translation_matrix(self, direction):
M = np.eye(4)
M[:3, 3] = direction[:3]
return M
def quaternion_matrix(self, quaternion):
"""Return homogeneous rotation matrix from quaternion.
# >>> R = quaternion_matrix([0.06146124, 0, 0, 0.99810947])
# >>> numpy.allclose(R, rotation_matrix(0.123, (1, 0, 0)))
True
"""
q = np.array(quaternion[:4], dtype=np.float64, copy=True)
nq = np.dot(q, q)
if nq < _EPS:
return np.eye(4)
q *= np.sqrt(2.0 / nq)
q = np.outer(q, q)
return np.array((
(1.0 - q[1, 1] - q[2, 2], q[0, 1] - q[2, 3], q[0, 2] + q[1, 3], 0.0),
(q[0, 1] + q[2, 3], 1.0 - q[0, 0] - q[2, 2], q[1, 2] - q[0, 3], 0.0),
(q[0, 2] - q[1, 3], q[1, 2] + q[0, 3], 1.0 - q[0, 0] - q[1, 1], 0.0),
(0.0, 0.0, 0.0, 1.0)
), dtype=np.float64)
def fromTranslationRotation(self, trans, rot):
return np.dot(self.translation_matrix(trans), self.quaternion_matrix(rot))
The code can run in a realtime setting with the camera ros driver running and getting realtime camera feedbacks. However when I attempted to analyse the rosbags using robag play tf2 was unable to lookuptransform for the second camera (I am using two cameras). It is peculiar that the code is able to get camera transform from one camera only using rosbag play, but failed when getting transforms from both cameras.
Following is the error snippet when attempting to get transforms from the two cameras (left and right):
File src/tracker/SynchronisedCamera.py", line 231, in get_transform_with_retry transform = self.tf_buffer.lookup_transform(self.camera_frame, self.reference_frame, rospy.Time(0), File "/opt/ros/noetic/lib/python3/dist-packages/tf2_ros/buffer.py", line 90, in lookup_transform return self.lookup_transform_core(target_frame, source_frame, time) tf2.LookupException: "camera_right_rgb_optical_frame" passed to lookupTransform argument target_frame does not exist.