Find intrisic camera matrix from gazebo model

Question

Hello,

I made a my camera model in gazebo and I want to extract the intrinsic matrix to use Opencv matrix. Here is my model:

<gazebo reference="camera">
    <material>Gazebo/Green</material>
    <sensor type="camera" name="camera">
      <update_rate>30.0</update_rate>
      <camera name="head">
        <horizontal_fov>1.3962634</horizontal_fov>
        <image>
          <width>1280</width>
          <height>720</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
      </camera>
      <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>0.0</updateRate>
        <cameraName>mdt/camera1</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>camera</frameName>
        <hackBaseline>0.07</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
  </gazebo>

My intrinsic matrix is define like that : M = [ ax 0 cx; 0 ay cy ; 0 0 1 ] I think that cx = 1280 /2 and cy = 720/2 but what about ax/ay ?

---

Originally posted by xavier12358 on ROS Answers with karma: 62 on 2021-04-28

Post score: 0

Answer 1

You can get the intrinsics by subscribing to the /camera_info topic defined in the plugin:

<cameraInfoTopicName>camera_info</cameraInfoTopicName>

The sensor_msgs/CameraInfo message will contain the camera matrix:

# Intrinsic camera matrix for the raw (distorted) images.
#     [fx  0 cx]
# K = [ 0 fy cy]
#     [ 0  0  1]
# Projects 3D points in the camera coordinate frame to 2D pixel
# coordinates using the focal lengths (fx, fy) and principal point
# (cx, cy).
float64[9]  K # 3x3 row-major matrix

Also, you can explicitly set the intrinsics in Gazebo as indicated in the camera SDF specification:

---

###Update:

Note the ROS plugin and Gazebo sensor use two separate intrinsic parameter sets, so it's on the user to make sure they are correct. The ROS plugin source code shows how it uses the parameters only to fill the camera info. Here's an exampe:

<sensor type="camera" name="camera">
  <update_rate>30.0</update_rate>
  <camera name="head">
    <horizontal_fov>1.3962634</horizontal_fov>
    <image>
      <width>800</width>
      <height>800</height>
      <format>R8G8B8</format>
    </image>
    <clip>
      <near>0.02</near>
      <far>300</far>
    </clip>
    <noise>
      <type>gaussian</type>
      <!-- Noise is sampled independently per pixel on each frame.
           That pixel's noise value is added to each of its color
           channels, which at that point lie in the range [0,1]. -->
      <mean>0.0</mean>
      <stddev>0.007</stddev>
    </noise>
    <lens>
      <intrinsics>
        <fx>100</fx>
        <fy>100</fy>
        <cx>640</cx>
        <cy>360</cy>
        <s>0</s>
      </intrinsics>
    </lens>
  </camera>
  <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
    <alwaysOn>true</alwaysOn>
    <updateRate>0.0</updateRate>
    <cameraName>rrbot/camera1</cameraName>
    <imageTopicName>image_raw</imageTopicName>
    <cameraInfoTopicName>camera_info</cameraInfoTopicName>
    <frameName>camera_link</frameName>
    <Cx>640</Cx>
    <Cy>360</Cy>
    <focalLength>100</focalLength>
  </plugin>
</sensor>

---

Originally posted by tryan with karma: 1421 on 2021-04-28

This answer was ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by xavier12358 on 2021-04-29:
Indeed I can't get the intrinsic matrix like this. Thank you. About the SDF camera specifications. I tried this:

<?xml version="1.0"?>
<robot>
      <gazebo reference="camera">
        <material>Gazebo/Green</material>
        <sensor type="camera" name="camera">
          <update_rate>30.0</update_rate>
          <camera name="head">
         .....
              <near>0.2</near>
              <far>600</far>
            </clip>
            <lens>
                <intrinsics>
                        <fx>100</fx>
                        <fy>100</fy>
                        <cx>640</cx>
                        <cy>360</cy>
                        <s>0</s>s>
                </intrinsics>
            </lens>
          </camera>

I get this:

Warning [parser.cc:950] XML Element[intrinsics], child of element[lens] not defined in SDF. Ignoring[intrinsics]. You may have an incorrect SDF file, or an sdformat version that doesn't support this element.

Comment by tryan on 2021-04-29:\

Indeed I can't get the intrinsic matrix like this.

Why not?

As for the SDF, I don't get that error. Maybe you have a different Gazebo/SDF version. I updated my answer with an example.

Comment by xavier12358 on 2021-04-29:
Sorry I made a error when I wrote that. I CAN get the intrinsic file . Thank you.

Comment by xavier12358 on 2021-04-30:
I try to add the intrinsics parameters like in your example. The problem is the horizontal FOV had to be defined also with the intrinsic parameters, which is not consistent. If I not specify horizontal FOV, it use the default HFOV and I get that warning (and the camera info is not published):

[ WARN] [1619767559.786164054]: The <focal_length>[1067.000000] you have provided for camera_ [camera] is inconsistent with specified image_width [1280] and HFOV [1.047000].   Please double check to see that focal_length = width_ / (2.0 * tan(HFOV/2.0)), the explected focal_lengtth value is [1108.765426], please update your camera_ model description accordingly.

So I just use that formula to set the HFOV and to fill the matrix and not the intrinsic section which is useless...

Comment by Anvesh Som on 2022-01-24:
Thanks for the tip about FOV, but I still cannot solve the problem with the parser not parsing the intrinsics tag and ignoring it. Child of lens tag.

Comment by noah on 2022-06-27:
It seems that your issue is that you have <s>0</s>s> rather than <s>0</s> as one of the children of <intrinsics>