Add nvCOMP & Use Hybrid Depth Compression

.gitattributes

@@ -12,3 +12,4 @@
 *.dylib filter=lfs diff=lfs merge=lfs -text
 *.glb filter=lfs diff=lfs merge=lfs -text
 *.bin filter=lfs diff=lfs merge=lfs -text
+*.raw filter=lfs diff=lfs merge=lfs -text

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2ImageMessageVisualizer.java

@@ -10,6 +10,7 @@
 import us.ihmc.communication.ROS2Tools;
 import us.ihmc.log.LogTools;
 import us.ihmc.perception.comms.ImageMessageFormat;
+import us.ihmc.perception.cuda.CUDACompressionTools;
 import us.ihmc.perception.opencv.OpenCVTools;
 import us.ihmc.perception.tools.NativeMemoryTools;
 import us.ihmc.pubsub.DomainFactory.PubSubImplementation;
@@ -33,6 +34,7 @@ public class RDXROS2ImageMessageVisualizer extends RDXROS2OpenCVVideoVisualizer<
    private final ImageMessage imageMessage = new ImageMessage();
    private final SampleInfo sampleInfo = new SampleInfo();
    private final Object syncObject = new Object();
+   private CUDACompressionTools compressionTools;
    private int imageWidth;
    private int imageHeight;
    private int numberOfPixels;
