Gazebo | Ignition | Community
Ask Your Question
0

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

asked 2021-03-17 10:45:17 -0500

FirefoxMetzger gravatar image

updated 2021-03-17 10:46:45 -0500

I have a world in Ignition Gazebo that contains a camera and a robot arm (panda). The camera has the robot in view. I would like to compute the pixel position of the robot's end-effector within the recorded camera image.

What I would like to know is how/where to find the camera matrix (and information about distortion if applicable) so that I can compute the projection.

I can find the projected position of the end-effector in the camera's frame of reference, but to convert the location into pixels I need the camera's intrinsic parameters. I thought they would be published on the /camera_info topic, but the simulator doesn't appear to publish any messages on this topic. My next thought was to check the camera sensor's source code, but I can't seem to figure out where it is located.

Any pointers where to find this information are highly appreciated :)

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2021-03-19 09:59:55 -0500

FirefoxMetzger gravatar image

updated 2021-03-24 08:17:45 -0500

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:

Projection

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.

edit flag offensive delete link more

Comments

Great that you made it work, good job! To have camera with "standard camera coordinate frame", you can also set the link or sensor pose (relative_to requires SDF 1.7).

<model name="camera_model">
  <pose>0 0 10 0 3.14 0</pose>
  <link name="sensor_link">
    <sensor name="camera_sensor" type="camera">
      <pose relative_to="camera_model::sensor_link">0 0 0 -1.57 -1.57 -1.57</pose>
      <!-- ... -->
    </sensor>
    <!-- ... -->
  </link>
</model>
AndrejOrsula gravatar imageAndrejOrsula ( 2021-03-19 14:42:08 -0500 )edit

@AndrejOrsula Thanks! I would upvote your comment because it is really useful; unfortunately, it requires 3 rep to do so, meaning I will have to wait with that until somebody upvotes my question or answer :)

FirefoxMetzger gravatar imageFirefoxMetzger ( 2021-03-20 01:21:10 -0500 )edit
1

answered 2021-03-17 15:49:11 -0500

AndrejOrsula gravatar image

Your intuition about /camera_info topic is correct. Can you try the following example to see if that works for you?

ign gazebo camera_sensor.sdf -r
ign topic -e -t /camera_info

You should see ign_msgs.CameraInfo messages printed to stdout. In order to see more information about these messages, you can use ign msg -i ign_msgs.CameraInfo.

edit flag offensive delete link more

Comments

Thanks for the reply! Unfortunately, I have a blank output when I run the two lines you mentioned, i.e., echo is waiting for messages to arrive. However, after I click on the button "Refresh list of topics publishing images" in the simulator GUI I start to see messages printed by the echo subscriber. Is this intended? If so, is it possible to get a camera to publish /camera_info immediately after the simulation starts?

FirefoxMetzger gravatar imageFirefoxMetzger ( 2021-03-18 10:50:44 -0500 )edit

I do not believe this behaviour is intended, as sensors should publish data whenever simulation is running. Have you tried subscribing to the topic programatically?

What's interesting is that when I run the example locally, I get the messages immediately. However, I am able to reproduce your issue when I run the same commands using one of my Docker images. The main difference is that locally I build Ignition from source but install Debian package inside Docker.

AndrejOrsula gravatar imageAndrejOrsula ( 2021-03-18 13:12:04 -0500 )edit

As a workaround, you can add <topic>camera</topic> under the ImageDisplay GUI plugin inside SDF and that seems to make it publish immediately.

Don't ask my why, I have no logical explanation. :D Maybe someone who worked on it has an idea.

AndrejOrsula gravatar imageAndrejOrsula ( 2021-03-18 13:32:04 -0500 )edit

Sounds a bit like a bug that has been fixed on master, but didn't propagate to the package archive yet. I've noticed this same behavior when subscribing to topics from python [tracking issue]. There is something funny about tracking subscribers in ign-transport. Another workaround is to additional subscribe to /camera via ign -e -t /camera, which then triggers publication of /camera_info as well. . .

FirefoxMetzger gravatar imageFirefoxMetzger ( 2021-03-18 16:01:07 -0500 )edit

Thanks @AndrejOrdula, I was facing the same issue with my rover world. I was trying to retrieve the image taken by the sensor.

Adwait gravatar imageAdwait ( 2021-04-18 08:11:05 -0500 )edit
Login/Signup to Answer

Question Tools

1 follower

Stats

Asked: 2021-03-17 10:45:17 -0500

Seen: 234 times

Last updated: Mar 24