Camera frames do not correspond to the docs

[kschwan]

Camera frames do not correspond to the docs

[kschwan]

Just to be clear, what I'm doing here is using the following script to publish frames on /tf and then visualizing that with RViz.

import rospy
import rostopic
import geometry_msgs.msg
import tf2_ros


class Listener:
    def __init__(self, topic):
        self.msg = None
        msg_type = rostopic.get_topic_class(topic)[0]
        self.sub = rospy.Subscriber(topic, msg_type, self.set_msg)

    def set_msg(self, msg):
        self.msg = msg


def wait_for_message(topic):
    msg_type = rostopic.get_topic_class(topic)[0]
    return rospy.wait_for_message(topic, msg_type)


def pose_to_transform(pose):
    tf = geometry_msgs.msg.Transform()
    tf.translation.x = pose.position.x
    tf.translation.y = pose.position.y
    tf.translation.z = pose.position.z
    tf.rotation.w = pose.orientation.w
    tf.rotation.x = pose.orientation.x
    tf.rotation.y = pose.orientation.y
    tf.rotation.z = pose.orientation.z
    return tf


rospy.init_node('ambf_state_publisher')
ambf_static = {
    'psm1/baselink': wait_for_message('/ambf/env/psm1/baselink/State'),
    'psm2/baselink': wait_for_message('/ambf/env/psm2/baselink/State'),
}
ambf_moving = {
    'Needle': Listener('/ambf/env/Needle/State'),
    'CameraFrame': Listener('/ambf/env/CameraFrame/State'),
    'cameraL': Listener('/ambf/env/cameras/cameraL/State'),
    'cameraR': Listener('/ambf/env/cameras/cameraR/State'),
    'Entry1': Listener('/ambf/env/Entry1/State'),
    'Entry2': Listener('/ambf/env/Entry2/State'),
    'Entry3': Listener('/ambf/env/Entry3/State'),
    'Entry4': Listener('/ambf/env/Entry4/State'),
    'Exit1': Listener('/ambf/env/Exit1/State'),
    'Exit2': Listener('/ambf/env/Exit2/State'),
    'Exit3': Listener('/ambf/env/Exit3/State'),
    'Exit4': Listener('/ambf/env/Exit4/State'),
    # 'psm1/toolyawlink': Listener('/ambf/env/psm1/toolyawlink/State'),
    # 'psm2/toolyawlink': Listener('/ambf/env/psm2/toolyawlink/State'),
}

tf_static_broadcaster = tf2_ros.StaticTransformBroadcaster()
tf_broadcaster = tf2_ros.TransformBroadcaster()
transforms = []

for k, v in ambf_static.items():
    tf = geometry_msgs.msg.TransformStamped()
    tf.header.stamp = rospy.Time.now()
    tf.header.frame_id = 'world'
    tf.child_frame_id = k
    tf.transform = pose_to_transform(v.pose)
    transforms.append(tf)

tf_static_broadcaster.sendTransform(transforms)
rate = rospy.Rate(30)

while not rospy.is_shutdown():
    transforms = []

    for k, v in ambf_moving.items():
        if v.msg is None:
            continue

        tf = geometry_msgs.msg.TransformStamped()
        tf.header.stamp = rospy.Time.now()
        try:
            tf.header.frame_id = v.msg.parent_name.data.replace('BODY ', '')
        except AttributeError:
            tf.header.frame_id = 'world'
        tf.child_frame_id = k
        tf.transform = pose_to_transform(v.msg.pose)
        transforms.append(tf)

    tf_broadcaster.sendTransform(transforms)
    rate.sleep()

[adnanmunawar]

Hi Kim,
Thanks for bringing this up. Hmm, The frames for cameraL and cameraR appear correct to me in the camera_conventions.md doc. @jabarragann what do you think?

[adnanmunawar]

Thanks for posting the code for visualizing the frames. I want to check what specific part of the camera_conventions.md do you think is incorrect.
This one?

[kschwan]

Yes, if I'm not mistaken, the one you posted is rotated 180 deg about the Z axis compared to the actual one from the simulator.

[adnanmunawar]

Umm, just so that we are on the same page, the conventions depicted in the two images are not related. This image:

shows the image plane with respect to any AMBF camera, i.e. it is along the camera's negative X axis. It also shows the difference between the OpenCV and AMBF camera conventions with respect to the image plane.

On the other hand, this image that you shared shows the cameraR and cameraL with respect to an empty kinematic body which is called CameraFrame. This kinematic body is placed similar to the dVRK endoscopes end-effector frame and does not have anything to do with the OpenCV frame.

[kschwan]

Maybe I'm completely misunderstanding something or I'm not explaining myself very well 😅

Let me try drawing:

To be clear, I'm talking about how cameraL and cameraR are oriented -- not the OpenCV frame convention or the CameraFrame frame.

[kschwan]

Oooohhh, wait! I now realize I was reading the figure wrong. Stupid me. Sorry to have you spend time on this :|

[adnanmunawar]

No problem, happy to help. :)