Cleanup code mainly for height map filter

ihmcrobotics · Feb 24, 2025 · 9bec35b · 9bec35b
1 parent 5431014
commit 9bec35b
Show file tree

Hide file tree

Showing 4 changed files with 4 additions and 44 deletions.
diff --git a/...atar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java b/...atar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java
@@ -189,11 +189,6 @@ public DRCRobotModel getRobotModel()
       return robotModel;
    }
 
-   public FullHumanoidRobotModel getFullHumanoidRobotModel()
-   {
-      return fullRobotModel;
-   }
-
    public SideDependentList<HandWrenchCalculator> getHandWrenchCalculators()
    {
       return handWrenchCalculators;

diff --git a/...iors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java b/...iors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java
@@ -234,6 +234,9 @@ public void setupForROS2ImageMessages(ROS2Node ros2Node, ROS2Topic<ImageMessage>
    {
       this.ros2Node = ros2Node;
       this.ros2Helper = new ROS2Helper(ros2Node);
+
+      depthImagePublisher = ros2Node.createPublisher(ros2DepthTopic);
+
       this.ros2DepthTopic = ros2DepthTopic;
       this.ros2ColorTopic = ros2ColorTopic;
 
@@ -245,9 +248,6 @@ public void setupForROS2PointCloud(ROS2Node ros2Node, ROS2Topic<?> ros2PointClou
    {
       this.ros2Node = ros2Node;
       this.ros2PointCloudTopic = ros2PointCloudTopic;
-
-      depthImagePublisher = ros2Node.createPublisher(ros2DepthTopic);
-
       ros2PointsToPublish = new RecyclingArrayList<>(imageWidth * imageHeight, Point3D::new);
       pointCloudMessageType = ros2PointCloudTopic.getType();
       if (pointCloudMessageType.equals(StereoVisionPointCloudMessage.class))

diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapUpdateThread.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapUpdateThread.java
@@ -64,7 +64,7 @@ protected void runTask()
          if (heightMapManager == null)
          {
             heightMapManager = new RapidHeightMapManager(ros2Node,
-                                                         syncedRobotModel.getFullHumanoidRobotModel(),
+                                                         syncedRobotModel.getFullRobotModel(),
                                                          syncedRobotModel.getRobotModel().getSimpleRobotName(),
                                                          leftFootFrame,
                                                          rightFootFrame,

diff --git a/...ion/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu b/...ion/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu
@@ -14,7 +14,6 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA,
         return;
 
     int sum = 0;
-    float* variance = (float*)malloc(params[LAYERS] * sizeof(float));
 
     // Compute the average height of the history in order to get the variance at each layer
     for (int layer = 0; layer < params[LAYERS]; layer++)
@@ -28,36 +27,6 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA,
 
     unsigned short avg = sum / params[LAYERS];
 
-    // Compute the variance for each layer
-    for (int layer = 0; layer < params[LAYERS]; layer++)
-    {
-        unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize);
-        unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX;
-
-        float diff = (float)(*matrixPointerRow) - avg;
-        variance[layer] = abs(diff);
-    }
-
-    double heightSum = 0;
-    double varianceSum = 0;
-
-    // Compute the height and variance sum
-    for (int layer = 0; layer < params[LAYERS]; layer++)
-    {
-        unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize);
-        unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX;
-
-        if (variance[layer] > 0.0f)  // Prevent division by zero
-        {
-            heightSum += (double)(*matrixPointerRow) / (double)variance[layer];
-            varianceSum += 1.0 / (double)variance[layer];
-        }
-    }
-
-//     printf("Height sum: %f\n", heightSum);
-//     printf("Variance sum: %f\n", varianceSum);
-    unsigned short newHeight = heightSum / varianceSum;
-
     unsigned short * heightValue = (unsigned short*)((char*) newestHeightMap + indexY * pitchNewestHeightMap) + indexX;
 
     int heightValueInt = (int) *heightValue;
@@ -70,8 +39,4 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA,
 
     unsigned short *outputPointer = (unsigned short *)((char*) resultPointer + indexY * pitchResult) + indexX;
     *outputPointer = (unsigned short) heightEstimate;
-
-    free(variance);
-//     printf("GPU Height Estimate: %f\n", heightEstimate);
 }
-