Clean code for Image display (#1271)

Signed-off-by: Peng Wang <[email protected]>
ros2 · Oct 15, 2024 · f7112b1 · f7112b1
1 parent 2a34490
commit f7112b1
Showing 1 changed file with 31 additions and 60 deletions.
diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/image/ros_image_texture.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/image/ros_image_texture.cpp
@@ -39,6 +39,7 @@
 #include <sstream>
 #include <iostream>
 #include <string>
+#include <tuple>
 #include <vector>
 #include <utility>
 
@@ -190,6 +191,32 @@ bool ROSImageTexture::update()
   return true;
 }
 
+// Considering frequent execution, use inline
+static inline std::tuple<uint8_t, uint8_t, uint8_t> pixelYUVToRGB(int y, int u, int v)
+{
+  int r = 0;
+  int b = 0;
+  int g = 0;
+
+  // Values generated based on this formula
+  // for converting YUV to RGB
+  // R = Y + 1.403V'
+  // G = Y + 0.344U' - 0.714V'
+  // B = Y + 1.770U'
+
+  v -= 128;
+  u -= 128;
+
+  r = y + (1403 * v) / 1000;
+  g = y + (344 * u - 714 * v) / 1000;
+  b = y + (1770 * u) / 1000;
+
+  // pixel value must fit in a uint8_t
+  return {
+    std::clamp(r, 0, 255), std::clamp(g, 0, 255), std::clamp(b, 0, 255)
+  };
+}
+
 struct yuyv
 {
   uint8_t y0;
@@ -216,12 +243,6 @@ static void imageConvertUYVYToRGB(
   int final_u = 0;
   int final_y1 = 0;
   int final_v = 0;
-  int r1 = 0;
-  int b1 = 0;
-  int g1 = 0;
-  int r2 = 0;
-  int b2 = 0;
-  int g2 = 0;
 
   uint32_t stride_in_pixels = stride_in_bytes / 4;
 
@@ -237,30 +258,8 @@ static void imageConvertUYVYToRGB(
       final_y1 = pixel->y1;
       final_v = pixel->v;
 
-      // Values generated based on this formula
-      // for converting YUV to RGB
-      // R = Y + 1.403V'
-      // G = Y + 0.344U' - 0.714V'
-      // B = Y + 1.770U'
-
-      final_v -= 128;
-      final_u -= 128;
-
-      r1 = final_y0 + (1403 * final_v) / 1000;
-      g1 = final_y0 + (344 * final_u - 714 * final_v) / 1000;
-      b1 = final_y0 + (1770 * final_u) / 1000;
-
-      r2 = final_y1 + (1403 * final_v) / 1000;
-      g2 = final_y1 + (344 * final_u - 714 * final_v) / 1000;
-      b2 = final_y1 + (1770 * final_u) / 1000;
-
-      // pixel value must fit in a uint8_t
-      dst_img[0] = ((r1 & 0xFFFFFF00) == 0) ? r1 : (r1 < 0) ? 0 : 0xFF;
-      dst_img[1] = ((g1 & 0xFFFFFF00) == 0) ? g1 : (g1 < 0) ? 0 : 0xFF;
-      dst_img[2] = ((b1 & 0xFFFFFF00) == 0) ? b1 : (b1 < 0) ? 0 : 0xFF;
-      dst_img[3] = ((r2 & 0xFFFFFF00) == 0) ? r2 : (r2 < 0) ? 0 : 0xFF;
-      dst_img[4] = ((g2 & 0xFFFFFF00) == 0) ? g2 : (g2 < 0) ? 0 : 0xFF;
-      dst_img[5] = ((b2 & 0xFFFFFF00) == 0) ? b2 : (b2 < 0) ? 0 : 0xFF;
+      std::tie(dst_img[0], dst_img[1], dst_img[2]) = pixelYUVToRGB(final_y0, final_u, final_v);
+      std::tie(dst_img[3], dst_img[4], dst_img[5]) = pixelYUVToRGB(final_y1, final_u, final_v);
       dst_img += 6;
     }
   }
@@ -276,12 +275,6 @@ static void imageConvertYUYVToRGB(
   int final_u = 0;
   int final_y1 = 0;
   int final_v = 0;
-  int r1 = 0;
-  int b1 = 0;
-  int g1 = 0;
-  int r2 = 0;
-  int b2 = 0;
-  int g2 = 0;
 
   uint32_t stride_in_pixels = stride_in_bytes / 4;
 
@@ -297,30 +290,8 @@ static void imageConvertYUYVToRGB(
       final_y1 = pixel->y1;
       final_v = pixel->v;
 
-      // Values generated based on this formula
-      // for converting YUV to RGB
-      // R = Y + 1.403V'
-      // G = Y + 0.344U' - 0.714V'
-      // B = Y + 1.770U'
-
-      final_v -= 128;
-      final_u -= 128;
-
-      r1 = final_y0 + (1403 * final_v) / 1000;
-      g1 = final_y0 + (344 * final_u - 714 * final_v) / 1000;
-      b1 = final_y0 + (1770 * final_u) / 1000;
-
-      r2 = final_y1 + (1403 * final_v) / 1000;
-      g2 = final_y1 + (344 * final_u - 714 * final_v) / 1000;
-      b2 = final_y1 + (1770 * final_u) / 1000;
-
-      // pixel value must fit in a uint8_t
-      dst_img[0] = ((r1 & 0xFFFFFF00) == 0) ? r1 : (r1 < 0) ? 0 : 0xFF;
-      dst_img[1] = ((g1 & 0xFFFFFF00) == 0) ? g1 : (g1 < 0) ? 0 : 0xFF;
-      dst_img[2] = ((b1 & 0xFFFFFF00) == 0) ? b1 : (b1 < 0) ? 0 : 0xFF;
-      dst_img[3] = ((r2 & 0xFFFFFF00) == 0) ? r2 : (r2 < 0) ? 0 : 0xFF;
-      dst_img[4] = ((g2 & 0xFFFFFF00) == 0) ? g2 : (g2 < 0) ? 0 : 0xFF;
-      dst_img[5] = ((b2 & 0xFFFFFF00) == 0) ? b2 : (b2 < 0) ? 0 : 0xFF;
+      std::tie(dst_img[0], dst_img[1], dst_img[2]) = pixelYUVToRGB(final_y0, final_u, final_v);
+      std::tie(dst_img[3], dst_img[4], dst_img[5]) = pixelYUVToRGB(final_y1, final_u, final_v);
       dst_img += 6;
     }
   }