An example on performing a 3D projection using Ignitionrobotics, gym-ignition and ropy.

project-3d-camera-info.py

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:
    # Fix: the first step doesn't generate messages.
    # I don't exactly know why; I assume it has
    # to do with subscriptions being updated at the end
    # of the sim loop instead of the beginning?
    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 cone
    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()

world1.sdf

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