zed occupancy map fix

Author: LuigiPenco93

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXZEDOccupancyMapDemo.java

@@ -5,8 +5,10 @@
 import us.ihmc.euclid.tuple3D.Point3D;
 import us.ihmc.perception.RawImage;
 import us.ihmc.rdx.Lwjgl3ApplicationAdapter;
+import us.ihmc.rdx.RDXPointCloudRenderer;
 import us.ihmc.rdx.imgui.ImGuiTools;
 import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap;
+import us.ihmc.rdx.sceneManager.RDXSceneLevel;
 import us.ihmc.rdx.ui.RDXBaseUI;
 import us.ihmc.rdx.ui.graphics.RDXImageVisualizer;
 import us.ihmc.rdx.ui.graphics.RDXPerceptionVisualizersPanel;
@@ -16,12 +18,14 @@
 import us.ihmc.sensors.zed.ZEDSVOPlaybackSensor;
 import us.ihmc.zed.global.zed;
 
+import java.util.List;
+
 /**
  * Demo class for ZED occupancy map.
  */
 public class RDXZEDOccupancyMapDemo extends Lwjgl3ApplicationAdapter
 {
-   private static final String SVO_FILE = "/home/duncan/Downloads/20251110_162146_H1ZEDXMiniFirstMustardGrab.svo2";
+   private static final String SVO_FILE = "/home/duncan/Downloads/20251020_ZEDXMini_DoorChargeBarrierBottle.svo2";
    private static final RDXBaseUI baseUI = new RDXBaseUI();
    private final ZEDSVOPlaybackSensor zedSensor = new ZEDSVOPlaybackSensor(0, ZEDModelData.ZED_2I, zed.SL_DEPTH_MODE_PERFORMANCE, SVO_FILE);
    private RDXPerceptionVisualizersPanel visualizers;
@@ -30,6 +34,9 @@ public class RDXZEDOccupancyMapDemo extends Lwjgl3ApplicationAdapter
    private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass());
    private final ImInt requestedPosition = new ImInt();
 
+   private RDXPointCloudRenderer occupancyRenderer;
+   private static final int MAX_POINTS_TO_RENDER = 100000;
+
    @Override
    public void create()
    {
@@ -47,8 +54,16 @@ public void create()
       }
       visualizers.create(baseUI);
 
+      // Create occupancy renderer from spatial mesh
+      occupancyRenderer = new RDXPointCloudRenderer();
+      occupancyRenderer.create(MAX_POINTS_TO_RENDER);
+      occupancyRenderer.setColoringMethod(RDXPointCloudRenderer.ColoringMethod.GRADIENT_WORLD_Z);
+      baseUI.getPrimaryScene().addRenderableProvider(occupancyRenderer, RDXSceneLevel.VIRTUAL);
+
       baseUI.getImGuiPanelManager().addPanel("Demo", this::renderImGuiWidgets);
 
+      zedSensor.enablePositionalTracking(true);
+      zedSensor.enableSpatialMapping(true);
       zedSensor.startSensor();
       zedSensor.getGrabThread().setFrequencyLimit(15.0);
    }
@@ -91,11 +106,55 @@ public void render()
             image.release();
          }
       }
+      updateOccupancyPointCloud();
 
       baseUI.renderBeforeOnScreenUI();
       baseUI.renderEnd();
    }
 
