From 9bec35b9484a01c2acf7b8770fb4c03bed35e7fc Mon Sep 17 00:00:00 2001 From: nkitchel Date: Mon, 24 Feb 2025 15:12:04 -0600 Subject: [PATCH] Cleanup code mainly for height map filter --- .../CommunicationsSyncedRobotModel.java | 5 --- .../RDXHighLevelDepthSensorSimulator.java | 6 ++-- .../RapidHeightMapUpdateThread.java | 2 +- .../FilteredRapidHeightMapExtractor.cu | 35 ------------------- 4 files changed, 4 insertions(+), 44 deletions(-) diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java index 8139c9dc2ce..e4cea554c9a 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java +++ b/ihmc-avatar-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 getHandWrenchCalculators() { return handWrenchCalculators; diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java index 36de7bb4023..60cf189d907 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java @@ -234,6 +234,9 @@ public void setupForROS2ImageMessages(ROS2Node ros2Node, ROS2Topic { 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 index b2fa79a438d..baaf42479a3 100644 --- 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/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu index 43be54370c3..8974b9b4ef0 100644 --- a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu +++ b/ihmc-perception/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); } -