Unable to apply Camera Distortion on Gazebo Harmonic

Question

I’m currently working on a project for a drone, trying to simulate a Raspberry Pi V2.1 camera, but I’m currently facing difficulties in visualizing the distortion being applied in the camera image. I'm using ROS2 Humble and Gazebo Harmonic.

This is my camera model:

      <sensor name="IMX219" type="camera">
        <pose>0.01233 -0.03 .01878 0 0 0</pose>
        <camera>
          <image>
            <width>640</width>
            <height>480</height>
          </image>
          <clip>
            <near>0.1</near>
            <far>100</far>
          </clip>
          <distortion>
            <k1>1.578758331993112829e-01</k1>
            <k2>-5.784303605943902360e-01</k2>
            <k3>-8.740723680089258069e-03</k3>
            <p1>1.359451618091441112e-03</p1>
            <p2>1.640043376158566713e-01</p2>
            <center>0.5 0.5</center>
          </distortion>
          <lens>
            <type>pinhole</type>
            <scale_to_hfov>false</scale_to_hfov>
            <intrinsics>
              <fx>4.970870507466349295e+02</fx>
              <fy>4.976011127668800782e+02</fy>
              <cx>3.168387473166100108e+02</cx>
              <cy>2.342740555042151982e+02</cy>
              <s>0.0</s>
            </intrinsics>
          </lens>
        </camera>
        <always_on>1</always_on>
        <update_rate>30</update_rate>
        <visualize>true</visualize>
        <topic>camera</topic>
      </sensor>

Reading the values from /camera_info topic, I can see that the values are being set:

header {
  stamp {
    sec: 123
    nsec: 224000000
  }
  data {
    key: "frame_id"
    value: "x500_depth_0::OakD-Lite/base_link::IMX219"
  }
}
width: 640
height: 480
distortion {
  k: 0.15787583319931128
  k: -0.57843036059439024
  k: 0.0013594516180914411
  k: 0.16400433761585667
  k: -0.0087407236800892581
}
intrinsics {
  k: 497.08705074663493
  k: 0
  k: 316.83874731661
  k: 0
  k: 497.60111276688008
  k: 234.2740555042152
  k: 0
  k: 0
  k: 1
}
projection {
  p: 497.08705902099609
  p: 0
  p: 316.83874726295471
  p: 0
  p: 0
  p: 497.60112762451172
  p: 234.27405551075935
  p: 0
  p: 0
  p: 0
  p: 1
  p: 0
}
rectification_matrix: 1
rectification_matrix: 0
rectification_matrix: 0
rectification_matrix: 0
rectification_matrix: 1
rectification_matrix: 0
rectification_matrix: 0
rectification_matrix: 0
rectification_matrix: 1

But when I view the /camera topic, it doesn't appear to have any distortion applied (I tested changing values and also copying values from the gazebo forum but never got any visual distortion)

When I try to do marker detection, I first perform undistortion using the values from camera_matrix.txt and camera_distortion_coefficients.txt files:

camera_distortion_coefficients.txt:

1.578758331993112829e-01,-5.784303605943902360e-01,-8.740723680089258069e-03,1.359451618091441112e-03,1.640043376158566713e-01


camera_matrix.txt:

4.970870507466349295e+02,0.000000000000000000e+00,3.168387473166100108e+02
0.000000000000000000e+00,4.976011127668800782e+02,2.342740555042151982e+02
0.000000000000000000e+00,0.000000000000000000e+00,1.000000000000000000e+00

But now what I get is an image being visually distorted:

This is the code for marker detection that applies undistortion to the image:

class MarkerDetector(Node):
    def __init__(self):
        super().__init__('marker_detector')

        # Subscription and publication setup
        self.camera_image_sub = self.create_subscription(Image, 'camera', self.image_callback, 10)
        self.aruco_image_pub = self.create_publisher(Image, 'aruco_image', 10)
        self.bridge = CvBridge()

        # Camera calibration parameters
        self.camera_matrix = np.loadtxt('camera_parameters/camera_matrix.txt', delimiter=',')
        self.distortion_coeffs = np.loadtxt('camera_parameters/camera_distortion_coefficients.txt', delimiter=',')

    def image_callback(self, msg):
        # Convert ROS Image to CV Image
        cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')

        # Undistort the image
        cv_image = cv.undistort(cv_image, self.camera_matrix, self.distortion_coeffs)

        # Marker detection
        # ...

        # Publish the image with detected markers
        self.aruco_image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, encoding='bgr8'))

def main(args=None):
    rclpy.init(args=args)
    marker_detector = MarkerDetector()
    rclpy.spin(marker_detector)
    marker_detector.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Based on this I can't understand why despite the distortion coefficients appearing on /camera_info topic, it doesn't appear that they are being applied to the image.