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)

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);

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);