Skip to content

Wideangle Camera Sensor does publish incorrect camera_info #592

@eubotx

Description

@eubotx

Environment

Ubuntu24.04 + Ros2 Jazzy + Gazebo Sim, version 8.10.0 (Harmonic)

Description

On using the wideangle camera sensor the camera_info topic contains wrong data.

  • Expected behavior: Camera info contains the correct distortion model and parameters. In this example equidistant and the correct parameters.
  • Actual behavior: Camera info contains wrong distortion model and parameters of unclear origin. In this example plumb_bob and distortion coefficients 0.

Steps to reproduce

Used model sdf

<?xml version="1.0"?>
<sdf version="1.6">
  <model name="arena_camera_wideangle">
    <static>true</static>
    <link name="link">
      <pose>0.75 0.75 1.0 0 1.5708 0</pose>
      
      <!-- Visual element to see the camera -->
      <visual name="camera_visual">
        <geometry>
          <box>
            <size>0.1 0.1 0.1</size>
          </box>
        </geometry>
        <material>
          <ambient>1 0 0 1</ambient>
          <diffuse>1 0 0 1</diffuse>
        </material>
      </visual>
      
      <sensor name="arena_camera" type="wideanglecamera">
        <pose>0 0 0 0 0 0</pose>
        <frame_id>arena_camera_optical</frame_id>
        <gz_frame_id>arena_camera_optical</gz_frame_id>
        <camera>
          <horizontal_fov>2.2689</horizontal_fov> <!-- 130 degrees -->
          <image>
            <width>800</width>
            <height>600</height>
            <format>R8G8B8</format>
          </image>
          <clip>
            <near>0.01</near>
            <far>2</far>
          </clip>

          <optical_frame_id>arena_camera_optical</optical_frame_id>
          <camera_info_topic>arena_camera/camera_info</camera_info_topic>

          <lens>
            <type>equidistant</type>
            <scale_to_hfov>true</scale_to_hfov>
            <cutoff_angle>1.5707</cutoff_angle>
            <env_texture_size>512</env_texture_size>
          </lens>
        </camera>
        <always_on>true</always_on>
        <update_rate>60</update_rate>
        <visualize>true</visualize>
        <topic>arena_camera/image</topic>
        <plugin filename="ignition-gazebo-sensors-system" name="ignition::gazebo::systems::Sensors">
          <render_engine>ogre2</render_engine>
        </plugin>
      </sensor>
    </link>
  </model>
</sdf>

Output

Published camera info message:

header:
  stamp:
    sec: 4
    nanosec: 849000000
  frame_id: arena_camera_optical
height: 600
width: 800
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
---

##Further Info

A camera sensor with pinhole model with the following configuration does publish correct information. However applying the same sdf style to the wideangle camera did not resolve the issue.

<?xml version="1.0"?>
<sdf version="1.9">
  <model name="arena_camera_pinhole">
    <static>true</static>
    <link name="link">
      <pose>0.75 0.75 1.0 0 1.5708 0</pose>
      
      <!-- Visual element to see the camera -->
      <visual name="camera_visual">
        <geometry>
          <box>
            <size>0.1 0.1 0.1</size>
          </box>
        </geometry>
        <material>
          <ambient>1 1 0 1</ambient> <!-- Yellow -->
          <diffuse>1 1 0 1</diffuse>
        </material>
      </visual>
      
      <sensor name="arena_camera" type="camera">
        <pose>0 0 0 0 0 0</pose>
        <gz_frame_id>arena_camera_optical</gz_frame_id>
        <frame_id>arena_camera_optical</frame_id> <!-- Future proofing for https://github.com/gazebosim/gz-sensors/pull/444 -->
        <camera name="arena_camera">
          <horizontal_fov>1.689</horizontal_fov>
          <lens>
            <intrinsics>
              <fx>355.923</fx>
              <fy>355.968</fy>
              <cx>399.419</cx>
              <cy>299.149</cy>
              <s>0.0</s>
            </intrinsics>
          </lens>
          <distortion>
            <k1>0.0</k1>
            <k2>0.0</k2>
            <p1>0.0</p1>
            <p2>0.0</p2>
            <k3>0.0</k3>
          </distortion>
          <image>
            <width>800</width>
            <height>600</height>
            <format>R8G8B8</format>
          </image>

          <clip>
            <near>0.01</near>
            <far>2</far>
          </clip>
          <noise>
             <type>gaussian</type>
             <mean>0</mean>
             <stddev>0.007</stddev>
           </noise>
        </camera>
        <always_on>true</always_on>
        <update_rate>60</update_rate>
        <visualize>false</visualize>
        <topic>arena_camera/image</topic>
        <plugin filename="gz-sim-sensors-system" name="gz::sim::systems::Sensors">
          <render_engine>ogre2</render_engine>
        </plugin>
      </sensor>
    </link>
  </model>
</sdf>

Metadata

Metadata

Assignees

No one assigned

    Labels

    bugSomething isn't working

    Type

    No type

    Projects

    Status

    Inbox

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions