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])

<sdf version='1.7'>
  <world name='camera_sensor'>
    <physics name='1ms' type='ignored'>
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1</real_time_factor>
      <real_time_update_rate>1000</real_time_update_rate>
    </physics>
    <plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
    <plugin name='ignition::gazebo::systems::Sensors' filename='ignition-gazebo-sensors-system'>
      <render_engine>ogre</render_engine>
    </plugin>
    <plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
    <plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
    <scene>
      <ambient>1 1 1 1</ambient>
      <background>0.8 0.8 0.8 1</background>
      <grid>1</grid>
      <shadows>1</shadows>
    </scene>
    <gui fullscreen='0'>
      <plugin name='3D View' filename='GzScene3D'>
        <ignition-gui>
          <title>3D View</title>
          <property type='bool' key='showTitleBar'>0</property>
          <property type='string' key='state'>docked</property>
        </ignition-gui>
        <engine>ogre</engine>
        <scene>scene</scene>
        <ambient_light>1.0 1.0 1.0</ambient_light>
        <background_color>0.8 0.8 0.8</background_color>
        <camera_pose>-6 0 6 0 0.5 0</camera_pose>
      </plugin>
      <plugin name='World control' filename='WorldControl'>
        <ignition-gui>
          <title>World control</title>
          <property type='bool' key='showTitleBar'>0</property>
          <property type='bool' key='resizable'>0</property>
          <property type='double' key='height'>72</property>
          <property type='double' key='width'>121</property>
          <property type='double' key='z'>1</property>
          <property type='string' key='state'>floating</property>
          <anchors target='3D View'>
            <line own='left' target='left'/>
            <line own='bottom' target='bottom'/>
          </anchors>
        </ignition-gui>
        <play_pause>1</play_pause>
        <step>1</step>
        <start_paused>1</start_paused>
      </plugin>
      <plugin name='World stats' filename='WorldStats'>
        <ignition-gui>
          <title>World stats</title>
          <property type='bool' key='showTitleBar'>0</property>
          <property type='bool' key='resizable'>0</property>
          <property type='double' key='height'>110</property>
          <property type='double' key='width'>290</property>
          <property type='double' key='z'>1</property>
          <property type='string' key='state'>floating</property>
          <anchors target='3D View'>
            <line own='right' target='right'/>
            <line own='bottom' target='bottom'/>
          </anchors>
        </ignition-gui>
        <sim_time>1</sim_time>
        <real_time>1</real_time>
        <real_time_factor>1</real_time_factor>
        <iterations>1</iterations>
      </plugin>
      <plugin name='Image Display' filename='ImageDisplay'>
        <ignition-gui>
          <property key='state' type='string'>docked</property>
        </ignition-gui>
      </plugin>
    </gui>
    <light name='sun' type='directional'>
      <cast_shadows>1</cast_shadows>
      <pose>0 0 10 0 -0 0</pose>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.8 0.8 0.8 1</specular>
      <attenuation>
        <range>1000</range>
        <constant>0.9</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <direction>-0.5 0.1 -0.9</direction>
      <spot>
        <inner_angle>0</inner_angle>
        <outer_angle>0</outer_angle>
        <falloff>0</falloff>
      </spot>
    </light>
    <gravity>0 0 -9.8</gravity>
    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
    <atmosphere type='adiabatic'/>
    <model name='ground_plane'>
      <static>1</static>
      <link name='link'>
        <collision name='collision'>
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <contact/>
          </surface>
        </collision>
        <visual name='visual'>
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <material>
            <ambient>0.8 0.8 0.8 1</ambient>
            <diffuse>0.8 0.8 0.8 1</diffuse>
            <specular>0.8 0.8 0.8 1</specular>
          </material>
          <plugin name='__default__' filename='__default__'/>
        </visual>
      </link>
      <plugin name='__default__' filename='__default__'/>
      <pose>0 0 0 0 -0 0</pose>
    </model>
    <model name='box'>
      <pose>0 -1 0.5 0 -0 0</pose>
      <link name='box_link'>
        <inertial>
          <inertia>
            <ixx>1</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>1</iyy>
            <iyz>0</iyz>
            <izz>1</izz>
          </inertia>
          <mass>1</mass>
        </inertial>
        <collision name='box_collision'>
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <contact/>
          </surface>
        </collision>
        <visual name='box_visual'>
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
          <material>
            <ambient>1 0 0 1</ambient>
            <diffuse>1 0 0 1</diffuse>
            <specular>1 0 0 1</specular>
          </material>
          <plugin name='__default__' filename='__default__'/>
        </visual>
      </link>
      <plugin name='__default__' filename='__default__'/>
    </model>
    <model name='sphere'>
      <static>1</static>
      <pose>3 0 0.5 0 -0 0</pose>
      <link name='sphere_link'>
        <collision name='sphere_collision'>
          <geometry>
            <sphere>
              <radius>0.125</radius>
            </sphere>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <contact/>
          </surface>
        </collision>
        <visual name='sphere_visual'>
          <geometry>
            <sphere>
              <radius>0.125</radius>
            </sphere>
          </geometry>
          <material>
            <ambient>0 1 0 1</ambient>
            <diffuse>0 1 0 1</diffuse>
            <specular>0 1 0 1</specular>
          </material>
          <cast_shadows>0</cast_shadows>
          <plugin name='__default__' filename='__default__'/>
        </visual>
      </link>
      <plugin name='__default__' filename='__default__'/>
    </model>
    <model name='camera'>
      <static>1</static>
      <pose>4 -0 1 0 -0 3.14</pose>
      <link name='link'>
        <pose>0.05 0.05 0.05 0 -0 0</pose>
        <inertial>
          <mass>0.1</mass>
          <inertia>
            <ixx>0.000166667</ixx>
            <iyy>0.000166667</iyy>
            <izz>0.000166667</izz>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyz>0</iyz>
          </inertia>
        </inertial>
        <collision name='collision'>
          <geometry>
            <box>
              <size>0.1 0.1 0.1</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <contact/>
          </surface>
        </collision>
        <visual name='visual'>
          <geometry>
            <box>
              <size>0.1 0.1 0.1</size>
            </box>
          </geometry>
          <plugin name='__default__' filename='__default__'/>
        </visual>
        <sensor name='camera' type='camera'>
          <camera>
            <horizontal_fov>1.047</horizontal_fov>
            <image>
              <width>320</width>
              <height>240</height>
            </image>
            <clip>
              <near>0.1</near>
              <far>100</far>
            </clip>
          </camera>
          <always_on>1</always_on>
          <update_rate>30</update_rate>
          <visualize>1</visualize>
          <topic>camera</topic>
          <plugin name='__default__' filename='__default__'/>
        </sensor>
      </link>
      <plugin name='__default__' filename='__default__'/>
    </model>
  </world>
