From ac446b0a049e7c9a2aecdd69065e40fa9b17c3e4 Mon Sep 17 00:00:00 2001 From: Duncan Calvert Date: Wed, 3 Dec 2025 12:18:40 -0600 Subject: [PATCH] Start implementing ZED spatial mapping support. --- .../perception/RDXZEDSpatialMappingDemo.java | 169 +++++++++++++ .../RDXZEDSpatialMappingDemo/ImGuiPanels.json | 10 + .../ImGuiSettings.ini | 51 ++++ .../us/ihmc/sensors/zed/ZEDImageSensor.java | 236 ++++++++++++++++++ 4 files changed, 466 insertions(+) create mode 100644 ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXZEDSpatialMappingDemo.java create mode 100644 ihmc-high-level-behaviors/src/libgdx/resources/configurations/RDXZEDSpatialMappingDemo/ImGuiPanels.json create mode 100644 ihmc-high-level-behaviors/src/libgdx/resources/configurations/RDXZEDSpatialMappingDemo/ImGuiSettings.ini diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXZEDSpatialMappingDemo.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXZEDSpatialMappingDemo.java new file mode 100644 index 000000000000..81bff07f79b2 --- /dev/null +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXZEDSpatialMappingDemo.java @@ -0,0 +1,169 @@ +package us.ihmc.rdx.perception; + +import imgui.ImGui; +import imgui.type.ImInt; +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; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SideDependentList; +import us.ihmc.sensors.zed.ZEDModelData; +import us.ihmc.sensors.zed.ZEDSVOPlaybackSensor; +import us.ihmc.zed.global.zed; + +import java.util.List; + +/** + * Demo class for ZED spatial mapping. + */ +public class RDXZEDSpatialMappingDemo extends Lwjgl3ApplicationAdapter +{ + 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; + private final SideDependentList zedPanels = new SideDependentList<>(); + private long lastZEDTimestamp = 1; + private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass()); + private final ImInt requestedPosition = new ImInt(); + + private RDXPointCloudRenderer spatialMapRenderer; + private static final int MAX_POINTS_TO_RENDER = 100000; + + @Override + public void create() + { + baseUI.create(); + baseUI.getPrimary3DPanel().getCamera3D().setCameraFocusPoint(new Point3D(0.7, 0.0, 0.4)); + baseUI.getPrimary3DPanel().getCamera3D().changeCameraPosition(-3.0, -4.0, 4.0); + + visualizers = new RDXPerceptionVisualizersPanel(); + for (RobotSide side : RobotSide.values) + { + RDXImageVisualizer zedPanel = new RDXImageVisualizer("%s ZED".formatted(side.getPascalCaseName()), "%s ZED".formatted(side.getPascalCaseName()), false); + zedPanels.put(side, zedPanel); + zedPanel.setActive(true); + visualizers.addVisualizer(zedPanel); + } + visualizers.create(baseUI); + + // Create spatial map renderer from spatial mesh + spatialMapRenderer = new RDXPointCloudRenderer(); + spatialMapRenderer.create(MAX_POINTS_TO_RENDER); + spatialMapRenderer.setColoringMethod(RDXPointCloudRenderer.ColoringMethod.GRADIENT_WORLD_Z); + baseUI.getPrimaryScene().addRenderableProvider(spatialMapRenderer, RDXSceneLevel.VIRTUAL); + + baseUI.getImGuiPanelManager().addPanel("Demo", this::renderImGuiWidgets); + + zedSensor.enablePositionalTracking(true); + zedSensor.enableSpatialMapping(true); + zedSensor.startSensor(); + zedSensor.getGrabThread().setFrequencyLimit(15.0); + } + + private void renderImGuiWidgets() + { + ImGui.text("Current SVO:"); + ImGui.sameLine(); + ImGui.textWrapped(zedSensor.getSVOFileName()); + + ImGui.text("Current Position: %d".formatted(zedSensor.getCurrentPosition())); + ImGui.text("Length: %d".formatted(zedSensor.getLength())); + + requestedPosition.set(zedSensor.getCurrentPosition()); + + if (ImGui.button(labels.get("Play"))) + zedSensor.run(true); + ImGui.sameLine(); + if (ImGui.button(labels.get("Pause"))) + zedSensor.run(false); + + if (ImGuiTools.sliderInt(labels.get("Position"), requestedPosition, 0, Math.max(zedSensor.getLength(), 0))) + zedSensor.setCurrentPosition(requestedPosition.get()); + } + + @Override + public void render() + { + visualizers.update(); + + long lastGrabTimestamp = zedSensor.getLastGrabTimestamp(); + if (lastGrabTimestamp > lastZEDTimestamp) + { + lastZEDTimestamp = lastGrabTimestamp; + + for (RobotSide side : RobotSide.values) + { + RawImage image = zedSensor.getImage(zedSensor.getImageKeys()[side.ordinal()]); + zedPanels.get(side).setImage(image); + image.release(); + } + } + updateOccupancyPointCloud(); + + baseUI.renderBeforeOnScreenUI(); + baseUI.renderEnd(); + } + + private void updateOccupancyPointCloud() + { + if (spatialMapRenderer == null) + return; + + float[] vertices = zedSensor.getMeshVertices(); + int numVertices = zedSensor.getMeshNumVertices(); + + if (vertices == null || numVertices == 0) + { + spatialMapRenderer.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; + } + + spatialMapRenderer.updateMesh(sampledPoints); + } + + @Override + public void dispose() + { + visualizers.destroy(); + baseUI.dispose(); + } + + public static void main(String[] args) + { + baseUI.launchRDXApplication(new RDXZEDSpatialMappingDemo()); + } +} diff --git a/ihmc-high-level-behaviors/src/libgdx/resources/configurations/RDXZEDSpatialMappingDemo/ImGuiPanels.json b/ihmc-high-level-behaviors/src/libgdx/resources/configurations/RDXZEDSpatialMappingDemo/ImGuiPanels.json new file mode 100644 index 000000000000..2c5f42b42ba7 --- /dev/null +++ b/ihmc-high-level-behaviors/src/libgdx/resources/configurations/RDXZEDSpatialMappingDemo/ImGuiPanels.json @@ -0,0 +1,10 @@ +{ + "dockspacePanels" : { }, + "windows" : { + "3D View" : true, + "Demo" : true, + "Perception Visualizers" : true, + "Left ZED###RDXImageVisualizer1:Left ZED" : true, + "Right ZED###RDXImageVisualizer3:Right ZED" : true + } +} \ No newline at end of file diff --git a/ihmc-high-level-behaviors/src/libgdx/resources/configurations/RDXZEDSpatialMappingDemo/ImGuiSettings.ini b/ihmc-high-level-behaviors/src/libgdx/resources/configurations/RDXZEDSpatialMappingDemo/ImGuiSettings.ini new file mode 100644 index 000000000000..1641fa774368 --- /dev/null +++ b/ihmc-high-level-behaviors/src/libgdx/resources/configurations/RDXZEDSpatialMappingDemo/ImGuiSettings.ini @@ -0,0 +1,51 @@ +[Window][DockSpaceViewport_11111111] +Pos=0,22 +Size=1734,979 +Collapsed=0 + +[Window][3D View] +Pos=409,22 +Size=1325,979 +Collapsed=0 +DockId=0x00000002,0 + +[Window][Debug##Default] +Pos=60,60 +Size=400,400 +Collapsed=0 + +[Window][Demo] +Pos=0,162 +Size=407,358 +Collapsed=0 +DockId=0x00000007,0 + +[Window][Perception Visualizers] +Pos=0,22 +Size=407,138 +Collapsed=0 +DockId=0x00000003,0 + +[Window][###RDXImageVisualizer1:Left ZED] +Pos=0,522 +Size=407,237 +Collapsed=0 +DockId=0x00000008,0 + +[Window][###RDXImageVisualizer3:Right ZED] +Pos=0,761 +Size=407,240 +Collapsed=0 +DockId=0x00000006,0 + +[Docking][Data] +DockSpace ID=0x8B93E3BD Window=0xA787BDB4 Pos=191,162 Size=1734,979 Split=X Selected=0x2E64DC63 + DockNode ID=0x00000001 Parent=0x8B93E3BD SizeRef=407,979 Split=Y Selected=0x7670709E + DockNode ID=0x00000003 Parent=0x00000001 SizeRef=299,138 Selected=0x4E964DE0 + DockNode ID=0x00000004 Parent=0x00000001 SizeRef=299,839 Split=Y Selected=0x7670709E + DockNode ID=0x00000005 Parent=0x00000004 SizeRef=485,597 Split=Y Selected=0x7670709E + DockNode ID=0x00000007 Parent=0x00000005 SizeRef=485,358 Selected=0x7670709E + DockNode ID=0x00000008 Parent=0x00000005 SizeRef=485,237 Selected=0xA473CDA8 + DockNode ID=0x00000006 Parent=0x00000004 SizeRef=485,240 Selected=0x05C832D5 + DockNode ID=0x00000002 Parent=0x8B93E3BD SizeRef=1325,979 CentralNode=1 Selected=0x2E64DC63 + diff --git a/ihmc-perception/src/main/java/us/ihmc/sensors/zed/ZEDImageSensor.java b/ihmc-perception/src/main/java/us/ihmc/sensors/zed/ZEDImageSensor.java index 33f6a59e4fac..872ca75baa79 100644 --- a/ihmc-perception/src/main/java/us/ihmc/sensors/zed/ZEDImageSensor.java +++ b/ihmc-perception/src/main/java/us/ihmc/sensors/zed/ZEDImageSensor.java @@ -1,5 +1,6 @@ package us.ihmc.sensors.zed; +import org.bytedeco.javacpp.IntPointer; import org.bytedeco.javacpp.Pointer; import org.bytedeco.opencv.opencv_core.GpuMat; import us.ihmc.euclid.referenceFrame.ReferenceFrame; @@ -21,6 +22,7 @@ import us.ihmc.zed.SL_PositionalTrackingParameters; import us.ihmc.zed.SL_Quaternion; import us.ihmc.zed.SL_RuntimeParameters; +import us.ihmc.zed.SL_SpatialMappingParameters; import us.ihmc.zed.SL_Vector3; import us.ihmc.zed.library.ZEDJavaAPINativeLibrary; @@ -78,10 +80,18 @@ public class ZEDImageSensor extends ImageSensor protected final SL_RuntimeParameters zedRuntimeParameters = new SL_RuntimeParameters(); private boolean positionalTrackingEnabled = false; + private boolean spatialMappingEnabled = false; private final MutableReferenceFrame trackedSensorFrame; private final SL_Quaternion sensorRotation = new SL_Quaternion(); private final SL_Vector3 sensorTranslation = new SL_Vector3(); + // Spatial mapping mesh data + private float[] meshVertices = null; + private int[] meshTriangles = null; + private byte[] meshColors = null; + private int meshNumVertices = 0; + private int meshNumTriangles = 0; + public ZEDImageSensor(int cameraID, ZEDModelData zedModel, int slInputType, int slDepthMode) { this(cameraID, zedModel, slInputType, slDepthMode, DEFAULT_RESOLUTION, DEFAULT_FPS); @@ -174,9 +184,37 @@ protected boolean startSensor() if (positionalTrackingEnabled) { SL_PositionalTrackingParameters positionalTrackingParameters = sl_get_positional_tracking_parameters(cameraID); + positionalTrackingParameters.enable_area_memory(true); + positionalTrackingParameters.enable_imu_fusion(true); + positionalTrackingParameters.enable_pose_smoothing(false); + positionalTrackingParameters.set_floor_as_origin(false); sl_enable_positional_tracking(cameraID, positionalTrackingParameters, ""); } + if (spatialMappingEnabled) + { + // Positional tracking is required for spatial mapping + if (!positionalTrackingEnabled) + { + LogTools.warn("Spatial mapping requires positional tracking. Enabling positional tracking automatically."); + SL_PositionalTrackingParameters positionalTrackingParameters = sl_get_positional_tracking_parameters(cameraID); + positionalTrackingParameters.enable_area_memory(true); + positionalTrackingParameters.enable_imu_fusion(true); + sl_enable_positional_tracking(cameraID, positionalTrackingParameters, ""); + positionalTrackingEnabled = true; + } + + SL_SpatialMappingParameters spatialMappingParameters = new SL_SpatialMappingParameters(); + spatialMappingParameters.map_type(SL_SPATIAL_MAP_TYPE_MESH); // Mesh, not point cloud + spatialMappingParameters.resolution_meter(0.05f); // 5cm resolution + spatialMappingParameters.range_meter(0.0f); // Automatic range detection + spatialMappingParameters.save_texture(false); // Enable if RGB texture needed + spatialMappingParameters.use_chunk_only(true); // Use chunked mesh format + spatialMappingParameters.max_memory_usage(2048); // MB + spatialMappingParameters.reverse_vertex_order(false); + sl_enable_spatial_mapping(cameraID, spatialMappingParameters); + } + // Get camera intrinsics SL_CalibrationParameters sensorIntrinsics = sl_get_calibration_parameters(cameraID, false); imageWidth = sl_get_width(cameraID); @@ -314,6 +352,30 @@ protected boolean grab() trackedSensorFrame.update(transformToWorld -> transformToWorld.set(euclidRotation, euclidTranslation)); } + if (spatialMappingEnabled) + { + // Monitor mapping state + int mappingState = sl_get_spatial_mapping_state(cameraID); + switch (mappingState) + { + case SL_SPATIAL_MAPPING_STATE_INITIALIZING -> + LogTools.debug("Spatial mapping initializing..."); + case SL_SPATIAL_MAPPING_STATE_OK -> { /* Normal operation */ } + case SL_SPATIAL_MAPPING_STATE_NOT_ENOUGH_MEMORY -> + LogTools.warn("Spatial mapping: Not enough memory"); + case SL_SPATIAL_MAPPING_STATE_NOT_ENABLED -> + LogTools.warn("Spatial mapping not enabled"); + case SL_SPATIAL_MAPPING_STATE_FPS_TOO_LOW -> + LogTools.warn("Spatial mapping: FPS too low"); + } + + // Extract and update mesh data periodically + if (grabSequenceNumber % 30 == 0) // Update mesh every 30 frames + { + updateMeshData(); + } + } + // Retrieve the grabbed depth image Pointer depthImagePointer = slMatPointers[DEPTH_IMAGE_KEY]; returnCode = sl_retrieve_measure(cameraID, depthImagePointer, SL_MEASURE_DEPTH_U16_MM, SL_MEM_GPU, imageWidth, imageHeight, null); // TODO: Pass custream @@ -437,9 +499,183 @@ public void enablePositionalTracking(boolean enable) positionalTrackingEnabled = enable; } + public void enableSpatialMapping(boolean enable) + { + spatialMappingEnabled = enable; + } + + /** + * Updates the mesh data from the spatial mapping system. + * Extracts vertices, triangles, and colors from the ZED spatial map. + */ + private void updateMeshData() + { + // 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)) + { + int maxSubmesh = 1000; + + 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; + } + + 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]; + int[] triangles = new int[numTriangles * 3]; + byte[] colors = new byte[numVertices * 4]; + float[] uvs = null; + byte[] texturePtr = null; + + // Retrieve using the calculated sizes + int retrieveResult = sl_retrieve_mesh(cameraID, vertices, triangles, colors, uvs, texturePtr, maxSubmesh); + + if (retrieveResult == SL_ERROR_CODE_SUCCESS) + { + synchronized (this) + { + meshVertices = vertices; + meshTriangles = triangles; + meshColors = colors; + meshNumVertices = numVertices; + meshNumTriangles = numTriangles; + } + } + else + { + LogTools.error("Failed to retrieve mesh: " + getZEDErrorName(retrieveResult)); + } + } + } + catch (Exception e) + { + LogTools.error("Error updating mesh data: {}", e.getMessage()); + } + } + + /** + * Saves the current spatial mesh to a file. + * @param filePath Path to save the mesh (supports .obj, .ply, .bin formats) + */ + public void saveMesh(String filePath) + { + if (!spatialMappingEnabled) + { + LogTools.warn("Spatial mapping is not enabled. Cannot save mesh."); + return; + } + + sl_extract_whole_spatial_map(cameraID); + boolean result = sl_save_mesh(cameraID, filePath, SL_MESH_FILE_FORMAT_OBJ); + + if (result) + LogTools.info("Mesh saved to: {}", filePath); + else + LogTools.error("Failed to save mesh to: {}", filePath); + } + + /** + * @return Current mesh vertices as float array [x1,y1,z1, x2,y2,z2, ...], or null if no mesh available + */ + public synchronized float[] getMeshVertices() + { + return meshVertices; + } + + /** + * @return Current mesh triangles as int array [v1,v2,v3, v1,v2,v3, ...], or null if no mesh available + */ + public synchronized int[] getMeshTriangles() + { + return meshTriangles; + } + + /** + * @return Current mesh colors as byte array [r,g,b,a, r,g,b,a, ...], or null if no mesh available + */ + public synchronized byte[] getMeshColors() + { + return meshColors; + } + + /** + * @return Number of vertices in the current mesh + */ + public synchronized int getMeshNumVertices() + { + return meshNumVertices; + } + + /** + * @return Number of triangles in the current mesh + */ + public synchronized int getMeshNumTriangles() + { + return meshNumTriangles; + } + @Override public void close() { + if (spatialMappingEnabled) + { + sl_disable_spatial_mapping(cameraID); + } + System.out.println("Closing " + getClass().getSimpleName()); super.close();