CameraInfo's projection matrix not updating when resolution changes

[FirefoxMetzger]

Environment

• OS Version: Ubuntu 20.04
• Source or binary build? Source Build

Description

• Expected behavior: The camera projection matrix changes when the resolution of the camera's image sensor changes
• Actual behavior: The projection matrix remains invariant to changes in resolution.

Steps to reproduce

Example World 1 (camera resolution 320x240 [default])

```xml 0.001 1 1000 ogre 1 1 1 1 0.8 0.8 0.8 1 1 1 0 docked ogre scene 1.0 1.0 1.0 0.8 0.8 0.8 -6 0 6 0 0.5 0 0 0 72 121 1 floating 1 1 1 0 0 110 290 1 floating 1 1 1 1 docked 1 0 0 10 0 -0 0 0.8 0.8 0.8 1 0.8 0.8 0.8 1 1000 0.9 0.01 0.001 -0.5 0.1 -0.9 0 0 0 0 0 -9.8 6e-06 2.3e-05 -4.2e-05 1 0 0 1 100 100 0 0 1 100 100 0.8 0.8 0.8 1 0.8 0.8 0.8 1 0.8 0.8 0.8 1 0 0 0 0 -0 0 0 -1 0.5 0 -0 0 1 0 0 1 0 1 1 1 1 1 1 1 1 1 0 0 1 1 0 0 1 1 0 0 1 1 3 0 0.5 0 -0 0 0.125 0.125 0 1 0 1 0 1 0 1 0 1 0 1 0 1 4 -0 1 0 -0 3.14 0.05 0.05 0.05 0 -0 0 0.1 0.000166667 0.000166667 0.000166667 0 0 0 0.1 0.1 0.1 0.1 0.1 0.1 1.047 320 240 0.1 100 1 30 1 camera ```

Example World 2 (camera resolution 640x480)

```xml 0.001 1 1000 ogre 1 1 1 1 0.8 0.8 0.8 1 1 1 0 docked ogre scene 1.0 1.0 1.0 0.8 0.8 0.8 -6 0 6 0 0.5 0 0 0 72 121 1 floating 1 1 1 0 0 110 290 1 floating 1 1 1 1 docked 1 0 0 10 0 -0 0 0.8 0.8 0.8 1 0.8 0.8 0.8 1 1000 0.9 0.01 0.001 -0.5 0.1 -0.9 0 0 0 0 0 -9.8 6e-06 2.3e-05 -4.2e-05 1 0 0 1 100 100 0 0 1 100 100 0.8 0.8 0.8 1 0.8 0.8 0.8 1 0.8 0.8 0.8 1 0 0 0 0 -0 0 0 -1 0.5 0 -0 0 1 0 0 1 0 1 1 1 1 1 1 1 1 1 0 0 1 1 0 0 1 1 0 0 1 1 3 0 0.5 0 -0 0 0.125 0.125 0 1 0 1 0 1 0 1 0 1 0 1 0 1 4 -0 1 0 -0 3.14 0.05 0.05 0.05 0 -0 0 0.1 0.000166667 0.000166667 0.000166667 0 0 0 0.1 0.1 0.1 0.1 0.1 0.1 1.047 640 480 0.1 100 1 30 1 camera ```

Both worlds are identical except for the resolution of the camera. The camera resolution being different means that the projection matrix published under /camera_info should change; however, it doesn't. For both worlds it is

[
[277, 0, 160, 0],
[0, 277, 120, 0],
[0, 0, 1, 0]
]

whereas for the second world, it should be

[
[277, 0, 320, 0],
[0, 277, 240, 0],
[0, 0, 1, 0]
]

This leads to problems down the line because projections are off:

Projection World 1

Projection World 2

There are also two lower-level problems:

1. The camera takes pictures with the x-axis pointing forward, but the projection matrix is given assuming that the z-axis is pointing forward (can be fixed by applying an appropriate rotation)
2. It appears to be impossible to change the topic that publishes the camera info. This leads to conflicts if there are multiple cameras in the scene. (minor)

