Fix bug retrieving depth buffer for mouse scene collision

ihmc-graphics/src/libgdx/java/us/ihmc/rdx/input/ImGui3DViewInput.java

@@ -129,9 +129,9 @@ public Point3DReadOnly getPickPointInWorld(double fallbackXYPlaneIntersectionHei
 
             int rowAdjustment = aliasedFlippedMouseY * aliasedRenderedAreaWidth * Float.BYTES;
             int columnAdjustment = aliasedMouseX * Float.BYTES;
-            float normalizedDeviceCoordinateZ = depthBuffer.getFloat(rowAdjustment + columnAdjustment);
+            float normalizedDeviceCoordinateZ = 2.0f * depthBuffer.getFloat(rowAdjustment + columnAdjustment) - 1.0f;
 
-            if (normalizedDeviceCoordinateZ > 0.503)
+            if (normalizedDeviceCoordinateZ < 1.0f) // 1.0 corresponds to no collision
             {
                fallbackToXYPlaneIntersection = false;
 

ihmc-graphics/src/libgdx/java/us/ihmc/rdx/ui/RDX3DPanel.java

@@ -193,10 +193,10 @@ public void render()
             renderScene(RDXSceneLevel.MODEL.SINGLETON_SET);
 
             normalizedDeviceCoordinateDepthDirectByteBuffer.rewind(); // SIGSEV otherwise
-            GL41.glReadBuffer(GL41.GL_COLOR_ATTACHMENT1);
+            GL41.glReadBuffer(GL41.GL_DEPTH_ATTACHMENT);
             GL41.glPixelStorei(GL41.GL_UNPACK_ALIGNMENT, 4); // to read floats
             // Note: This line has significant performance impact
-            GL41.glReadPixels(0, 0, (int) renderSizeX, (int) renderSizeY, GL41.GL_RED, GL41.GL_FLOAT, normalizedDeviceCoordinateDepthDirectByteBuffer);
+            GL41.glReadPixels(0, 0, (int) renderSizeX, (int) renderSizeY, GL41.GL_DEPTH_COMPONENT, GL41.GL_FLOAT, normalizedDeviceCoordinateDepthDirectByteBuffer);
             GL41.glPixelStorei(GL41.GL_UNPACK_ALIGNMENT, 1); // undo what we did
 
             frameBuffer.end();

ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXHighLevelDepthSensorDemo.java

@@ -3,7 +3,9 @@
 import com.badlogic.gdx.graphics.Color;
 import com.badlogic.gdx.graphics.g3d.ModelInstance;
 import imgui.ImGui;
+import imgui.flag.ImGuiMouseButton;
 import org.bytedeco.opencv.global.opencv_core;
+import perception_msgs.msg.dds.HeightMapMessage;
 import us.ihmc.rdx.Lwjgl3ApplicationAdapter;
 import us.ihmc.rdx.sceneManager.RDXSceneLevel;
 import us.ihmc.rdx.simulation.environment.RDXEnvironmentBuilder;
@@ -13,8 +15,10 @@
 import us.ihmc.rdx.ui.RDX3DPanel;
 import us.ihmc.rdx.ui.RDXBaseUI;
 import us.ihmc.rdx.ui.gizmo.RDXPose3DGizmo;
+import us.ihmc.rdx.ui.graphics.RDXHeightMapGraphicNew;
 import us.ihmc.rdx.visualizers.RDXFrustumGraphic;
 import us.ihmc.perception.BytedecoImage;
+import us.ihmc.sensorProcessing.heightMap.HeightMapMessageTools;
 
 import java.nio.ByteBuffer;
 
@@ -31,6 +35,7 @@ public class RDXHighLevelDepthSensorDemo
    private RDXFrustumGraphic frustumVisualizer;
    private RDXBytedecoImagePanel mainViewDepthPanel;
    private BytedecoImage image;
+   private RDXHeightMapGraphicNew heightMap;
 
    public RDXHighLevelDepthSensorDemo()
    {
@@ -40,6 +45,7 @@ public RDXHighLevelDepthSensorDemo()
          public void create()
          {
             baseUI.create();
+            baseUI.setModelSceneMouseCollisionEnabled(true);
 
             environmentBuilder = new RDXEnvironmentBuilder(baseUI.getPrimary3DPanel());
             environmentBuilder.create();
@@ -99,16 +105,36 @@ public void create()
             baseUI.getImGuiPanelManager().addPanel("Mouse Picking", () ->
             {
                ImGui.text("Mouse x: " + mousePosX + " y: " + mousePosY);
+
+               if (ImGui.isMouseClicked(ImGuiMouseButton.Right))
+               {  // Place breakpoint here for debugging
+                  ImGui.text("Right mouse button clicked at x: " + mousePosX + " y: " + mousePosY);
+               }
             });
 
             RDX3DPanel panel3D = new RDX3DPanel("3D View 2", true);
             baseUI.add3DPanel(panel3D);
 
             frustumVisualizer = new RDXFrustumGraphic();
-            baseUI.getPrimaryScene().addRenderableProvider(frustumVisualizer::getRenderables, RDXSceneLevel.VIRTUAL);
+            baseUI.getPrimaryScene().addRenderableProvider(frustumVisualizer, RDXSceneLevel.VIRTUAL);
+
+            ModelInstance box = RDXModelBuilder.createBox(0.2f, 0.2f, 0.2f, Color.YELLOW);
+            box.transform.translate(0.4f, 0.3f, 0.25f);
+            baseUI.getPrimaryScene().addRenderableProvider(box, RDXSceneLevel.MODEL);
+            baseUI.getPrimaryScene().addRenderableProvider(box, RDXSceneLevel.GROUND_TRUTH);
+
+            heightMap = new RDXHeightMapGraphicNew();
+            HeightMapMessage heightMapMessage = new HeightMapMessage();
+            heightMapMessage.setXyResolution(0.1);
+            heightMapMessage.setGridSizeXy(2.0);
+            heightMapMessage.setGridCenterX(1.0);
+            heightMapMessage.setGridCenterY(1.0);
+            heightMapMessage.setEstimatedGroundHeight(0.0);
+            HeightMapMessageTools.setToFlatGround(heightMapMessage);
+            heightMap.generateMeshesAsync(heightMapMessage);
+            baseUI.getPrimaryScene().addRenderableProvider(heightMap, RDXSceneLevel.MODEL);
          }
 
-
          @Override
          public void render()
          {
@@ -142,6 +168,8 @@ public void render()
             frustumVisualizer.generateMeshAsync(baseUI.getPrimary3DPanel().getCamera3D().frustum);
             frustumVisualizer.update();
 
+            heightMap.update();
+
             baseUI.renderBeforeOnScreenUI();
             baseUI.renderEnd();
          }
@@ -152,6 +180,7 @@ public void dispose()
             baseUI.dispose();
             environmentBuilder.destroy();
             highLevelDepthSensorSimulator.dispose();
+            heightMap.destroy();
          }
       });
    }

ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/heightMap/HeightMapMessageTools.java

@@ -65,4 +65,20 @@ public static void clear(HeightMapMessage messageToClear)
       messageToClear.getVariances().clear();
       messageToClear.getCentroids().clear();
    }
+
+   public static void setToFlatGround(HeightMapMessage message)
+   {
+      int centerIndex = HeightMapTools.computeCenterIndex(message.getGridSizeXy(), message.getXyResolution());
+      int cellsPerAxis = 2 * centerIndex + 1;
+
+      for (int xIndex = 0; xIndex < cellsPerAxis; xIndex++)
+      {
+         for (int yIndex = 0; yIndex < cellsPerAxis; yIndex++)
+         {
+            int key = HeightMapTools.indicesToKey(xIndex, yIndex, centerIndex);
+            message.getKeys().add(key);
+            message.getHeights().add(0.0f);
+         }
+      }
+   }
 }