</sdf>

Example World 2 (camera resolution 640x480)

<sdf version='1.7'>
  <world name='camera_sensor'>
    <physics name='1ms' type='ignored'>
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1</real_time_factor>
      <real_time_update_rate>1000</real_time_update_rate>
    </physics>
    <plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
    <plugin name='ignition::gazebo::systems::Sensors' filename='ignition-gazebo-sensors-system'>
      <render_engine>ogre</render_engine>
    </plugin>
    <plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
    <plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
    <scene>
      <ambient>1 1 1 1</ambient>
      <background>0.8 0.8 0.8 1</background>
      <grid>1</grid>
      <shadows>1</shadows>
    </scene>
    <gui fullscreen='0'>
      <plugin name='3D View' filename='GzScene3D'>
        <ignition-gui>
          <title>3D View</title>
          <property type='bool' key='showTitleBar'>0</property>
          <property type='string' key='state'>docked</property>
        </ignition-gui>
        <engine>ogre</engine>
        <scene>scene</scene>
        <ambient_light>1.0 1.0 1.0</ambient_light>
        <background_color>0.8 0.8 0.8</background_color>
        <camera_pose>-6 0 6 0 0.5 0</camera_pose>
      </plugin>
      <plugin name='World control' filename='WorldControl'>
        <ignition-gui>
          <title>World control</title>
          <property type='bool' key='showTitleBar'>0</property>
          <property type='bool' key='resizable'>0</property>
          <property type='double' key='height'>72</property>
          <property type='double' key='width'>121</property>
          <property type='double' key='z'>1</property>
          <property type='string' key='state'>floating</property>
          <anchors target='3D View'>
            <line own='left' target='left'/>
            <line own='bottom' target='bottom'/>
          </anchors>
        </ignition-gui>
        <play_pause>1</play_pause>
        <step>1</step>
        <start_paused>1</start_paused>
      </plugin>
      <plugin name='World stats' filename='WorldStats'>
        <ignition-gui>
          <title>World stats</title>
          <property type='bool' key='showTitleBar'>0</property>
          <property type='bool' key='resizable'>0</property>
          <property type='double' key='height'>110</property>
          <property type='double' key='width'>290</property>
          <property type='double' key='z'>1</property>
          <property type='string' key='state'>floating</property>
          <anchors target='3D View'>
            <line own='right' target='right'/>
            <line own='bottom' target='bottom'/>
          </anchors>
        </ignition-gui>
        <sim_time>1</sim_time>
        <real_time>1</real_time>
        <real_time_factor>1</real_time_factor>
        <iterations>1</iterations>
      </plugin>
      <plugin name='Image Display' filename='ImageDisplay'>
        <ignition-gui>
          <property key='state' type='string'>docked</property>
        </ignition-gui>
      </plugin>
    </gui>
    <light name='sun' type='directional'>
      <cast_shadows>1</cast_shadows>
      <pose>0 0 10 0 -0 0</pose>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.8 0.8 0.8 1</specular>
      <attenuation>
        <range>1000</range>
        <constant>0.9</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <direction>-0.5 0.1 -0.9</direction>
      <spot>
        <inner_angle>0</inner_angle>
        <outer_angle>0</outer_angle>
        <falloff>0</falloff>
      </spot>
    </light>
    <gravity>0 0 -9.8</gravity>
    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
    <atmosphere type='adiabatic'/>
    <model name='ground_plane'>
      <static>1</static>
      <link name='link'>
        <collision name='collision'>
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <contact/>
          </surface>
        </collision>
        <visual name='visual'>
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <material>
            <ambient>0.8 0.8 0.8 1</ambient>
            <diffuse>0.8 0.8 0.8 1</diffuse>
            <specular>0.8 0.8 0.8 1</specular>
          </material>
          <plugin name='__default__' filename='__default__'/>
        </visual>
      </link>
      <plugin name='__default__' filename='__default__'/>
      <pose>0 0 0 0 -0 0</pose>
    </model>
    <model name='box'>
      <pose>0 -1 0.5 0 -0 0</pose>
      <link name='box_link'>
        <inertial>
          <inertia>
            <ixx>1</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>1</iyy>
            <iyz>0</iyz>
            <izz>1</izz>
          </inertia>
          <mass>1</mass>
        </inertial>
        <collision name='box_collision'>
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <contact/>
          </surface>
        </collision>
        <visual name='box_visual'>
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
          <material>
            <ambient>1 0 0 1</ambient>
            <diffuse>1 0 0 1</diffuse>
            <specular>1 0 0 1</specular>
          </material>
          <plugin name='__default__' filename='__default__'/>
        </visual>
      </link>
      <plugin name='__default__' filename='__default__'/>
    </model>
    <model name='sphere'>
      <static>1</static>
      <pose>3 0 0.5 0 -0 0</pose>
      <link name='sphere_link'>
        <collision name='sphere_collision'>
          <geometry>
            <sphere>
              <radius>0.125</radius>
            </sphere>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <contact/>
          </surface>
        </collision>
        <visual name='sphere_visual'>
          <geometry>
            <sphere>
              <radius>0.125</radius>
            </sphere>
          </geometry>
          <material>
            <ambient>0 1 0 1</ambient>
            <diffuse>0 1 0 1</diffuse>
            <specular>0 1 0 1</specular>
          </material>
          <cast_shadows>0</cast_shadows>
          <plugin name='__default__' filename='__default__'/>
        </visual>
      </link>
      <plugin name='__default__' filename='__default__'/>
    </model>
    <model name='camera'>
      <static>1</static>
      <pose>4 -0 1 0 -0 3.14</pose>
      <link name='link'>
        <pose>0.05 0.05 0.05 0 -0 0</pose>
        <inertial>
          <mass>0.1</mass>
          <inertia>
            <ixx>0.000166667</ixx>
            <iyy>0.000166667</iyy>
            <izz>0.000166667</izz>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyz>0</iyz>
          </inertia>
        </inertial>
        <collision name='collision'>
          <geometry>
            <box>
              <size>0.1 0.1 0.1</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <contact/>
          </surface>
        </collision>
        <visual name='visual'>
          <geometry>
            <box>
              <size>0.1 0.1 0.1</size>
            </box>
          </geometry>
          <plugin name='__default__' filename='__default__'/>
        </visual>
        <sensor name='camera' type='camera'>
          <camera>
            <horizontal_fov>1.047</horizontal_fov>
            <image>
              <width>640</width>
              <height>480</height>
            </image>
            <clip>
              <near>0.1</near>
              <far>100</far>
            </clip>
          </camera>
          <always_on>1</always_on>
          <update_rate>30</update_rate>
          <visualize>1</visualize>
          <topic>camera</topic>
          <plugin name='__default__' filename='__default__'/>
        </sensor>
      </link>
      <plugin name='__default__' filename='__default__'/>
    </model>
  </world>
</sdf>

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

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 gazebosim/gz-sensors#383

[andyblarblar]

Not really sure which Gazebo version are you using @andyblarblar, but I backported some commits that should fix the issue gazebosim/gz-sensors#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