Feature/cuda program rapid height map extractor

ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java

@@ -16,6 +16,7 @@
 import us.ihmc.euclid.transform.RigidBodyTransform;
 import us.ihmc.perception.camera.CameraIntrinsics;
 import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor;
+import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractorCuda;
 import us.ihmc.perception.heightMap.TerrainMapData;
 import us.ihmc.perception.opencl.OpenCLManager;
 import us.ihmc.perception.opencv.OpenCVTools;
@@ -29,7 +30,7 @@
  */
 public class RapidHeightMapManager
 {
-   private final RapidHeightMapExtractor rapidHeightMapExtractor;
+   private final RapidHeightMapExtractorCuda rapidHeightMapExtractor;
    private final ImageMessage croppedHeightMapImageMessage = new ImageMessage();
    private final FramePose3D cameraPoseForHeightMap = new FramePose3D();
    private final RigidBodyTransform sensorToWorldForHeightMap = new RigidBodyTransform();
@@ -46,7 +47,8 @@ public RapidHeightMapManager(OpenCLManager openCLManager,
                                 CameraIntrinsics depthImageIntrinsics,
                                 ROS2PublishSubscribeAPI ros2)
    {
-      rapidHeightMapExtractor = new RapidHeightMapExtractor(openCLManager, leftFootSoleFrame, rightFootSoleFrame);
+
+      rapidHeightMapExtractor = new RapidHeightMapExtractorCuda(leftFootSoleFrame, rightFootSoleFrame);
       rapidHeightMapExtractor.setDepthIntrinsics(depthImageIntrinsics);
 
       heightMapBytedecoImage = new BytedecoImage(depthImageIntrinsics.getWidth(), depthImageIntrinsics.getHeight(), opencv_core.CV_16UC1);
@@ -111,7 +113,7 @@ public HeightMapData getLatestHeightMapData()
                                              (float) RapidHeightMapExtractor.getHeightMapParameters().getGlobalWidthInMeters(),
                                              rapidHeightMapExtractor.getSensorOrigin().getX(),
                                              rapidHeightMapExtractor.getSensorOrigin().getY());
-      RapidHeightMapExtractor.packHeightMapData(rapidHeightMapExtractor, temp);
+//      RapidHeightMapExtractor.packHeightMapData(rapidHeightMapExtractor, temp);
       return temp;
    }
 

ihmc-perception/src/main/java/us/ihmc/perception/cuda/CUDAKernelHandle.java

@@ -0,0 +1,52 @@
+package us.ihmc.perception.cuda;
+
+import org.bytedeco.cuda.cudart.dim3;
+
+/**
+ * This class contains the information that the {@link CUDAProgram} needs to successfully run the loaded CUDA kernel. This consists of the name of the kernel,
+ * as well as the grid size and block size. These are then retrieved by the program when running using {@link CUDAKernelHandle#getName()},
+ * {@link CUDAKernelHandle#getBlockSize()}, and {@link CUDAKernelHandle#getGridSize()} .
+ *
+ * <p>
+ * By default, the grid size and block size are loaded as zero dimensional {@link dim3} objects. This can be overriden (or set) by calling
+ * {@link CUDAKernelHandle#setBlockSize(dim3)} and {@link CUDAKernelHandle#setGridSize(dim3)}.
+ * </p>
+ */
+public class CUDAKernelHandle
+{
+   private final String name;
+
+   // TODO figure out how to load these from the inputs when the kernel is loaded.
+   private dim3 gridSize = new dim3();
+   private dim3 blockSize = new dim3();
+
+   public CUDAKernelHandle(String name)
+   {
+      this.name = name;
+   }
+
+   public String getName()
+   {
+      return name;
+   }
+
+   public void setGridSize(dim3 gridSize)
+   {
+      this.gridSize = gridSize;
+   }
+
+   public void setBlockSize(dim3 blockSize)
+   {
+      this.blockSize = blockSize;
+   }
+
+   public dim3 getGridSize()
+   {
+      return gridSize;
+   }
+
+   public dim3 getBlockSize()
+   {
+      return blockSize;
+   }
+}

