0
$\begingroup$

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)

```
$\endgroup$
5
  • 1
    $\begingroup$ So it seems like you figured it out. But just to restate it. Convert the yaw,pitch,roll, translation to homogeneous 4x4 matrices. Then the equation from transform of frame $a$ to $b$ is $T_{ab}=T_{oa}^{-1}*T_{ob}$. Here $o$ is the common origin frame. Also rotation is actually the independent quantity. You can calculate it just by itself without taking the translation into account by utilizing the rotation matrix part of the Transform. $R_{ab}=R_{oa}^-1*R_{ob}$ Translation ,however, is coupled with the rotation and thus requires both. $\endgroup$
    – edwinem
    Nov 18 '20 at 23:50
  • $\begingroup$ Thanks for the response @edwinem! I still don't fully understand why the translation isn't the independent quantity. It seems like if the camera is 3m from the base and the Velodyne is 2m from the base, then you know the camera is 1m from the Velodyne (without needing to take into account how the camera or Velodyne are rotated). $\endgroup$ Nov 19 '20 at 4:17
  • 2
    $\begingroup$ You are actually right. In this example it actually works out since both Transforms are relative to the origin. So any frames in a common coordinate system can solve for the translation with simple vector addition/subtraction. However, as soon as you introduce another frame dependent on a frame in origin. Say an object detected by the LIDAR, or a second camera relative to the first, then this is no longer the case. $\endgroup$
    – edwinem
    Nov 19 '20 at 6:08
  • $\begingroup$ @edwinem got it. Thanks so much! $\endgroup$ Nov 19 '20 at 13:09
  • $\begingroup$ @edwinem - Can you please re-post your comments as an answer? Right now it looks like there are no answers to the question, but it looks like you've got the problem solved. If the question doesn't get an "accepted" or upvoted answer then it gets flagged as unresolved and this question will get bumped for eternity. $\endgroup$
    – Chuck
    Nov 20 '20 at 14:56
0
$\begingroup$

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

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.