How to get the camera matrix of a camera sensor in Ignition Gazebo

edit

It is indeed possible to get the camera matrix from the /camera_info topic. It also directly provides a projection matrix to map between pixel coordinates and meters.

However, there is a gotcha that is not so obvious: Gazebo doesn't follow the typical convention of defining the camera's coordinate system with the z-axis pointing forward. Instead, it follows the default robotics joint convention and cameras have the x-axis pointing forward (z-axis pointing up). The projection matrix, however, is given in a format that assumes the standard camera coordinate frame (z-axis pointing forward, y-axis pointing down). As a result, an additional transformation (rotation) is needed to convert the position, before applying the projection.

Here is the critical piece of code in python to (hopefully) make things more clear:

def world2px_cam_info(pos_world):
    # camera info msg based projection

    ## pos in world coords -> pos in camera coords
    # gazebo doesn't use conventional camera coordinates
    # camera points in x-direction instead of z-direction
    tf = transform(np.array((0,0,0, np.pi/2, np.pi/2, 0)))
    pos_cam_gazebo = homogenize(world2cam(pos_world))
    pos_cam = np.matmul(tf, pos_cam_gazebo)

    projection = np.array(cam_info.projection.p).reshape((3, 4))
    pos_px = cartesianize(np.matmul(projection, pos_cam))

    #image indexing uses y-x order instead of x-y
    pos_px = pos_px[::1]

    return pos_px

I've added a full code example below (using gym-ignition and ropy). It produces the following image:

https://gist.github.com/FirefoxMetzge...

Edit: The .sdf was causing problems because it contained absolute paths for my local file system due to the traffic cone. I updated the .sdf, moved the code into a gist for syntax highlighting, and added ropy which takes care of the additional rotation and makes the code cleaner overall.