How to make two frames relative to each other

Question

I have a Velodyne and camera that both have transformations specified relative to the origin of the robot.

The original transformations were specified as yaw, roll, pitch, x, y, z. I wrote a script to convert these to extrinsic matrices (my end goal is to have an extrinsic matrix describing the camera's position relative to the Velodyne).

Figuring out the relative transformation of the camera to the Velodyne is easy for translation - I just calculate the offsets. However, I'm not sure how to approach making the camera rotation relative to the Velodyne, since I believe the translation will also need to be taken into account.

Here is the original ROS transform:

    <node pkg="tf" type="static_transform_publisher" 
        respawn="true" 
        name="camera_static_transform_publisher" 
        args="0.5080 0.0 0.1778 0 0.05 0 base_link camera 100"
    />

    <node pkg="tf" type="static_transform_publisher" 
        respawn="true" 
        name="velodyne_static_transform_publisher" 
        args="0.4445 0.0 0.09525 0.0 0.06981 0.0 base_link velodyne 100"
    />

Here are the extrinsic matrices I calculated:

    Camera extrinsic relative to base_link: 
    [[ 0.99875026  0.          0.04997917  0.508     ]
    [ 0.          1.          0.          0.        ]
    [-0.04997917  0.          0.99875026  0.1778    ]]

    Velodyne extrinsic  relative to base_link: 
    [[ 0.99756427  0.          0.06975331  0.4445    ]
    [ 0.          1.          0.          0.        ]
    [-0.06975331  0.          0.99756427  0.09525   ]]

Research:

• I found this post that mentioned needing to do the inverse of one transformation * the other transformation, but not sure if that works.
• I also found this post that also seems to invert the first transformation and multiply it by the second. I will try this out.

Edit: Here is my attempt

Here is my attempt based on the second post I linked. I'm not sure this is correct because the translation of camera_to_velodyne doesn't match what you would get if you just calculated the offsets between the camera and velodyne.

def get_homogeneous_transformation(rotation, translation):
    """
    Parameters:
    - rotation: a 3x3 rotation np array
    - translation: a 3x1 translation np array
    """
    homogeneous = np.zeros((4, 4))
    homogeneous[-1][-1] = 1
    homogeneous[:3, :3] = rotation
    homogeneous[:3, -1:] = translation
    return homogeneous

def get_relative_transformation(a_to_base, b_to_base):
    """
    Finds the relative transformation from a to b given transformations for both 
    relative to base_link.

    See: https://stackoverflow.com/a/55169091/6942666 for more details

    Parameters:
    - a_to_base: 4x4 np.array representing homoegenous transformation of a to base_link
    - b_to_base: 4x4 np.array representing homoegenous transformation of b to base_link
    """

    base_to_a = np.linalg.inv(a_to_base)
    return base_to_a @ b_to_base

if __name__ == "__main__":
    """
    <node pkg="tf" type="static_transform_publisher" 
        respawn="true" 
        name="camera_static_transform_publisher" 
        args="0.5080 0.0 0.1778 0 0.05 0 base_link camera 100"
    />

    <node pkg="tf" type="static_transform_publisher" 
        respawn="true" 
        name="velodyne_static_transform_publisher" 
        args="0.4445 0.0 0.09525 0.0 0.06981 0.0 base_link velodyne 100"
    />

    Camera static transform:
    [[ 0.99875026  0.          0.04997917  0.508     ]
    [ 0.          1.          0.          0.        ]
    [-0.04997917  0.          0.99875026  0.1778    ]
    [ 0.          0.          0.          1.        ]]

    Velodyne static transform:
    [[ 0.99756427  0.          0.06975331  0.4445    ]
    [ 0.          1.          0.          0.        ]
    [-0.06975331  0.          0.99756427  0.09525   ]
    [ 0.          0.          0.          1.        ]]

    Camera relative to Velodyne
    [[ 0.99980379  0.          0.0198087  -0.05929486]
    [ 0.          1.          0.          0.        ]
    [-0.0198087   0.          0.99980379 -0.08562051]
    [ 0.          0.          0.          1.        ]]
    """

    print("Camera static transform: ")
    camera_to_base = static_transform_to_extrinsic([0.5080, 0.0, 0.1778, 0, 0.05, 0])
    print(camera_to_base)

    print("Velodyne static transform: ")
    velodyne_to_base = static_transform_to_extrinsic([0.4445, 0.0, 0.09525, 0.0, 0.06981, 0.0])
    print(velodyne_to_base)

    print("Camera relative to Velodyne")
    camera_to_velodyne = get_relative_transformation(
        camera_to_base, velodyne_to_base)
    print(camera_to_velodyne)

```

Answer 1

So it seems like what you want to perform is camera-lidar extrinsic calibration. Seems like you have the translations figured out but not the rotation?

You can use one of the already available packages that do this for you. Like Autoware's calibration tool autoware_camera_lidar_calibration. This tool will let you select points in an image and corresponding points in the point-cloud that will be used to calculate a transform between the two and a new frame can be published