@@ -105,46 +107,40 @@ private void queueRenderImage(Subscriber<ImageMessage> subscriber)
 
             if (incomingCompressedImageBuffer == null)
             {
-               switch (ImageMessageFormat.getFormat(imageMessage))
+               ImageMessageFormat imageMessageFormat = ImageMessageFormat.getFormat(imageMessage);
+               LogTools.info("Creating Image Message Visualizer for {} with type {}", topic.getName(), imageMessageFormat.name());
+
+               bytesIfUncompressed = imageMessageFormat.getBytesPerPixel() * numberOfPixels;
+               incomingCompressedImageBuffer = NativeMemoryTools.allocate(bytesIfUncompressed);
+               incomingCompressedImageBytePointer = new BytePointer(incomingCompressedImageBuffer);
+
+               switch (imageMessageFormat)
                {
                   case GRAY_PNG_8UC1 ->
                   {
-                     LogTools.info("Creating Image Message Visualizer for {} with type PNG_8UC1", topic.getName());
-                     bytesIfUncompressed = numberOfPixels;
-                     incomingCompressedImageBuffer = NativeMemoryTools.allocate(bytesIfUncompressed);
-                     incomingCompressedImageBytePointer = new BytePointer(incomingCompressedImageBuffer);
-
                      compressedBytesMat = new Mat(1, 1, opencv_core.CV_8UC1);
                      decompressedImage = new Mat(imageHeight, imageWidth, opencv_core.CV_8UC1);
                   }
                   case DEPTH_PNG_16UC1 ->
                   {
-                     LogTools.info("Creating Image Message Visualizer for {} with type DEPTH_PNG_16UC1", topic.getName());
-                     bytesIfUncompressed = numberOfPixels * 2;
-                     incomingCompressedImageBuffer = NativeMemoryTools.allocate(bytesIfUncompressed);
-                     incomingCompressedImageBytePointer = new BytePointer(incomingCompressedImageBuffer);
-
                      compressedBytesMat = new Mat(1, 1, opencv_core.CV_8UC1);
                      decompressedImage = new Mat(imageHeight, imageWidth, opencv_core.CV_16UC1);
                      normalizedScaledImage = new Mat(imageHeight, imageWidth, opencv_core.CV_32FC1);
                   }
-                  case COLOR_JPEG_YUVI420 ->
+                  case DEPTH_HYBRID_ZSTD_JPEG_16UC1 ->
                   {
-                     LogTools.info("Creating Image Message Visualizer for {} with type COLOR_JPEG_YUVI420", topic.getName());
-                     bytesIfUncompressed = numberOfPixels * 3;
-                     incomingCompressedImageBuffer = NativeMemoryTools.allocate(bytesIfUncompressed);
-                     incomingCompressedImageBytePointer = new BytePointer(incomingCompressedImageBuffer);
+                     decompressedImage = new Mat(imageHeight, imageWidth, opencv_core.CV_16UC1);
+                     normalizedScaledImage = new Mat(imageHeight, imageWidth, opencv_core.CV_32FC1);
 
+                     compressionTools = new CUDACompressionTools();
+                  }
+                  case COLOR_JPEG_YUVI420 ->
+                  {
                      compressedBytesMat = new Mat(1, 1, opencv_core.CV_8UC1);
                      decompressedImage = new Mat(imageHeight, imageWidth, opencv_core.CV_8UC3);
                   }
                   case COLOR_JPEG_BGR8 ->
                   {
-                     LogTools.info("Creating Image Message Visualizer for {} with the type COLOR_JPEG_BGR8", topic.getName());
-                     bytesIfUncompressed = numberOfPixels * 3;
-                     incomingCompressedImageBuffer = NativeMemoryTools.allocate(bytesIfUncompressed);
-                     incomingCompressedImageBytePointer = new BytePointer(incomingCompressedImageBuffer);
-
                      compressedBytesMat = new Mat(1, 1, opencv_core.CV_8UC1);
                      decompressedImage = new Mat(imageHeight, imageWidth, opencv_core.CV_8UC3);
                   }
@@ -164,9 +160,16 @@ private void queueRenderImage(Subscriber<ImageMessage> subscriber)
             sequenceDiscontinuityPlot.update(imageMessage.getSequenceNumber());
          }
 
-         compressedBytesMat.cols(numberOfBytes);
-         compressedBytesMat.data(incomingCompressedImageBytePointer);
-         opencv_imgcodecs.imdecode(compressedBytesMat, opencv_imgcodecs.IMREAD_UNCHANGED, decompressedImage);
+         if (ImageMessageFormat.getFormat(imageMessage) == ImageMessageFormat.DEPTH_HYBRID_ZSTD_JPEG_16UC1)
+         {
+            compressionTools.decompressDepth(incomingCompressedImageBytePointer, decompressedImage);
+         }
+         else
+         {
+            compressedBytesMat.cols(numberOfBytes);
+            compressedBytesMat.data(incomingCompressedImageBytePointer);
+            opencv_imgcodecs.imdecode(compressedBytesMat, opencv_imgcodecs.IMREAD_UNCHANGED, decompressedImage);
+         }
 
          synchronized (this) // synchronize with the update method
          {
@@ -178,7 +181,7 @@ private void queueRenderImage(Subscriber<ImageMessage> subscriber)
                {
                   OpenCVTools.convertGrayToRGBA(decompressedImage, getOpenCVVideoVisualizer().getRGBA8Mat());
                }
-               case DEPTH_PNG_16UC1 ->
+               case DEPTH_PNG_16UC1, DEPTH_HYBRID_ZSTD_JPEG_16UC1 ->
                {
                   OpenCVTools.clampTo8BitUnsignedChar(decompressedImage, normalizedScaledImage, 0.0, 255.0);
                   OpenCVTools.convertGrayToRGBA(normalizedScaledImage, getOpenCVVideoVisualizer().getRGBA8Mat());
@@ -222,6 +225,8 @@ public void destroy()
       unsubscribe();
       super.destroy();
       getOpenCVVideoVisualizer().destroy();
+      if (compressionTools != null)
+         compressionTools.destroy();
    }
 
    public void renderStatistics()

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXROS2ColoredPointCloudVisualizerDepthChannel.java

@@ -5,6 +5,8 @@
 import org.bytedeco.opencv.global.opencv_imgcodecs;
 import perception_msgs.msg.dds.ImageMessage;
 import us.ihmc.perception.BytedecoImage;
+import us.ihmc.perception.comms.ImageMessageFormat;
+import us.ihmc.perception.cuda.CUDACompressionTools;
 import us.ihmc.perception.opencl.OpenCLManager;
 import us.ihmc.ros2.ROS2Topic;
 import us.ihmc.tools.thread.SwapReference;
@@ -14,6 +16,7 @@
  */
 public class RDXROS2ColoredPointCloudVisualizerDepthChannel extends RDXROS2ColoredPointCloudVisualizerChannel
 {
+   private CUDACompressionTools compressionTools;
    private SwapReference<BytedecoImage> depth16UC1ImageSwapReference;
 
    public RDXROS2ColoredPointCloudVisualizerDepthChannel(ROS2Topic<ImageMessage> topic)
@@ -31,16 +34,27 @@ protected void initialize(OpenCLManager openCLManager)
          getFrequencyText().ping();
          return depth16UC1Image;
       });
+
+      if (ImageMessageFormat.getFormat(imageMessage) == ImageMessageFormat.DEPTH_HYBRID_ZSTD_JPEG_16UC1)
+         compressionTools = new CUDACompressionTools();
    }
 
    @Override
    protected void decompress()
    {
       synchronized (decompressionInputSwapReference)
       {
-         opencv_imgcodecs.imdecode(decompressionInputSwapReference.getForThreadTwo().getInputMat(),
-                                   opencv_imgcodecs.IMREAD_UNCHANGED,
-                                   depth16UC1ImageSwapReference.getForThreadOne().getBytedecoOpenCVMat());
+         if (ImageMessageFormat.getFormat(imageMessage) == ImageMessageFormat.DEPTH_HYBRID_ZSTD_JPEG_16UC1)
+         {
+            compressionTools.decompressDepth(decompressionInputSwapReference.getForThreadTwo().getInputPointer(),
+                                             depth16UC1ImageSwapReference.getForThreadOne().getBytedecoOpenCVMat());
+         }
+         else
+         {
+            opencv_imgcodecs.imdecode(decompressionInputSwapReference.getForThreadTwo().getInputMat(),
+                                      opencv_imgcodecs.IMREAD_UNCHANGED,
+                                      depth16UC1ImageSwapReference.getForThreadOne().getBytedecoOpenCVMat());
+         }
       }
       depth16UC1ImageSwapReference.swap();
    }
@@ -51,6 +65,14 @@ protected Object getDecompressionAccessSyncObject()
       return depth16UC1ImageSwapReference;
    }
 
+   @Override
+   public void destroy()
+   {
+      super.destroy();
+      if (compressionTools != null)
+         compressionTools.destroy();
+   }
+
    public BytedecoImage getDepth16UC1Image()
    {
       return depth16UC1ImageSwapReference.getForThreadTwo();

ihmc-perception/src/main/java/us/ihmc/perception/comms/ImageMessageFormat.java

@@ -8,6 +8,7 @@ public enum ImageMessageFormat
    COLOR_JPEG_BGR8(3),
    COLOR_PNG_RGB8(3), // TODO: Implement receiver and visualizer
    DEPTH_PNG_16UC1(2),
+   DEPTH_HYBRID_ZSTD_JPEG_16UC1(2),
    GRAY_PNG_8UC1(1)
    ;