ihmc-perception/src/main/java/us/ihmc/perception/cuda/CUDAProgram.java

@@ -24,7 +24,7 @@
 import static us.ihmc.perception.cuda.CUDATools.checkCUDAError;
 import static us.ihmc.perception.cuda.CUDATools.checkNVRTCError;
 
-class CUDAProgram
+public class CUDAProgram
 {
    private static final String[] DEFAULT_OPTIONS = {"-arch=" + getComputeVersion(),  // Target fairly recent GPU architecture
                                                     "--dopt=on",                     // Optimize code
@@ -37,6 +37,7 @@ class CUDAProgram
 
    /**
     * Construct a {@link CUDAProgram} with default compilation options
+    *
     * @param programPath {@link Path} to the .cu file.
     * @param headerPaths {@link Path}s to the header files included (with {@code #include}) in the .cu file.
     */
@@ -47,10 +48,12 @@ public CUDAProgram(Path programPath, Path... headerPaths)
 
    /**
     * Construct a {@link CUDAProgram} specifying the path to the .cu file, paths to the header files, and compilation options.
-    * @param programPath {@link Path} to the .cu file.
-    * @param headerPaths {@link Path}s to the header files included (with {@code #include}) in the .cu file.
+    *
+    * @param programPath        {@link Path} to the .cu file.
+    * @param headerPaths        {@link Path}s to the header files included (with {@code #include}) in the .cu file.
     * @param compilationOptions List of compilation options
-    *                           (You can see the available options <a href="https://docs.nvidia.com/cuda/nvrtc/index.html#supported-compile-options">here</a>)
+    *                           (You can see the available options <a
+    *                           href="https://docs.nvidia.com/cuda/nvrtc/index.html#supported-compile-options">here</a>)
     */
    public CUDAProgram(Path programPath, Path[] headerPaths, String... compilationOptions)
    {
@@ -89,9 +92,10 @@ public CUDAProgram(String programName, String programCode)
 
    /**
     * Construct a {@link CUDAProgram} with default compilation options.
-    * @param programName The name of the program (usually the file name with a .cu extension).
-    * @param programCode The program code (i.e. the contents of the .cu file).
-    * @param headerNames List of header names included (with {@code #include}) in the code.
+    *
+    * @param programName    The name of the program (usually the file name with a .cu extension).
+    * @param programCode    The program code (i.e. the contents of the .cu file).
+    * @param headerNames    List of header names included (with {@code #include}) in the code.
     * @param headerContents Contents of the headers included in the code.
     */
    public CUDAProgram(String programName, String programCode, String[] headerNames, String[] headerContents)
@@ -101,25 +105,37 @@ public CUDAProgram(String programName, String programCode, String[] headerNames,
 
    /**
     * Construct a {@link CUDAProgram} specifying the name, code, header names, header contents, and compilation options.
-    * @param programName The name of the program (usually the file name with a .cu extension).
-    * @param programCode The program code (i.e. the contents of the .cu file).
-    * @param headerNames List of header names included (with {@code #include}) in the code.
-    * @param headerContents Contents of the headers included in the code.
+    *
+    * @param programName        The name of the program (usually the file name with a .cu extension).
+    * @param programCode        The program code (i.e. the contents of the .cu file).
+    * @param headerNames        List of header names included (with {@code #include}) in the code.
+    * @param headerContents     Contents of the headers included in the code.
     * @param compilationOptions List of compilation options
-    *                           (You can see the available options <a href="https://docs.nvidia.com/cuda/nvrtc/index.html#supported-compile-options">here</a>)
+    *                           (You can see the available options <a
+    *                           href="https://docs.nvidia.com/cuda/nvrtc/index.html#supported-compile-options">here</a>)
     */
    public CUDAProgram(String programName, String programCode, String[] headerNames, String[] headerContents, String... compilationOptions)
    {
       initialize(programName, programCode, headerNames, headerContents, compilationOptions);
    }
 
-   public void loadKernel(String kernelName)
+   public CUDAKernelHandle loadKernel(String kernelName)
    {
       // Load the kernel from module
       CUfunc_st kernel = new CUfunc_st();
       error = cuModuleGetFunction(kernel, module, kernelName);
       checkCUDAError(error);
       kernels.put(kernelName, kernel);
+
+      CUDAKernelHandle kernelHandle = new CUDAKernelHandle(kernelName);
+      // TODO figure out how to load the grid size and block size from the kernel definition when the kernel is loaded. Then set them here.
+
+      return kernelHandle;
+   }
+
+   public void runKernel(CUstream_st stream, CUDAKernelHandle kernelHandle, int sharedMemorySize, Pointer... arguments)
+   {
+      runKernel(stream, kernelHandle.getName(), kernelHandle.getGridSize(), kernelHandle.getBlockSize(), sharedMemorySize, arguments);
    }
 
    public void runKernel(CUstream_st stream, String kernelName, dim3 gridSize, dim3 blockSize, int sharedMemorySize, Pointer... arguments)
@@ -161,7 +177,7 @@ private void initialize(String programName, String programCode, String[] headerN
 
       // Load the module using the PTX
       error = cuModuleLoadData(module, ptx);
-      checkCUDAError(error);
+      checkNVRTCError(error);
 
       // Release stuff
       nvrtcDestroyProgram(compiledProgram);

ihmc-perception/src/main/java/us/ihmc/perception/cuda/CudaHeightMapTools.java

@@ -0,0 +1,45 @@
+package us.ihmc.perception.cuda;
+
+import gnu.trove.list.array.TFloatArrayList;
+import org.bytedeco.hdf5.H5FD_file_image_callbacks_t.Image_malloc_long_int_Pointer;
+import org.bytedeco.javacpp.PointerPointer;
+import us.ihmc.perception.BytedecoImage;
+
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Paths;
+import java.util.Arrays;
+
+public class CudaHeightMapTools
+{
+   public static void TransformToPointer(float[] transform, PointerPointer transformPointer)
+   {
+      // TODO make it an array pointer
+
+   }
+
+   public static void BytedecoImageToPointer(BytedecoImage bdimage, Image_malloc_long_int_Pointer imagePointer)
+   {
+      //TODO MAKE A BYTEDECO IMAGE A PONTER
+   }
+
+   public static void CUFileAsString(String[] filePath)
+   {
+      try
+      {
+         // Read the file content into a byte array
+         byte[] bytes = Files.readAllBytes(Paths.get(Arrays.toString(filePath)));
+
+         // Convert the byte array to a string
+         String content = new String(bytes);
+
+         // Print the content
+         System.out.println(content);
+      }
+      catch (IOException e)
+      {
+         e.printStackTrace();
+      }
+   }
+}
+

ihmc-perception/src/main/java/us/ihmc/perception/cuda/dataTypes/CUDAInteger.java

@@ -0,0 +1,9 @@
+package us.ihmc.perception.cuda.dataTypes;
+
+public class CUDAInteger extends CUDAIntegerArray
+{
+   public CUDAInteger()
+   {
+      super(1L);
+   }
+}

ihmc-perception/src/main/java/us/ihmc/perception/cuda/dataTypes/CUDAIntegerArray.java

@@ -0,0 +1,34 @@
+package us.ihmc.perception.cuda.dataTypes;
+
+import org.bytedeco.javacpp.IntPointer;
+import org.bytedeco.javacpp.PointerPointer;
+
+public class CUDAIntegerArray extends PointerPointer<IntPointer>
+{
+   private final IntPointer javaSideValueContainer;
+
+   public CUDAIntegerArray(long arraySize)
+   {
+       super(arraySize);
+
+       javaSideValueContainer = new IntPointer(arraySize);
+       this.put(javaSideValueContainer);
+   }
+
+   public int getFirst()
+   {
+      return javaSideValueContainer.get();
+   }
+
+   public int get(int element)
+   {
+      return javaSideValueContainer.get(element);
+   }
+
+   @Override
+   public void close()
+   {
+      javaSideValueContainer.close();
+      super.close();
+   }
+}