+   private void updateOccupancyPointCloud()
+   {
+      if (occupancyRenderer == null)
+         return;
+
+      float[] vertices = zedSensor.getMeshVertices();
+      int numVertices = zedSensor.getMeshNumVertices();
+
+      if (vertices == null || numVertices == 0)
+      {
+         occupancyRenderer.updateMesh(List.of());
+         return;
+      }
+
+      // CRITICAL: Enforce strict limit - never exceed MAX_POINTS_TO_RENDER
+      int pointsToSample = Math.min(numVertices, MAX_POINTS_TO_RENDER);
+      // Calculate stride to hit exactly MAX_POINTS_TO_RENDER points
+      int stride = numVertices > MAX_POINTS_TO_RENDER ?
+            (numVertices + MAX_POINTS_TO_RENDER - 1) / MAX_POINTS_TO_RENDER : 1;
+      // Pre-allocate exactly the number we'll use
+      Point3D[] sampledPoints = new Point3D[pointsToSample];
+
+      int idx = 0;
+      for (int i = 0; i < numVertices && idx < MAX_POINTS_TO_RENDER; i += stride)
+      {
+         int base = 3 * i;
+         if (base + 2 >= vertices.length)
+            break;
+
+         sampledPoints[idx++] = new Point3D(vertices[base], vertices[base + 1], vertices[base + 2]);
+      }
+
+      // Only send what we actually filled
+      if (idx < sampledPoints.length)
+      {
+         Point3D[] trimmed = new Point3D[idx];
+         System.arraycopy(sampledPoints, 0, trimmed, 0, idx);
+         sampledPoints = trimmed;
+      }
+
+      occupancyRenderer.updateMesh(sampledPoints);
+   }
+
    @Override
    public void dispose()
    {

ihmc-perception/src/main/java/us/ihmc/sensors/zed/ZEDImageSensor.java

@@ -510,48 +510,95 @@ public void enableSpatialMapping(boolean enable)
     */
    private void updateMeshData()
    {
-      try
+      // Use IntPointers for output parameters to ensure we read C-side writes
+      try (IntPointer nbVerticesPerSubmesh = new IntPointer(1000);
+           IntPointer nbTrianglesPerSubmesh = new IntPointer(1000);
+           IntPointer nbSubmeshes = new IntPointer(1);
+           IntPointer updatedIndices = new IntPointer(1000);
+           IntPointer nbVerticesTot = new IntPointer(1);
+           IntPointer nbTrianglesTot = new IntPointer(1))
       {
-         // Extract the complete spatial map
-         sl_extract_whole_spatial_map(cameraID);
-
-         // Get mesh size information
          int maxSubmesh = 1000;
-         int[] nbVerticesPerSubmesh = new int[maxSubmesh];
-         int[] nbTrianglesPerSubmesh = new int[maxSubmesh];
-         int[] nbSubmeshes = new int[1];
-         int[] updatedIndices = new int[maxSubmesh];
-         int[] nbVerticesTot = new int[1];
-         int[] nbTrianglesTot = new int[1];
 
-         sl_update_mesh(cameraID, nbVerticesPerSubmesh, nbTrianglesPerSubmesh, nbSubmeshes, updatedIndices, nbVerticesTot, nbTrianglesTot, maxSubmesh);
+         int mappingState = sl_get_spatial_mapping_state(cameraID);
+         LogTools.debug("Spatial mapping state: {}", mappingState);
+         switch (mappingState)
+         {
+            case SL_SPATIAL_MAPPING_STATE_INITIALIZING:
+               LogTools.debug("Spatial mapping initializing...");
+               break;
+            case SL_SPATIAL_MAPPING_STATE_OK:
+               LogTools.debug("Spatial mapping OK");
+               break;
+            case SL_SPATIAL_MAPPING_STATE_NOT_ENOUGH_MEMORY:
+               LogTools.debug("Not enough memory for spatial mapping");
+               return;
+            case SL_SPATIAL_MAPPING_STATE_NOT_ENABLED:
+               LogTools.debug("Spatial mapping not enabled");
+               return;
+            case SL_SPATIAL_MAPPING_STATE_FPS_TOO_LOW:
+               LogTools.debug("FPS too low for spatial mapping");
+               return;
+         }
 
-         int numVertices = nbVerticesTot[0];
-         int numTriangles = nbTrianglesTot[0];
+         sl_request_mesh_async(cameraID);
+
+         // Check if mesh is ready
+         int meshReadyStatus = sl_get_mesh_request_status_async(cameraID);
+         if (meshReadyStatus != SL_ERROR_CODE_SUCCESS)
+         {
+            LogTools.debug("Mesh not ready yet: {}", getZEDErrorName(meshReadyStatus));
+            return;
+         }
 
+         // Call update_mesh with Pointers
+         int updateResult = sl_update_mesh(cameraID,
+                                           nbVerticesPerSubmesh,
+                                           nbTrianglesPerSubmesh,
+                                           nbSubmeshes,
+                                           updatedIndices,
+                                           nbVerticesTot,
+                                           nbTrianglesTot,
+                                           maxSubmesh);
+
+         // Check result code
+         if (updateResult != SL_ERROR_CODE_SUCCESS)
+         {
+            LogTools.warn("sl_update_mesh failed with error: {} ({})",
+                          getZEDErrorName(updateResult), updateResult);
+            return;
+         }
+
+         // Read values from Pointers
+         int numVertices = nbVerticesTot.get();
+         int numTriangles = nbTrianglesTot.get();
          if (numVertices > 0 && numTriangles > 0)
          {
             // Allocate arrays for mesh data
-            float[] vertices = new float[numVertices * 3]; // x, y, z for each vertex
-            int[] triangles = new int[numTriangles * 3]; // v1, v2, v3 indices for each triangle
-            byte[] colors = new byte[numVertices * 4]; // RGBA for each vertex
-            float[] uvs = null; // Texture coordinates (not needed if save_texture is false)
-            byte[] texturePtr = null; // Texture data (not needed if save_texture is false)
+            float[] vertices = new float[numVertices * 3];
+            int[] triangles = new int[numTriangles * 3];
+            byte[] colors = new byte[numVertices * 4];
+            float[] uvs = null;
+            byte[] texturePtr = null;
 
-            // Retrieve mesh data
-            sl_retrieve_mesh(cameraID, vertices, triangles, colors, uvs, texturePtr, nbSubmeshes[0]);
+            // Retrieve using the calculated sizes
+            int retrieveResult = sl_retrieve_mesh(cameraID, vertices, triangles, colors, uvs, texturePtr, maxSubmesh);
 
-            // Store mesh data in fields
-            synchronized (this)
+            if (retrieveResult == SL_ERROR_CODE_SUCCESS)
             {
-               meshVertices = vertices;
-               meshTriangles = triangles;
-               meshColors = colors;
-               meshNumVertices = numVertices;
-               meshNumTriangles = numTriangles;
+               synchronized (this)
+               {
+                  meshVertices = vertices;
+                  meshTriangles = triangles;
+                  meshColors = colors;
+                  meshNumVertices = numVertices;
+                  meshNumTriangles = numTriangles;
+               }
+            }
+            else
+            {
+               LogTools.error("Failed to retrieve mesh: " + getZEDErrorName(retrieveResult));
             }
-
-            LogTools.info("Mesh updated: {} vertices, {} triangles (from {} submeshes)", numVertices, numTriangles, nbSubmeshes[0]);
          }
       }
       catch (Exception e)