Switch to the new method to pack the message to be faster (#878)

PotatoPeeler3000 · web-flow · commit 5a6e8128016f · 2025-06-25T13:31:17.000-05:00
diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2HeightMapVisualizer.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2HeightMapVisualizer.java
@@ -108,19 +108,27 @@ public void acceptHeightMapMessage(HeightMapMessage heightMapMessage)
                                               if (sequenceId > 1)
                                               {
                                                  // We add +1 here because the height map is
-                                                 cellsPerAxis = (int) (heightMapMessage.getGridSizeXy() / heightMapMessage.getXyResolution()) + 1;
+                                                 int centerIndex = HeightMapTools.computeCenterIndex(4.0, 0.02);
+                                                 cellsPerAxis = 2 * centerIndex + 1;
                                               }
 
                                               heightMapCenter.setX(heightMapMessage.getGridCenterX());
                                               heightMapCenter.setY(heightMapMessage.getGridCenterY());
-                                              latestHeightMapData = HeightMapMessageTools.unpackMessageToHeightMapData(heightMapMessage);
-                                              if (heightMap == null)
+                                              heightMap = HeightMapMessageTools.unpackMessageToMat(heightMapMessage, heightMapParameters);
+
+                                              if (latestHeightMapData == null)
                                               {
-                                                 heightMap = new Mat(latestHeightMapData.getCellsPerAxis(),
-                                                                     latestHeightMapData.getCellsPerAxis(),
-                                                                     opencv_core.CV_16UC1);
+                                                 latestHeightMapData = new HeightMapData(heightMapMessage.getXyResolution(),
+                                                                                         heightMapMessage.getGridSizeXy(),
+                                                                                         heightMapMessage.getGridCenterX(),
+                                                                                         heightMapMessage.getGridCenterY());
                                               }
-                                              HeightMapTools.convertHeightMapDataToMat(heightMap, latestHeightMapData, heightMapParameters);
+                                              HeightMapTools.convertToHeightMapData(heightMap,
+                                                                                    latestHeightMapData,
+                                                                                    heightMapCenter,
+                                                                                    (float) heightMapParameters.getGlobalWidthInMeters(),
+                                                                                    (float) heightMapParameters.getCellSizeInMeters(),
+                                                                                    heightMapParameters);
 
                                               // This prevents the rendering from happening to early, it was throwing exceptions
                                               if (stopwatch.lapElapsed() > 3)
@@ -272,4 +280,4 @@ public RDXOpenCVVideoVisualizer getHeightMapImageVisualizer()
    {
       return heightMapImageVisualizer;
    }
-}
+}
diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.java
@@ -542,7 +542,7 @@ private void deallocateFloatPointer(FloatPointer hostPointer, Pointer devicePoin
 
    public GpuMat getHeightMap()
    {
-      return scaledHeightMap.clone();
+      return globalMeanMap;
    }
 
    public GpuMat getTerrainCroppedHeightMap()
diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java
@@ -40,9 +40,10 @@ public class RapidHeightMapManager
 
    private final ROS2Publisher<HeightMapMessage> heightMapMessagePublisher;
    private final BytePointer compressedHeightMapPointer = new BytePointer();
-   private final HeightMapData latestHeightMapData;
    private final HeightMapData latestTerrainHeightMapData;
    private final Point3D gridCellLocation = new Point3D();
+   // This is created globally cause it takes compute time to create it in the update loop
+   private final HeightMapMessage heightMapMessage = new HeightMapMessage();
    private long sequenceId = 0;
 
    public RapidHeightMapManager(ROS2Node ros2Node,
@@ -54,10 +55,7 @@ public RapidHeightMapManager(ROS2Node ros2Node,
    {
       this.heightMapCenter = heightMapCenter;
       this.heightMapParameters = heightMapParameters;
-      latestHeightMapData = new HeightMapData((float) heightMapParameters.getCellSizeInMeters(),
-                                              (float) heightMapParameters.getGlobalWidthInMeters(),
-                                              0.0,
-                                              0.0);
+
       latestTerrainHeightMapData = new HeightMapData((float) heightMapParameters.getCellSizeInMeters(),
                                                      (float) heightMapParameters.getTerrainWidthInMeters(),
                                                      0.0,
@@ -79,9 +77,7 @@ public RapidHeightMapManager(ROS2Node ros2Node,
    public void updateAndPublishHeightMap(GpuMat latestDepthImage, CameraIntrinsics depthIntrinsics, ReferenceFrame cameraFrame, ReferenceFrame cameraZUpFrame)
    {
       // Update the sensor origin here with the latest reference frame
-      RigidBodyTransform transformToWorldFrame = cameraFrame.getTransformToWorldFrame();
       RigidBodyTransform heightMapFrameToWorldFrame = heightMapCenter.getTransformToWorldFrame();
-      Point3D sensorOrigin = new Point3D(transformToWorldFrame.getTranslation());
       Point3D heightMapCenterOrigin = new Point3D(heightMapFrameToWorldFrame.getTranslation());
 
       updateInternal(latestDepthImage, depthIntrinsics, cameraFrame, cameraZUpFrame, heightMapCenterOrigin);
@@ -96,7 +92,6 @@ public void updateAndPublishHeightMap(GpuMat latestDepthImage, CameraIntrinsics
       publishHeightMap(hostGlobalHeightMap, heightMapCenterOrigin);
 
       hostGlobalHeightMap.close();
-      deviceGlobalHeightMap.close();
    }
 
    private void publishHeightMap(Mat hostGlobalHeightMap, Point3D heightMapCenterOrigin)
@@ -109,15 +104,11 @@ private void publishHeightMap(Mat hostGlobalHeightMap, Point3D heightMapCenterOr
       FramePose3D cameraPose = new FramePose3D();
       cameraPose.getTranslation().set(gridCellLocation);
 
-      HeightMapTools.convertToHeightMapData(hostGlobalHeightMap,
-                                            latestHeightMapData,
-                                            gridCellLocation,
-                                            (float) heightMapParameters.getGlobalWidthInMeters(),
-                                            (float) heightMapParameters.getCellSizeInMeters(),
-                                            heightMapParameters);
-
-      HeightMapMessage heightMapMessage = new HeightMapMessage();
-      HeightMapMessageTools.toMessage(latestHeightMapData, heightMapMessage);
+      HeightMapMessageTools.toMessage(hostGlobalHeightMap,
+                                      heightMapMessage,
+                                      gridCellLocation,
+                                      heightMapParameters.getGlobalWidthInMeters(),
+                                      heightMapParameters.getCellSizeInMeters());
       sequenceId++;
       heightMapMessage.setSequenceId(sequenceId);
       heightMapMessagePublisher.publish(heightMapMessage);
diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/heightMap/HeightMapTools.java b/ihmc-perception/src/main/java/us/ihmc/perception/heightMap/HeightMapTools.java
@@ -1,6 +1,5 @@
 package us.ihmc.perception.heightMap;
 
-import org.bytedeco.opencv.global.opencv_core;
 import org.bytedeco.opencv.opencv_core.Mat;
 import us.ihmc.commons.InterpolationTools;
 import us.ihmc.euclid.tuple3D.Point3D;
@@ -10,17 +9,17 @@
 /**
  * Height map indexing tools. The height map spans a square region and is parametrized by the following values:
  * - A discretization value
- * - The grid size, i.e. side length of the square region it covers
- * - Grid center, an xy coordinate which is the middle of the grid
- *
+ * - The grid size, i.e., side length of the square region it covers
+ * - a Grid center, a xy coordinate which is the middle of the grid
+ * <p>
  * Cells are indexed two ways:
- * - A unique integer key, which is zero-indexed and starts at the corner of the grid which is the negative-most x and y coordinates.
+ * - A unique integer key, which is zero-indexed and starts in the corner of the grid which is the negative-most x and y coordinates.
  * - An (x,y) integer index pair, which is zero at the negative-most cell along each axis
  */
 public class HeightMapTools
 {
    /**
-    * The xy-indices of the center of the grid.
+    * The xy-indices of the center, of the grid.
     */
    public static int computeCenterIndex(double gridSize, double resolution)
    {
@@ -87,7 +86,7 @@ public static double getCoordinateFromIndex(int index, double resolution, int of
    public static double[] getRedGreenBlue(double height)
    {
       // Using interpolation between key color points
-      double r = 0, g = 0, b = 0;
+      double r, g, b;
       double magentaR = 1.0, magentaG = 0.0, magentaB = 1.0;
       double orangeR = 1.0, orangeG = 200.0 / 255.0, orangeB = 0.0;
       double yellowR = 1.0, yellowG = 1.0, yellowB = 0.0;
@@ -160,7 +159,8 @@ public static void convertHeightMapDataToMat(Mat heightMapToPack, HeightMapData
             int colMajorIndex = col * cellsPerAxis + row;
 
             // Get the height as for row major, and save it as column major
-            short height = (short) (((float) heightsAsDoubles[rowMajorIndex] + (float) heightMapParameters.getHeightOffset()) * heightMapParameters.getHeightScaleFactor());
+            short height = (short) (((float) heightsAsDoubles[rowMajorIndex] + (float) heightMapParameters.getHeightOffset())
+                                    * heightMapParameters.getHeightScaleFactor());
             heightsAsFloats[colMajorIndex] = height;
          }
       }
diff --git a/ihmc-perception/src/test/java/us/ihmc/perception/heightmap/HeightMapToolsTest.java b/ihmc-perception/src/test/java/us/ihmc/perception/heightmap/HeightMapToolsTest.java
@@ -79,7 +79,7 @@ public void testSpeedConvertMatToHeightMapData()
 
       System.out.printf("Average time per pack: %.3f ms%n", averageTimePerIteration);
    }
-   
+
    @Test
    public void testIndexing()
    {

Original file line number	Diff line number	Diff line change
`@@ -542,7 +542,7 @@ private void deallocateFloatPointer(FloatPointer hostPointer, Pointer devicePoin`
`542`	`542`
`543`	`543`	`public GpuMat getHeightMap()`
`544`	`544`	`{`
`545`		`- return scaledHeightMap.clone();`
	`545`	`+ return globalMeanMap;`
`546`	`546`	`}`
`547`	`547`
`548`	`548`	`public GpuMat getTerrainCroppedHeightMap()`
Original file line number	Diff line number	Diff line change
`@@ -79,7 +79,7 @@ public void testSpeedConvertMatToHeightMapData()`
`79`	`79`
`80`	`80`	`System.out.printf("Average time per pack: %.3f ms%n", averageTimePerIteration);`
`81`	`81`	`}`
`82`		`-`
	`82`	`+`
`83`	`83`	`@Test`
`84`	`84`	`public void testIndexing()`
`85`	`85`	`{`