[cscore] Use Raw for CvSink and CvSource (wpilibsuite#6364)

Eventually we want to get to a point where we can remove OpenCV from the internals of cscore. The start to doing that is converting the existing CvSource and CvSink methods to RawFrame.

For CvSource, this is 100% a free operation. We can do everything the existing code could have done (with one small exception we can fairly easily fix).

For CvSink, by defaut this change would incur one extra copy, but no extra allocations. A set of direct methods were added to CvSink to add a method to avoid this extra copy.

cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java

@@ -366,6 +366,8 @@ private static String makeSourceValue(int source) {
         }
       case kCv:
         return "cv:";
+      case kRaw:
+        return "raw:";
       default:
         return "unknown:";
     }

cameraserver/src/main/native/cpp/cameraserver/CameraServer.cpp

@@ -113,6 +113,8 @@ static std::string_view MakeSourceValue(CS_Source source,
     }
     case CS_SOURCE_CV:
       return "cv:";
+    case CS_SOURCE_RAW:
+      return "raw:";
     default:
       return "unknown:";
   }

cscore/src/main/java/edu/wpi/first/cscore/CameraServerJNI.java

@@ -212,14 +212,15 @@ public static synchronized void forceLoad() throws IOException {
    * Creates a raw source.
    *
    * @param name Source name.
+   * @param isCv true for a Cv source.
    * @param pixelFormat Pixel format.
    * @param width Image width.
    * @param height Image height.
    * @param fps Source frames per second.
    * @return Raw source handle.
    */
   public static native int createRawSource(
-      String name, int pixelFormat, int width, int height, int fps);
+      String name, boolean isCv, int pixelFormat, int width, int height, int fps);
 
   //
   // Source Functions
