Enhance NV12 and NV21 Support in sensor_msgs::image_encodings

sensor_msgs/include/sensor_msgs/image_encodings.hpp

@@ -150,36 +150,41 @@ static inline bool hasAlpha(const std::string & encoding)
          encoding == RGBA16 || encoding == BGRA16;
 }
 
+static inline bool isPlanar(const std::string & encoding)
+{
+  return encoding == NV12 || encoding == NV21 || encoding == NV24;
+}
+
 static inline int numChannels(const std::string & encoding)
 {
   // First do the common-case encodings
   if (encoding == MONO8 ||
-    encoding == MONO16)
+      encoding == MONO16)
   {
     return 1;
   }
   if (encoding == BGR8 ||
-    encoding == RGB8 ||
-    encoding == BGR16 ||
-    encoding == RGB16)
+      encoding == RGB8 ||
+      encoding == BGR16 ||
+      encoding == RGB16)
   {
     return 3;
   }
   if (encoding == BGRA8 ||
-    encoding == RGBA8 ||
-    encoding == BGRA16 ||
-    encoding == RGBA16)
+      encoding == RGBA8 ||
+      encoding == BGRA16 ||
+      encoding == RGBA16)
   {
     return 4;
   }
   if (encoding == BAYER_RGGB8 ||
-    encoding == BAYER_BGGR8 ||
-    encoding == BAYER_GBRG8 ||
-    encoding == BAYER_GRBG8 ||
-    encoding == BAYER_RGGB16 ||
-    encoding == BAYER_BGGR16 ||
-    encoding == BAYER_GBRG16 ||
-    encoding == BAYER_GRBG16)
+      encoding == BAYER_BGGR8 ||
+      encoding == BAYER_GBRG8 ||
+      encoding == BAYER_GRBG8 ||
+      encoding == BAYER_RGGB16 ||
+      encoding == BAYER_BGGR16 ||
+      encoding == BAYER_GBRG16 ||
+      encoding == BAYER_GRBG16)
   {
     return 1;
   }
@@ -191,13 +196,16 @@ static inline int numChannels(const std::string & encoding)
     return (m[3] == "") ? 1 : std::atoi(m[3].str().c_str());
   }
 
+  if (encoding == NV12 ||
+      encoding == NV21)
+  {
+    return 1;
+  }
   if (encoding == YUV422 ||
-    encoding == YUV422_YUY2 ||
-    encoding == UYVY ||
-    encoding == YUYV ||
-    encoding == NV12 ||
-    encoding == NV21 ||
-    encoding == NV24)
+      encoding == YUV422_YUY2 ||
+      encoding == UYVY ||
+      encoding == YUYV ||
+      encoding == NV24)
   {
     return 2;
   }
@@ -212,27 +220,27 @@ static inline int bitDepth(const std::string & encoding)
     return 16;
   }
   if (encoding == MONO8 ||
-    encoding == BGR8 ||
-    encoding == RGB8 ||
-    encoding == BGRA8 ||
-    encoding == RGBA8 ||
-    encoding == BAYER_RGGB8 ||
-    encoding == BAYER_BGGR8 ||
-    encoding == BAYER_GBRG8 ||
-    encoding == BAYER_GRBG8)
+      encoding == BGR8 ||
+      encoding == RGB8 ||
+      encoding == BGRA8 ||
+      encoding == RGBA8 ||
+      encoding == BAYER_RGGB8 ||
+      encoding == BAYER_BGGR8 ||
+      encoding == BAYER_GBRG8 ||
+      encoding == BAYER_GRBG8)
   {
     return 8;
   }
 
   if (encoding == MONO16 ||
-    encoding == BGR16 ||
-    encoding == RGB16 ||
-    encoding == BGRA16 ||
-    encoding == RGBA16 ||
-    encoding == BAYER_RGGB16 ||
-    encoding == BAYER_BGGR16 ||
-    encoding == BAYER_GBRG16 ||
-    encoding == BAYER_GRBG16)
+      encoding == BGR16 ||
+      encoding == RGB16 ||
+      encoding == BGRA16 ||
+      encoding == RGBA16 ||
+      encoding == BAYER_RGGB16 ||
+      encoding == BAYER_BGGR16 ||
+      encoding == BAYER_GBRG16 ||
+      encoding == BAYER_GRBG16)
   {
     return 16;
   }
@@ -241,23 +249,34 @@ static inline int bitDepth(const std::string & encoding)
   std::cmatch m;
   // ex. 8UC -> 8, 8UC10 -> 10
   if (std::regex_match(encoding.c_str(), m, cv_type_regex)) {
-    return std::atoi(m[0].str().c_str());
+    return std::atoi(m[1].str().c_str());
   }
 
   if (encoding == YUV422 ||
-    encoding == YUV422_YUY2 ||
-    encoding == UYVY ||
-    encoding == YUYV ||
-    encoding == NV12 ||
-    encoding == NV21 ||
-    encoding == NV24)
+      encoding == YUV422_YUY2 ||
+      encoding == UYVY ||
+      encoding == YUYV ||
+      encoding == NV12 ||
+      encoding == NV21 ||
+      encoding == NV24)
   {
     return 8;
   }
 
   throw std::runtime_error("Unknown encoding " + encoding);
   return -1;
 }
+
+static inline float getHeightScaling(const std::string & encoding)
+{
+  if (isPlanar(encoding)) {
+    if (encoding == NV12 ||
+        encoding == NV21)
+      return 1.5f;
+  }
+  return 1.0f;
+}
+
 }  // namespace image_encodings
 }  // namespace sensor_msgs