diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java index c351bfc3786..f4cb8c3156c 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java @@ -7,6 +7,7 @@ import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; import us.ihmc.perception.StandAloneRealsenseProcess; +import us.ihmc.perception.gpuHeightMap.SnappingTerrainExtractor; import us.ihmc.robotics.physics.RobotCollisionModel; import us.ihmc.ros2.ROS2Node; import us.ihmc.ros2.ROS2NodeBuilder; @@ -26,6 +27,8 @@ public class ContinuousHikingProcess private final ContinuousPlannerSchedulingTask continuousPlannerSchedulingTask; private final StandAloneRealsenseProcess standAloneRealsenseProcess; + private final SnappingTerrainExtractor snappingTerrainExtractor; + public ContinuousHikingProcess(DRCRobotModel robotModel, RobotCollisionModel robotCollisionModel) { ROS2Node ros2Node = new ROS2NodeBuilder().build("nadia_terrain_perception_node"); @@ -41,8 +44,9 @@ public ContinuousHikingProcess(DRCRobotModel robotModel, RobotCollisionModel rob ControllerFootstepQueueMonitor controllerFootstepQueueMonitor = new ControllerFootstepQueueMonitor(ros2Node, robotModel.getSimpleRobotName()); - environmentHandler = new EnvironmentHandler(); activeMappingParameterToolBox = new ActiveMappingParameterToolBox(ros2Node, robotModel, "ForContinuousWalking"); + environmentHandler = new EnvironmentHandler(); + snappingTerrainExtractor = new SnappingTerrainExtractor(activeMappingParameterToolBox.getHeightMapParameters()); standAloneRealsenseProcess = new StandAloneRealsenseProcess(ros2Node, ros2Helper, syncedRobot, @@ -69,10 +73,13 @@ public void update() activeMappingParameterToolBox.update(); if (standAloneRealsenseProcess.getLatestHeightMapData() != null) + { environmentHandler.setHeightMapData(standAloneRealsenseProcess.getLatestHeightMapData()); + snappingTerrainExtractor.update(environmentHandler.getHeightMapData()); + } - if (standAloneRealsenseProcess.getLatestTerrainMapData() != null) - environmentHandler.setTerrainMapData(standAloneRealsenseProcess.getLatestTerrainMapData()); + if (snappingTerrainExtractor.getTerrainMapData() != null) + environmentHandler.setTerrainMapData(snappingTerrainExtractor.getTerrainMapData()); continuousPlannerSchedulingTask.setLatestEnvironmentHandler(environmentHandler); } @@ -80,6 +87,7 @@ public void update() public void destroy() { continuousPlannerSchedulingTask.destroy(); + snappingTerrainExtractor.close(); standAloneRealsenseProcess.destroy(); } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapAutonomyUpdateThread.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapAutonomyUpdateThread.java index 234751817cf..c77086a987c 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapAutonomyUpdateThread.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapAutonomyUpdateThread.java @@ -87,14 +87,6 @@ public HeightMapData getLatestHeightMapData() } } - public TerrainMapData getLatestTerrainMapData() - { - synchronized (heightMapLock) - { - return heightMapManager.getTerrainMapData(); - } - } - @Override public void kill() { diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapThread.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapThread.java index 112ded3eb42..7f9744fcf08 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapThread.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapThread.java @@ -94,14 +94,6 @@ public HeightMapData getLatestHeightMapData() } } - public TerrainMapData getLatestTerrainMapData() - { - synchronized (heightMapLock) - { - return heightMapManager.getTerrainMapData(); - } - } - public void destroy() { scheduler.shutdown(); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java index 7ef74f7e3d7..aa7446651f8 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java @@ -8,7 +8,6 @@ import us.ihmc.communication.ros2.ROS2Helper; import us.ihmc.communication.ros2.ROS2TunedRigidBodyTransform; import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; -import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.robotics.physics.RobotCollisionModel; import us.ihmc.ros2.ROS2Node; import us.ihmc.sensorProcessing.heightMap.HeightMapData; @@ -96,11 +95,6 @@ public HeightMapData getLatestHeightMapData() return rapidHeightMapThread.getLatestHeightMapData(); } - public TerrainMapData getLatestTerrainMapData() - { - return rapidHeightMapThread.getLatestTerrainMapData(); - } - public void destroy() { rapidHeightMapThread.destroy(); diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java index de9ae44a181..ad24b645fc1 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java @@ -35,8 +35,7 @@ public class RapidHeightMapExtractorCUDA private final GpuMat globalHeightMapImage; private final GpuMat terrainCostImage; private final GpuMat contactMapImage; - private final GpuMat sensorCroppedHeightMapImage; - private final GpuMat terrainHeightMapImage; + private final GpuMat croppedHeightMapImage; private final GpuMat emptyGlobalHeightMapImage; private final CUDAProgram heightMapCUDAProgram; @@ -65,11 +64,9 @@ public class RapidHeightMapExtractorCUDA private float gridOffsetX; private int centerIndex; private int cellsPerAxisLocal; - private int cellsPerAxisTerrain; private int globalCenterIndex; private int croppedCenterIndex; private int cellsPerAxisCropped; - private int terrainCenterIndex; private int globalCellsPerAxis; private dim3 blockSize; private dim3 updateKernelGridDim; @@ -117,8 +114,7 @@ public RapidHeightMapExtractorCUDA(int mode, HeightMapParameters heightMapParame globalHeightMapImage = new GpuMat(globalCellsPerAxis, globalCellsPerAxis, opencv_core.CV_16UC1); terrainCostImage = new GpuMat(globalCellsPerAxis, globalCellsPerAxis, opencv_core.CV_8UC1); contactMapImage = new GpuMat(globalCellsPerAxis, globalCellsPerAxis, opencv_core.CV_8UC1); - sensorCroppedHeightMapImage = new GpuMat(cellsPerAxisCropped, cellsPerAxisCropped, opencv_core.CV_16UC1); - terrainHeightMapImage = new GpuMat(cellsPerAxisTerrain, cellsPerAxisTerrain, opencv_core.CV_16UC1); + croppedHeightMapImage = new GpuMat(cellsPerAxisCropped, cellsPerAxisCropped, opencv_core.CV_16UC1); emptyGlobalHeightMapImage = new GpuMat(globalCellsPerAxis, globalCellsPerAxis, opencv_core.CV_16UC1); @@ -149,9 +145,6 @@ private void recomputeDerivedParameters() globalCenterIndex = HeightMapTools.computeCenterIndex(heightMapParameters.getInternalGlobalWidthInMeters(), heightMapParameters.getCellSizeInMeters()); globalCellsPerAxis = 2 * globalCenterIndex + 1; - terrainCenterIndex = HeightMapTools.computeCenterIndex(heightMapParameters.getTerrainWidthInMeters(), heightMapParameters.getCellSizeInMeters()); - cellsPerAxisTerrain = 2 * terrainCenterIndex + 1; - croppedCenterIndex = HeightMapTools.computeCenterIndex(heightMapParameters.getCroppedWidthInMeters(), heightMapParameters.getCellSizeInMeters()); cellsPerAxisCropped = 2 * croppedCenterIndex + 1; @@ -269,11 +262,9 @@ public void update(GpuMat latestDepthImageGPU, // Run the cropping kernel croppingKernel.withPointer(globalHeightMapImage.data()).withLong(globalHeightMapImage.step()); - croppingKernel.withPointer(sensorCroppedHeightMapImage.data()).withLong(sensorCroppedHeightMapImage.step()); - croppingKernel.withPointer(terrainHeightMapImage.data()).withLong(terrainHeightMapImage.step()); + croppingKernel.withPointer(croppedHeightMapImage.data()).withLong(croppedHeightMapImage.step()); croppingKernel.withPointer(parametersDevicePointer); croppingKernel.withInt(cellsPerAxisCropped); - croppingKernel.withInt(terrainHeightMapImage.rows()); error = cudaStreamSynchronize(stream); CUDATools.checkCUDAError(error); @@ -377,7 +368,6 @@ public float[] populateParameterArray(HeightMapParameters parameters, (float) parameters.getSearchWindowHeight(), (float) parameters.getSearchWindowWidth(), (float) croppedCenterIndex, - (float) terrainCenterIndex, (float) parameters.getMinClampHeight(), (float) parameters.getMaxClampHeight(), (float) parameters.getHeightOffset(), @@ -418,8 +408,7 @@ public void destroy() globalHeightMapImage.close(); terrainCostImage.close(); contactMapImage.close(); - sensorCroppedHeightMapImage.close(); - terrainHeightMapImage.close(); + croppedHeightMapImage.close(); filteredRapidHeightMapExtractor.destroy(); verticalSurfacesExtractor.destroy(); @@ -449,13 +438,8 @@ public int getSequenceNumber() return sequenceNumber; } - public GpuMat getTerrainHeightMapImage() - { - return terrainHeightMapImage; - } - - public GpuMat getVisualizedHeightMap() + public GpuMat getCroppedHeightMap() { - return sensorCroppedHeightMapImage.clone(); + return croppedHeightMapImage.clone(); } } 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 index cf2b169930b..1c864862f68 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java @@ -18,7 +18,6 @@ import us.ihmc.perception.camera.CameraIntrinsics; import us.ihmc.perception.cuda.CUDABodyCollisionFilter; import us.ihmc.perception.filters.CUDAFlyingPointsFilter; -import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.perception.opencv.OpenCVTools; import us.ihmc.perception.tools.PerceptionMessageTools; import us.ihmc.robotModels.FullHumanoidRobotModel; @@ -28,6 +27,7 @@ import us.ihmc.scs2.simulation.collision.Collidable; import us.ihmc.sensorProcessing.heightMap.HeightMapData; import us.ihmc.sensorProcessing.heightMap.HeightMapParameters; +import us.ihmc.sensorProcessing.heightMap.HeightMapTools; import java.time.Instant; import java.util.ArrayList; @@ -40,7 +40,6 @@ public class RapidHeightMapManager { private final HeightMapParameters heightMapParameters; private final RapidHeightMapExtractorCUDA rapidHeightMapExtractor; - private final SnappingTerrainExtractor snappedFootstepsExtractor; private final Point3D sensorOrigin = new Point3D(); private final FramePose3D cameraPose = new FramePose3D(); @@ -56,6 +55,7 @@ public class RapidHeightMapManager private final ROS2Publisher heightMapPublisher; private final ImageMessage croppedHeightMapImageMessage = new ImageMessage(); private final BytePointer compressedCroppedHeightMapPointer = new BytePointer(); + private final GpuMat deviceCroppedHeightMap; public RapidHeightMapManager(ROS2Node ros2Node, RobotCollisionModel robotCollisionModel, @@ -76,8 +76,10 @@ public RapidHeightMapManager(ROS2Node ros2Node, rapidHeightMapDriftOffset = new RapidHeightMapDriftOffset(controllerFootstepQueueMonitor); flyingPointsFilter = new CUDAFlyingPointsFilter(); + int croppedCenterIndex = HeightMapTools.computeCenterIndex(heightMapParameters.getCroppedWidthInMeters(), heightMapParameters.getCellSizeInMeters()); + int cellsPerAxisCropped = 2 * croppedCenterIndex + 1; + deviceCroppedHeightMap = new GpuMat(cellsPerAxisCropped, cellsPerAxisCropped, opencv_core.CV_16UC1); rapidHeightMapExtractor = new RapidHeightMapExtractorCUDA(1, heightMapParameters); - snappedFootstepsExtractor = new SnappingTerrainExtractor(heightMapParameters); // We use a notification to only call resetting the height map in one place heightMapPublisher = ros2Node.createPublisher(PerceptionAPI.HEIGHT_MAP_CROPPED); @@ -96,7 +98,7 @@ public RapidHeightMapManager(ROS2Node ros2Node, } } - public void updateAndPublishHeightMap(RawImage depthImage, ReferenceFrame cameraFrame, ReferenceFrame cameraZUpFrame) throws Exception + public void updateAndPublishHeightMap(RawImage depthImage, ReferenceFrame cameraFrame, ReferenceFrame cameraZUpFrame) { RawImage depthImageCopy = depthImage.get(); Mat latestDepthImage = depthImageCopy.getCpuImageMat(); @@ -104,7 +106,6 @@ public void updateAndPublishHeightMap(RawImage depthImage, ReferenceFrame camera CameraIntrinsics depthIntrinsicsCopy = depthImageCopy.getIntrinsicsCopy(); // -------- Update the Height Map with the latest depth image from the sensor -------------- - GpuMat deviceCroppedHeightMap = new GpuMat(); updateInternal(latestDepthImage, deviceCroppedHeightMap, depthIntrinsicsCopy, cameraFrame, cameraZUpFrame); // Publish the height map to anyone who is subscribing @@ -121,16 +122,18 @@ public void updateAndPublishHeightMap(RawImage depthImage, ReferenceFrame camera hostCroppedHeightMap.cols(), (float) heightMapParameters.getHeightScaleFactor()); - deviceCroppedHeightMap.close(); hostCroppedHeightMap.close(); depthImageCopy.release(); } + /** + * Update the Height Map with the latest depth image from the sensor + */ private void updateInternal(Mat latestDepthImage, GpuMat deviceHeightMapToPack, CameraIntrinsics depthIntrinsicsCopy, ReferenceFrame cameraFrame, - ReferenceFrame cameraZUpFrame) throws Exception + ReferenceFrame cameraZUpFrame) { // Option that gets triggered from a message sent from the user if (lowerHeightMapBackdropRequested.poll()) @@ -194,7 +197,7 @@ private void updateInternal(Mat latestDepthImage, sensorOrigin, computeFootHeight()); - GpuMat deviceCroppedHeightMap = rapidHeightMapExtractor.getVisualizedHeightMap(); + GpuMat deviceCroppedHeightMap = rapidHeightMapExtractor.getCroppedHeightMap(); // We have used the depth image without the robot, close this to avoid creating a memory leak depthImageWithoutRobot.close(); @@ -209,30 +212,24 @@ private void updateInternal(Mat latestDepthImage, // Don't close this mat as its being used in the extractor till that finish's deviceCroppedHeightMap.convertTo(deviceHeightMapToPack, deviceCroppedHeightMap.type()); - - // Now extract the maps to be used in the footstep planning algorithm - snappedFootstepsExtractor.update(rapidHeightMapExtractor.getTerrainHeightMapImage(), sensorOrigin); - } - - public TerrainMapData getTerrainMapData() - { - return snappedFootstepsExtractor.getTerrainMapData(); } public HeightMapData getLatestHeightMapData() { HeightMapData latestHeightMapData = new HeightMapData((float) heightMapParameters.getCellSizeInMeters(), - (float) heightMapParameters.getTerrainWidthInMeters(), + (float) heightMapParameters.getCroppedWidthInMeters(), sensorOrigin.getX(), sensorOrigin.getY()); - Mat heightMapMat = getTerrainMapData().getHeightMap(); - PerceptionMessageTools.convertToHeightMapData(heightMapMat, + Mat heightMap = new Mat(); + deviceCroppedHeightMap.download(heightMap); + PerceptionMessageTools.convertToHeightMapData(heightMap, latestHeightMapData, sensorOrigin, - (float) heightMapParameters.getTerrainWidthInMeters(), + (float) heightMapParameters.getCroppedWidthInMeters(), (float) heightMapParameters.getCellSizeInMeters(), heightMapParameters); + heightMap.close(); return latestHeightMapData; } @@ -257,8 +254,8 @@ private double computeFootHeight() public void destroy() { rapidHeightMapExtractor.destroy(); - snappedFootstepsExtractor.close(); flyingPointsFilter.destroy(); + deviceCroppedHeightMap.close(); compressedCroppedHeightMapPointer.close(); } } \ No newline at end of file diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractor.java index 59b55bb2872..17847a1b2f7 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractor.java @@ -6,14 +6,16 @@ import org.bytedeco.opencv.global.opencv_core; import org.bytedeco.opencv.opencv_core.GpuMat; import org.bytedeco.opencv.opencv_core.Mat; -import us.ihmc.euclid.tuple3D.Point3D; -import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.euclid.tuple2D.Point2D; +import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; import us.ihmc.perception.cuda.CUDAKernel; import us.ihmc.perception.cuda.CUDAProgram; import us.ihmc.perception.cuda.CUDAStreamManager; import us.ihmc.perception.cuda.CUDATools; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.perception.steppableRegions.SteppableRegionCalculatorParameters; +import us.ihmc.perception.tools.PerceptionMessageTools; +import us.ihmc.sensorProcessing.heightMap.HeightMapData; import us.ihmc.sensorProcessing.heightMap.HeightMapParameters; import us.ihmc.sensorProcessing.heightMap.HeightMapTools; @@ -130,16 +132,22 @@ public SnappingTerrainExtractor(HeightMapParameters heightMapParameters) */ private void computeDerivedParameters() { - int terrainCenterIndex = HeightMapTools.computeCenterIndex(heightMapParameters.getTerrainWidthInMeters(), heightMapParameters.getCellSizeInMeters()); + int terrainCenterIndex = HeightMapTools.computeCenterIndex(heightMapParameters.getCroppedWidthInMeters(), heightMapParameters.getCellSizeInMeters()); cellsPerAxisTerrain = 2 * terrainCenterIndex + 1; } - public void update(GpuMat terrainHeightMap, Point3D sensorOrigin) + public void update(HeightMapData heightMapData) { int error; + Point2D gridCenter = heightMapData.getGridCenter(); + + Mat heightMap = PerceptionMessageTools.convertHeightMapDataToMat(heightMapData, heightMapParameters); + GpuMat gpuHeightMap = new GpuMat(); + gpuHeightMap.upload(heightMap); + // Populate parameters buffer for the snapping kernel - float[] snappingParametersArray = populateSnappingParametersArray(sensorOrigin); + float[] snappingParametersArray = populateSnappingParametersArray(gridCenter); snappingParametersHostPointer.put(snappingParametersArray); // Handle memory allocation and copy values to the GPU @@ -148,7 +156,7 @@ public void update(GpuMat terrainHeightMap, Point3D sensorOrigin) checkCUDAError(); // Pass all the parameters to the kernel so that its setup to run correctly - snappingTerrainKernel.withPointer(terrainHeightMap.data()).withLong(terrainHeightMap.step()); + snappingTerrainKernel.withPointer(gpuHeightMap.data()).withLong(gpuHeightMap.step()); snappingTerrainKernel.withPointer(steppabilityMat.data()).withLong(steppabilityMat.step()); snappingTerrainKernel.withPointer(snapHeightMat.data()).withLong(snapHeightMat.step()); snappingTerrainKernel.withPointer(snapNormalXMat.data()).withLong(snapNormalXMat.step()); @@ -168,7 +176,7 @@ public void update(GpuMat terrainHeightMap, Point3D sensorOrigin) // --------------------- Run additional kernels for even more data to be used with the terrain map ------------------ - terrainCostKernel.withPointer(terrainHeightMap.data()).withLong(terrainHeightMap.step()); + terrainCostKernel.withPointer(gpuHeightMap.data()).withLong(gpuHeightMap.step()); terrainCostKernel.withPointer(terrainCostMat.data()).withLong(terrainCostMat.step()); terrainCostKernel.withPointer(snappingParametersDevicePointer); @@ -205,7 +213,7 @@ public void update(GpuMat terrainHeightMap, Point3D sensorOrigin) checkCUDAError(); // Update the terrain map data with the new results - terrainMapData.setSensorOrigin(sensorOrigin.getX(), sensorOrigin.getY()); + terrainMapData.setSensorOrigin(gridCenter.getX(), gridCenter.getY()); // This has to be done because we start to download to the CPU, so the data on the GPU needs to be finalized error = cudaStreamSynchronize(stream); @@ -223,7 +231,7 @@ public void update(GpuMat terrainHeightMap, Point3D sensorOrigin) cpuContactMap.close(); Mat cpuHeightMap = new Mat(); - terrainHeightMap.download(cpuHeightMap); + gpuHeightMap.download(cpuHeightMap); terrainMapData.setHeightMap(cpuHeightMap); cpuHeightMap.close(); @@ -285,12 +293,12 @@ private void checkCUDAError() * @param gridCenter is the location of the sensor origin, that will be the center of our map * @return a float array with the parameters for the snapping kernels. The order matters as it needs to match the order things are defined by in the kernel */ - public float[] populateSnappingParametersArray(Tuple3DReadOnly gridCenter) + public float[] populateSnappingParametersArray(Tuple2DReadOnly gridCenter) { return new float[] {(float) gridCenter.getX(), (float) gridCenter.getY(), (float) heightMapParameters.getCellSizeInMeters(), - (float) heightMapParameters.getTerrainWidthInMeters(), + (float) heightMapParameters.getCroppedWidthInMeters(), (float) heightMapParameters.getHeightScaleFactor(), (float) heightMapParameters.getHeightOffset(), (float) steppableRegionParameters.getFootLength(), diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java b/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java index 561950f50ba..98f02b65811 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java @@ -435,8 +435,8 @@ public static void convertToHeightMapData(Mat heightMapPointer, public static Mat convertHeightMapDataToMat(HeightMapData heightMapData, HeightMapParameters heightMapParameters) { - int centerIndex = HeightMapTools.computeCenterIndex(heightMapParameters.getTerrainWidthInMeters(), heightMapParameters.getCellSizeInMeters()); - int cellsPerAxis = 2 * centerIndex + 1; + int cellsPerAxis = heightMapData.getCellsPerAxis(); + int centerIndex = heightMapData.getCenterIndex(); // Create a new Mat object to hold the height map data Mat heightMapMat = new Mat(cellsPerAxis, cellsPerAxis, opencv_core.CV_16UC1); diff --git a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.cu b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.cu index 20cc668b058..855a0cd637c 100644 --- a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.cu +++ b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.cu @@ -28,19 +28,18 @@ extern "C" #define SEARCH_WINDOW_HEIGHT 23 #define SEARCH_WINDOW_WIDTH 24 #define CROPPED_WINDOW_CENTER_INDEX 25 -#define TERRAIN_WINDOW_CENTER_INDEX 26 -#define MIN_CLAMP_HEIGHT 27 -#define MAX_CLAMP_HEIGHT 28 -#define HEIGHT_OFFSET 29 -#define STEPPING_COSINE_THRESHOLD 30 -#define STEPPING_CONTACT_THRESHOLD 31 -#define CONTACT_WINDOW_SIZE 32 -#define SPATIAL_ALPHA 33 -#define SEARCH_SKIP_SIZE 34 -#define VERTICAL_SEARCH_SIZE 35 -#define VERTICAL_SEARCH_RESOLUTION 36 -#define FAST_SEARCH_SIZE 37 -#define GROUND_HEIGHT 38 +#define MIN_CLAMP_HEIGHT 26 +#define MAX_CLAMP_HEIGHT 27 +#define HEIGHT_OFFSET 28 +#define STEPPING_COSINE_THRESHOLD 29 +#define STEPPING_CONTACT_THRESHOLD 30 +#define CONTACT_WINDOW_SIZE 31 +#define SPATIAL_ALPHA 32 +#define SEARCH_SKIP_SIZE 33 +#define VERTICAL_SEARCH_SIZE 34 +#define VERTICAL_SEARCH_RESOLUTION 35 +#define FAST_SEARCH_SIZE 36 +#define GROUND_HEIGHT 37 #define VERTICAL_FOV 1.5707963267948966f #define HORIZONTAL_FOV 6.2831853f @@ -419,8 +418,7 @@ __global__ void heightMapRegistrationKernel(unsigned short *localMap, size_t pit extern "C" __global__ void croppingKernel(unsigned short *inputMap, size_t pitchInput, unsigned short *croppedMap, size_t pitchCropped, - unsigned short *terrainObjectMap, size_t pitchTerrain, - float *params, int croppedMapXY, int terrainObjectMapXY) + float *params, int croppedMapXY) { int xIndex = blockIdx.x * blockDim.x + threadIdx.x; int yIndex = blockIdx.y * blockDim.y + threadIdx.y; @@ -453,32 +451,6 @@ extern "C" __global__ void croppingKernel(unsigned short *inputMap, size_t pitch unsigned short *croppedRow = (unsigned short *)((char *)croppedMap + xIndex * pitchCropped); croppedRow[yIndex] = 0; // Assign 0 for out-of-bounds cells } - - if (xIndex >= terrainObjectMapXY || yIndex >= terrainObjectMapXY) - return; - - globalSensorIndex = coordinate_to_indices( - make_float2(params[HEIGHT_MAP_CENTER_X], params[HEIGHT_MAP_CENTER_Y]), - make_float2(0.0f, 0.0f), - params[GLOBAL_CELL_SIZE], - static_cast(params[GLOBAL_CENTER_INDEX])); - - globalCellIndexX = globalSensorIndex.x + xIndex - (params[TERRAIN_WINDOW_CENTER_INDEX]); - globalCellIndexY = globalSensorIndex.y + yIndex - (params[TERRAIN_WINDOW_CENTER_INDEX]); - - // Check if global cell index is within bounds - if (globalCellIndexX >= 0 && globalCellIndexX < globalMapSizeX && - globalCellIndexY >= 0 && globalCellIndexY < globalMapSizeY) - { - unsigned short *inputRow = (unsigned short *)((char *)inputMap + globalCellIndexX * pitchInput); - unsigned short *visualizedRow = (unsigned short *)((char *)terrainObjectMap + xIndex * pitchTerrain); - visualizedRow[yIndex] = inputRow[globalCellIndexY]; - } - else - { - unsigned short *visualizedRow = (unsigned short *)((char *)terrainObjectMap + xIndex * pitchTerrain); - visualizedRow[yIndex] = 0; // Assign 0 for out-of-bounds cells - } } extern "C" diff --git a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractor.cu b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractor.cu index 3e6ab10f0d0..93467cbfaee 100644 --- a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractor.cu +++ b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractor.cu @@ -6,7 +6,7 @@ extern "C" #define HEIGHT_MAP_CENTER_X 0 #define HEIGHT_MAP_CENTER_Y 1 #define CELL_SIZE_IN_CENTIMETERS 2 -#define TERRAIN_WIDTH_IN_METERS 3 +#define HEIGHT_MAP_WIDTH_IM_METERS 3 #define HEIGHT_SCALING_FACTOR 4 #define HEIGHT_OFFSET 5 #define FOOT_LENGTH 6 @@ -60,7 +60,7 @@ __global__ void computeTerrainData(unsigned short *heightMap, size_t pitchHeight float map_resolution = params[CELL_SIZE_IN_CENTIMETERS]; float max_dimension = fmaxf(params[FOOT_WIDTH], params[FOOT_LENGTH]); - int terrain_map_center_index = compute_center_index(params[TERRAIN_WIDTH_IN_METERS], map_resolution); + int terrain_map_center_index = compute_center_index(params[HEIGHT_MAP_WIDTH_IM_METERS], map_resolution); float2 terrain_map_center = make_float2(params[HEIGHT_MAP_CENTER_Y], params[HEIGHT_MAP_CENTER_X]); int cells_per_axis = 2 * terrain_map_center_index + 1; @@ -322,7 +322,7 @@ __global__ void computeSteppabilityConnections(unsigned short *steppableMap, siz int x_index = blockIdx.x * blockDim.x + threadIdx.x; int y_index = blockIdx.y * blockDim.y + threadIdx.y; - int cells_per_side = 2 * compute_center_index(params[TERRAIN_WIDTH_IN_METERS], params[CELL_SIZE_IN_CENTIMETERS]) + 1; + int cells_per_side = 2 * compute_center_index(params[HEIGHT_MAP_WIDTH_IM_METERS], params[CELL_SIZE_IN_CENTIMETERS]) + 1; int2 key = make_int2(x_index, y_index); @@ -377,7 +377,7 @@ __global__ void computeTerrainCost(unsigned short *heightMap, size_t pitchHeight { int xIndex = blockIdx.x * blockDim.x + threadIdx.x; int yIndex = blockIdx.y * blockDim.y + threadIdx.y; - int cells_per_axis = 2 * compute_center_index(params[TERRAIN_WIDTH_IN_METERS], params[CELL_SIZE_IN_CENTIMETERS]) + 1; + int cells_per_axis = 2 * compute_center_index(params[HEIGHT_MAP_WIDTH_IM_METERS], params[CELL_SIZE_IN_CENTIMETERS]) + 1; // Bounds check if (xIndex >= cells_per_axis || yIndex >= cells_per_axis) @@ -452,7 +452,7 @@ __global__ void computeContactMap(unsigned char *terrainCost, size_t pitchTerrai { int xIndex = blockIdx.x * blockDim.x + threadIdx.x; int yIndex = blockIdx.y * blockDim.y + threadIdx.y; - int cells_per_axis = 2 * compute_center_index(params[TERRAIN_WIDTH_IN_METERS], params[CELL_SIZE_IN_CENTIMETERS]) + 1; + int cells_per_axis = 2 * compute_center_index(params[HEIGHT_MAP_WIDTH_IM_METERS], params[CELL_SIZE_IN_CENTIMETERS]) + 1; if (xIndex >= cells_per_axis || yIndex >= cells_per_axis) return; diff --git a/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractorTest.java b/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractorTest.java index a84fe0d10ee..1ac22843b1d 100644 --- a/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractorTest.java +++ b/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractorTest.java @@ -2,10 +2,13 @@ import org.bytedeco.opencv.global.opencv_core; import org.bytedeco.opencv.opencv_core.GpuMat; +import org.bytedeco.opencv.opencv_core.Mat; import org.bytedeco.opencv.opencv_core.Scalar; import org.junit.jupiter.api.Disabled; import org.junit.jupiter.api.Test; import us.ihmc.euclid.tuple3D.Point3D; +import us.ihmc.perception.tools.PerceptionMessageTools; +import us.ihmc.sensorProcessing.heightMap.HeightMapData; import us.ihmc.sensorProcessing.heightMap.HeightMapParameters; public class SnappingTerrainExtractorTest @@ -21,13 +24,27 @@ public void testSnappingTerrainKernelRuns() HeightMapParameters heightMapParameters = new HeightMapParameters(); SnappingTerrainExtractor snappingTerrainExtractor = new SnappingTerrainExtractor(heightMapParameters); - GpuMat fakeHeightMap = new GpuMat(500, 500, opencv_core.CV_16UC1); + GpuMat fakeHeightMap = new GpuMat(401, 401, opencv_core.CV_16UC1); fakeHeightMap.setTo(new Scalar(100)); + Mat heightMap = new Mat(401, 401, opencv_core.CV_8UC1); + fakeHeightMap.download(heightMap); Point3D heightMapCenter = new Point3D(); heightMapCenter.set(new Point3D(0.0, 0.0, 0.0)); - snappingTerrainExtractor.update(fakeHeightMap, heightMapCenter); + HeightMapData heightMapData = new HeightMapData((float) heightMapParameters.getCellSizeInMeters(), + (float) heightMapParameters.getCroppedWidthInMeters(), + 0, + 0); + + PerceptionMessageTools.convertToHeightMapData(heightMap, + heightMapData, + new Point3D(0.0, 0.0, 0.0), + (float) 4.0, + 0.02F, + heightMapParameters); + + snappingTerrainExtractor.update(heightMapData); snappingTerrainExtractor.close(); } } diff --git a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java index a45442e3624..45c282150b1 100644 --- a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java +++ b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java @@ -36,7 +36,6 @@ public class HeightMapParameters extends StoredPropertySet implements HeightMapP public static final DoubleStoredPropertyKey maxClampHeight = keys.addDoubleKey("Max Clamp Height"); public static final DoubleStoredPropertyKey cellSizeInMeters = keys.addDoubleKey("Cell size in meters"); public static final DoubleStoredPropertyKey localWidthInMeters = keys.addDoubleKey("Local width in meters"); - public static final DoubleStoredPropertyKey terrainWidthInMeters = keys.addDoubleKey("Terrain width in meters"); public static final DoubleStoredPropertyKey croppedWidthInMeters = keys.addDoubleKey("Cropped width in meters"); public static final DoubleStoredPropertyKey internalGlobalWidthInMeters = keys.addDoubleKey("Internal global width in meters"); public static final DoubleStoredPropertyKey robotCollisionCylinderRadius = keys.addDoubleKey("Robot collision cylinder radius"); diff --git a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java index 9c999f5f342..b89bfa09544 100644 --- a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java +++ b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java @@ -98,11 +98,6 @@ default void setLocalWidthInMeters(double localWidthInMeters) set(HeightMapParameters.localWidthInMeters, localWidthInMeters); } - default void setTerrainWidthInMeters(double terrainWidthInMeters) - { - set(HeightMapParameters.terrainWidthInMeters, terrainWidthInMeters); - } - default void setCroppedWidthInMeters(double croppedWidthInMeters) { set(HeightMapParameters.croppedWidthInMeters, croppedWidthInMeters); diff --git a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java index 79d8d2ec687..9d44e634b87 100644 --- a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java +++ b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java @@ -100,11 +100,6 @@ default double getLocalWidthInMeters() return get(localWidthInMeters); } - default double getTerrainWidthInMeters() - { - return get(terrainWidthInMeters); - } - default double getCroppedWidthInMeters() { return get(croppedWidthInMeters); diff --git a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json index 592068af0bd..b6ccb2c8370 100644 --- a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json +++ b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json @@ -74,13 +74,8 @@ "lowerBound": 1.0, "upperBound": 4.5 }, - "Terrain width in meters": { - "value": 4.0, - "lowerBound": 2.0, - "upperBound": 8.0 - }, "Cropped width in meters": { - "value": 6.0, + "value": 4.0, "lowerBound": 2.0, "upperBound": 8.0 }, diff --git a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json index f43c80a27d4..01d3cf23e0c 100644 --- a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json +++ b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json @@ -5,7 +5,7 @@ "Enable alpha filter": true, "Enable vertical filter": true, "Search window height": { - "value": 400, + "value": 500, "lowerBound": 100, "upperBound": 800 }, @@ -74,13 +74,8 @@ "lowerBound": 1.0, "upperBound": 4.5 }, - "Terrain width in meters": { - "value": 4.0, - "lowerBound": 2.0, - "upperBound": 8.0 - }, "Cropped width in meters": { - "value": 6.0, + "value": 4.0, "lowerBound": 2.0, "upperBound": 8.0 },