From 3d0782ca3ca65bf46f26ad89e9716726a0e0236e Mon Sep 17 00:00:00 2001 From: nkitchel Date: Thu, 11 Dec 2025 12:45:06 -0600 Subject: [PATCH] Fix the bug where the live view of the camera would also adjust because of the drift status message. (we don't want to adjust the live view) --- .../gpuMapping/HeightMapExtractor.java | 11 +++++------ .../perception/gpuMapping/HeightMapExtractor.cu | 16 ++++++++++++---- 2 files changed, 17 insertions(+), 10 deletions(-) diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuMapping/HeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuMapping/HeightMapExtractor.java index 2e1b8b61177..37fc62658d6 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuMapping/HeightMapExtractor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuMapping/HeightMapExtractor.java @@ -72,8 +72,8 @@ public class HeightMapExtractor private final FloatPointer parametersHostPointer; private final FloatPointer parametersDevicePointer; - private int centerIndex; - private int cellsPerAxis; + private final int centerIndex; + private final int cellsPerAxis; private final RigidBodyTransform previousSensorToWorld = new RigidBodyTransform(); private int previousCellX; @@ -160,7 +160,6 @@ public void reset(double footHeight, float loweredValue) resetOffset -= loweredValue; globalMeanMap.setTo(new Scalar(resetOffset)); - emptyGlobalHeightMap.setTo(new Scalar(resetOffset)); } public void update(GpuMat latestDepthImageGPU, @@ -315,19 +314,19 @@ public void update(GpuMat latestDepthImageGPU, } } + float zeroValueForEmptySpaces = 0.0f; // ---------- Run the registration kernel for an empty global height map ---------- { int emptyRegistrationGridSizeXY = (cellsPerAxis + BLOCK_SIZE_XY - 1) / BLOCK_SIZE_XY; dim3 registerKernelGridDim = new dim3(emptyRegistrationGridSizeXY, emptyRegistrationGridSizeXY, 1); // Need to reset the empty global map before using it so when its filled it starts with all "zero" values - emptyGlobalHeightMap.setTo(new Scalar(resetOffset)); + emptyGlobalHeightMap.setTo(new Scalar(zeroValueForEmptySpaces)); emptyRegisterKernel.withPointer(localMeanMap.data()).withLong(localMeanMap.step()); emptyRegisterKernel.withPointer(emptyGlobalHeightMap.data()).withLong(emptyGlobalHeightMap.step()); emptyRegisterKernel.withPointer(zUpCameraToWorldAlignedGroundDevicePointer); emptyRegisterKernel.withPointer(parametersDevicePointer); - emptyRegisterKernel.withFloat(resetOffset); emptyRegisterKernel.run(stream, registerKernelGridDim, blockSize, 0); @@ -344,7 +343,7 @@ public void update(GpuMat latestDepthImageGPU, planOffsetKernel.withPointer(globalMeanMap.data()).withLong(globalMeanMap.step()); planOffsetKernel.withPointer(emptyGlobalHeightMap.data()).withLong(emptyGlobalHeightMap.step()); planOffsetKernel.withFloat(zDriftInMeters); - planOffsetKernel.withFloat(resetOffset); + planOffsetKernel.withFloat(zeroValueForEmptySpaces); planOffsetKernel.withPointer(parametersDevicePointer); planOffsetKernel.run(stream, planOffsetKernelGridDim, blockSize, 0); diff --git a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuMapping/HeightMapExtractor.cu b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuMapping/HeightMapExtractor.cu index b95ac6b9b98..8a9de82699c 100644 --- a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuMapping/HeightMapExtractor.cu +++ b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuMapping/HeightMapExtractor.cu @@ -277,7 +277,7 @@ extern "C" __global__ void heightMapEmptyRegistrationKernel(float *localMap, size_t pitchLocal, float *globalMap, size_t pitchGlobal, float *zUpCameraToWorldAlignedGround, - float *params, float resetOffset) + float *params) { int xIndex = blockIdx.x * blockDim.x + threadIdx.x; int yIndex = blockIdx.y * blockDim.y + threadIdx.y; @@ -297,6 +297,7 @@ __global__ void heightMapEmptyRegistrationKernel(float *localMap, size_t pitchLo float *localHeight = (float *)((char *)localMap + localIndex.x * pitchLocal) + localIndex.y; + // This is a way of checking if we have any read data in the spot, its very unlikely that any real data is 0.0 if (*localHeight == 0.0f) return; @@ -305,9 +306,17 @@ __global__ void heightMapEmptyRegistrationKernel(float *localMap, size_t pitchLo } extern "C" +/** + @brief Compute Plan Offset KERNEL: This kernel is not very intuitive. For background we've got a status message + being published that gives us an offset in Z in meters that the robot thinks its drifted by. + Our goal is to update the height map accordingly. However, it doesn't make sense to update any values that are + in the live view of the camera, because: a) they will just get overwritten the next image, and b) that data doesn't drift + because its live data. So we pass in this matrix of values to skip because those values are outside the live view + of the camera. +*/ __global__ void planOffsetKernel(float *matrixToModify, size_t pitchMatrixToModify, float *matrixValuesToSkip, size_t pitchMatrixValuesToSkip, - float offsetInZ, float resetOffset, float *params) + float offsetInZ, float zeroValueForEmptySpaces, float *params) { int indexX = blockIdx.x * blockDim.x + threadIdx.x; int indexY = blockIdx.y * blockDim.y + threadIdx.y; @@ -318,9 +327,8 @@ __global__ void planOffsetKernel(float *matrixToModify, size_t pitchMatrixToModi return; float *skipRow = (float *)((char *)matrixValuesToSkip + indexX * pitchMatrixValuesToSkip); - // This is less then or equal to due to a round error that can give +- 1 offsets // This skips the cells that have real data in them coming from the values to skip - if (fabsf(skipRow[indexY] - resetOffset) >= 2.0f) + if (skipRow[indexY] != zeroValueForEmptySpaces) return; float *matrixRow = (float *)((char *)matrixToModify + indexX * pitchMatrixToModify);