Here is the full code to produce the images above.

Code

```python from scenario import gazebo as scenario_gazebo import numpy as np import matplotlib.pyplot as plt from matplotlib.patches import Circle from scipy.spatial.transform import Rotation as R import ropy.transform as tf import ropy.ignition as ign def camera_parser(msg): image_msg = ign.messages.Image() image_msg.parse(msg[2]) image = np.frombuffer(image_msg.data, dtype=np.uint8) image = image.reshape((image_msg.height, image_msg.width, 3)) return image def camera_info_parser(msg): return ign.messages.CameraInfo().parse(msg[2]) gazebo = scenario_gazebo.GazeboSimulator(step_size=0.001, rtf=1.0, steps_per_run=1) assert gazebo.insert_world_from_sdf("./world1.sdf") gazebo.initialize() # Fix: available topics seem to only be updated at the end # of a step. This allows the subscribers to find the topic's # address gazebo.run(paused=True) # get extrinsic matrix camera = gazebo.get_world("camera_sensor").get_model("camera").get_link("link") cam_pos_world = np.array(camera.position()) cam_ori_world_quat = np.array(camera.orientation())[[1, 2, 3, 0]] cam_ori_world = R.from_quat(cam_ori_world_quat).as_euler("xyz") camera_frame_world = np.stack((cam_pos_world, cam_ori_world)).ravel() extrinsic_transform = tf.coordinates.transform(camera_frame_world) # scene objects box = gazebo.get_world("camera_sensor").get_model("box") with ign.Subscriber("/camera", parser=camera_parser) as camera_topic, ign.Subscriber( "/camera_info", parser=camera_info_parser ) as cam_info_topic: gazebo.run(paused=True) img = camera_topic.recv() # get intrinsic matrix cam_info = cam_info_topic.recv() rot = tf.coordinates.transform(np.array((0, 0, 0, -np.pi / 2, -np.pi / 2, 0))) projection = np.array(cam_info.projection.p).reshape((3, 4)) intrinsic_transform = np.matmul(projection, rot) # project corner box_corner = np.array(box.base_position()) + np.array((0.5, -0.5, 0.5)) pos_world = tf.homogenize(box_corner) pos_cam = np.matmul(extrinsic_transform, pos_world) pos_px_hom = np.matmul(intrinsic_transform, pos_cam) cube_pos_px = tf.cartesianize(pos_px_hom) # visualize fig, ax = plt.subplots(1) ax.imshow(img) ax.add_patch(Circle(cube_pos_px, radius=6)) plt.show() gazebo.close() ```

To run this, a working gym-ignition and ropy[ignition] (not on pypi yet, but easy to install locally) installation are required.

[andyblarblar]

Any update on this? This issue is causing major issues for a project I'm on.

[ahcorde]

Not really sure which Gazebo version are you using @andyblarblar, but I backported some commits that should fix the issue https://github.com/gazebosim/gz-sensors/pull/383

[andyblarblar]

Not really sure which Gazebo version are you using @andyblarblar, but I backported some commits that should fix the issue https://github.com/gazebosim/gz-sensors/pull/383

Yeah, sorry for not giving more details. I've been having issues in garden 7.6 binary release. I can try fortress as well to see if anything has changed, because I'm on Humble.

[wittenator]

This seems to still be a problem in Gazebo Harmonic. I changed the resolution of my camera to a higher resolution and checked the camera_info message which now has an intrinsic matrix that doesn't make much sense (especially cx and cy).

header:
  stamp:
    sec: 7
    nanosec: 992000000
  frame_id: spawn_robot/base_link/boundingbox_camera
height: 1536
width: 2048
distortion_model: plumb_bob
d:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
k:
- 277.0
- 0.0
- 160.0
- 0.0
- 277.0
- 120.0
- 0.0
- 0.0
- 1.0
r:
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
p:
- 277.0
- 0.0
- 160.0
- -0.0
- 0.0
- 277.0
- 120.0
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
binning_x: 0
binning_y: 0
roi:
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: false