gz-sim
gz-sim copied to clipboard
Published
20 hours ago •
gazebosim
gazebosim
CameraInfo's projection matrix not updating when resolution changes
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:
- 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)
- 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.