@@ -630,9 +631,10 @@ public static native void setSourceEnumPropertyChoices(
    * Creates a raw sink.
    *
    * @param name Sink name.
+   * @param isCv true for a Cv source.
    * @return Raw sink handle.
    */
-  public static native int createRawSink(String name);
+  public static native int createRawSink(String name, boolean isCv);
 
   //
   // Sink Functions

cscore/src/main/java/edu/wpi/first/cscore/CvSink.java

@@ -5,22 +5,62 @@
 package edu.wpi.first.cscore;
 
 import edu.wpi.first.util.PixelFormat;
+import edu.wpi.first.util.RawFrame;
+import java.nio.ByteBuffer;
+import org.opencv.core.CvType;
 import org.opencv.core.Mat;
 
 /**
  * A sink for user code to accept video frames as OpenCV images. These sinks require the WPILib
  * OpenCV builds. For an alternate OpenCV, see the documentation how to build your own with RawSink.
  */
 public class CvSink extends ImageSink {
+  private final RawFrame m_frame = new RawFrame();
+  private Mat m_tmpMat;
+  private ByteBuffer m_origByteBuffer;
+  private int m_width;
+  private int m_height;
+  private PixelFormat m_pixelFormat;
+
+  @Override
+  public void close() {
+    if (m_tmpMat != null) {
+      m_tmpMat.release();
+    }
+    m_frame.close();
+    super.close();
+  }
+
+  private int getCVFormat(PixelFormat pixelFormat) {
+    int type = 0;
+    switch (pixelFormat) {
+      case kYUYV:
+      case kRGB565:
+        type = CvType.CV_8UC2;
+        break;
+      case kBGR:
+        type = CvType.CV_8UC3;
+        break;
+      case kGray:
+      case kMJPEG:
+      default:
+        type = CvType.CV_8UC1;
+        break;
+    }
+    return type;
+  }
+
   /**
-   * Create a sink for accepting OpenCV images. WaitForFrame() must be called on the created sink to
+   * Create a sink for accepting OpenCV images. grabFrame() must be called on the created sink to
    * get each new image.
    *
    * @param name Source name (arbitrary unique identifier)
    * @param pixelFormat Source pixel format
    */
   public CvSink(String name, PixelFormat pixelFormat) {
-    super(CameraServerCvJNI.createCvSink(name, pixelFormat.getValue()));
+    super(CameraServerJNI.createRawSink(name, true));
+    m_pixelFormat = pixelFormat;
+    OpenCvLoader.forceStaticLoad();
   }
 
   /**
@@ -33,22 +73,9 @@ public CvSink(String name) {
     this(name, PixelFormat.kBGR);
   }
 
-  /// Create a sink for accepting OpenCV images in a separate thread.
-  /// A thread will be created that calls WaitForFrame() and calls the
-  /// processFrame() callback each time a new frame arrives.
-  /// @param name Source name (arbitrary unique identifier)
-  /// @param processFrame Frame processing function; will be called with a
-  ///        time=0 if an error occurred.  processFrame should call GetImage()
-  ///        or GetError() as needed, but should not call (except in very
-  ///        unusual circumstances) WaitForImage().
-  // public CvSink(wpi::StringRef name,
-  //       std::function<void(uint64_t time)> processFrame) {
-  //  super(CameraServerJNI.createCvSinkCallback(name, processFrame));
-  // }
-
   /**
    * Wait for the next frame and get the image. Times out (returning 0) after 0.225 seconds. The
-   * provided image will have three 3-bit channels stored in BGR order.
+   * provided image will have the pixelFormat this class was constructed with.
    *
    * @param image Where to store the image.
    * @return Frame time, or 0 on error (call GetError() to obtain the error message)
@@ -59,26 +86,148 @@ public long grabFrame(Mat image) {
 
   /**
    * Wait for the next frame and get the image. Times out (returning 0) after timeout seconds. The
-   * provided image will have three 3-bit channels stored in BGR order.
+   * provided image will have the pixelFormat this class was constructed with.
    *
    * @param image Where to store the image.
    * @param timeout Retrieval timeout in seconds.
    * @return Frame time, or 0 on error (call GetError() to obtain the error message); the frame time
    *     is in 1 us increments.
    */
   public long grabFrame(Mat image, double timeout) {
-    return CameraServerCvJNI.grabSinkFrameTimeout(m_handle, image.nativeObj, timeout);
+    long rv = grabFrameDirect(timeout);
+    if (rv <= 0) {
+      return rv;
+    }
+    m_tmpMat.copyTo(image);
+    return rv;
   }
 
   /**
-   * Wait for the next frame and get the image. May block forever. The provided image will have
-   * three 3-bit channels stored in BGR order.
+   * Wait for the next frame and get the image. May block forever. The provided image will have the
+   * pixelFormat this class was constructed with.
    *
    * @param image Where to store the image.
    * @return Frame time, or 0 on error (call GetError() to obtain the error message); the frame time
    *     is in 1 us increments.
    */
   public long grabFrameNoTimeout(Mat image) {
-    return CameraServerCvJNI.grabSinkFrame(m_handle, image.nativeObj);
+    long rv = grabFrameNoTimeoutDirect();
+    if (rv <= 0) {
+      return rv;
+    }
+    m_tmpMat.copyTo(image);
+    return rv;
+  }
+
+  /**
+   * Get the direct backing mat for this sink.
+   *
+   * <p>This mat can be invalidated any time any of the grab* methods are called, or when the CvSink
+   * is closed.
+   *
+   * @return The backing mat.
+   */
+  public Mat getDirectMat() {
+    return m_tmpMat;
+  }
+
+  /**
+   * Wait for the next frame and store the image. Times out (returning 0) after 0.225 seconds. The
+   * provided image will have the pixelFormat this class was constructed with. Use getDirectMat() to
+   * grab the image.
+   *
+   * @return Frame time, or 0 on error (call GetError() to obtain the error message)
+   */
+  public long grabFrameDirect() {
+    return grabFrameDirect(0.225);
+  }
+
+  /**
+   * Wait for the next frame and store the image. Times out (returning 0) after timeout seconds. The
+   * provided image will have the pixelFormat this class was constructed with. Use getDirectMat() to
+   * grab the image.
+   *
+   * @param timeout Retrieval timeout in seconds.
+   * @return Frame time, or 0 on error (call GetError() to obtain the error message); the frame time
+   *     is in 1 us increments.
+   */
+  @SuppressWarnings("PMD.CompareObjectsWithEquals")
+  public long grabFrameDirect(double timeout) {
+    m_frame.setInfo(0, 0, 0, m_pixelFormat);
+    long rv =
+        CameraServerJNI.grabRawSinkFrameTimeout(m_handle, m_frame, m_frame.getNativeObj(), timeout);
+    if (rv <= 0) {
+      return rv;
+    }
+
+    if (m_frame.getData() != m_origByteBuffer
+        || m_width != m_frame.getWidth()
+        || m_height != m_frame.getHeight()
+        || m_pixelFormat != m_frame.getPixelFormat()) {
+      m_origByteBuffer = m_frame.getData();
+      m_height = m_frame.getHeight();
+      m_width = m_frame.getWidth();
+      m_pixelFormat = m_frame.getPixelFormat();
+      if (m_frame.getStride() == 0) {
+        m_tmpMat =
+            new Mat(
+                m_frame.getHeight(),
+                m_frame.getWidth(),
+                getCVFormat(m_pixelFormat),
+                m_origByteBuffer);
+      } else {
+        m_tmpMat =
+            new Mat(
+                m_frame.getHeight(),
+                m_frame.getWidth(),
+                getCVFormat(m_pixelFormat),
+                m_origByteBuffer,
+                m_frame.getStride());
+      }
+    }
+    return rv;
+  }
+
+  /**
+   * Wait for the next frame and store the image. May block forever. The provided image will have
+   * the pixelFormat this class was constructed with. Use getDirectMat() to grab the image.
+   *
+   * @return Frame time, or 0 on error (call GetError() to obtain the error message); the frame time
+   *     is in 1 us increments.
+   */
+  @SuppressWarnings("PMD.CompareObjectsWithEquals")
+  public long grabFrameNoTimeoutDirect() {
+    m_frame.setInfo(0, 0, 0, m_pixelFormat);
+    long rv = CameraServerJNI.grabRawSinkFrame(m_handle, m_frame, m_frame.getNativeObj());
+    if (rv <= 0) {
+      return rv;
+    }
+
+    if (m_frame.getData() != m_origByteBuffer
+        || m_width != m_frame.getWidth()
+        || m_height != m_frame.getHeight()
+        || m_pixelFormat != m_frame.getPixelFormat()) {
+      m_origByteBuffer = m_frame.getData();
+      m_height = m_frame.getHeight();
+      m_width = m_frame.getWidth();
+      m_pixelFormat = m_frame.getPixelFormat();
+      if (m_frame.getStride() == 0) {
+        m_tmpMat =
+            new Mat(
+                m_frame.getHeight(),
+                m_frame.getWidth(),
+                getCVFormat(m_pixelFormat),
+                m_origByteBuffer);
+      } else {
+        m_tmpMat =
+            new Mat(
+                m_frame.getHeight(),
+                m_frame.getWidth(),
+                getCVFormat(m_pixelFormat),
+                m_origByteBuffer,
+                m_frame.getStride());
+      }
+    }
+    return rv;
   }
 }

cscore/src/main/java/edu/wpi/first/cscore/CvSource.java

@@ -20,8 +20,9 @@ public class CvSource extends ImageSource {
    */
   public CvSource(String name, VideoMode mode) {
     super(
-        CameraServerCvJNI.createCvSource(
-            name, mode.pixelFormat.getValue(), mode.width, mode.height, mode.fps));
+        CameraServerJNI.createRawSource(
+            name, true, mode.pixelFormat.getValue(), mode.width, mode.height, mode.fps));
+    OpenCvLoader.forceStaticLoad();
   }
 
   /**
@@ -34,7 +35,8 @@ public CvSource(String name, VideoMode mode) {
    * @param fps fps
    */
   public CvSource(String name, PixelFormat pixelFormat, int width, int height, int fps) {
-    super(CameraServerCvJNI.createCvSource(name, pixelFormat.getValue(), width, height, fps));
+    super(CameraServerJNI.createRawSource(name, true, pixelFormat.getValue(), width, height, fps));
+    OpenCvLoader.forceStaticLoad();
   }
 
   /**
@@ -47,6 +49,43 @@ public CvSource(String name, PixelFormat pixelFormat, int width, int height, int
    * @param image OpenCV image
    */
   public void putFrame(Mat image) {
-    CameraServerCvJNI.putSourceFrame(m_handle, image.nativeObj);
+    // We only support 8-bit images, convert if necessary
+    boolean cleanupRequired = false;
+    Mat finalImage;
+    if (image.depth() == 0) {
+      finalImage = image;
+    } else {
+      finalImage = new Mat();
+      image.convertTo(finalImage, 0);
+      cleanupRequired = true;
+    }
+
+    try {
+      int channels = finalImage.channels();
+      PixelFormat format;
+      if (channels == 1) {
+        format = PixelFormat.kGray;
+      } else if (channels == 3) {
+        format = PixelFormat.kBGR;
+      } else {
+        throw new VideoException("Unsupported image type");
+      }
+      // TODO old code supported BGRA, but the only way I can support that is slow.
+      // Update cscore to support BGRA for raw frames
+
+      CameraServerJNI.putRawSourceFrameData(
+          m_handle,
+          finalImage.dataAddr(),
+          (int) finalImage.total() * channels,
+          finalImage.width(),
+          finalImage.height(),
+          image.width(),
+          format.getValue());
+
+    } finally {
+      if (cleanupRequired) {
+        finalImage.release();
+      }
+    }
   }
 }