Feature/height map noise hacking (#1119)

* Add option to parameters to tune the minimum acceptable depth values

* Tuned value

* Update transform and parameter for swing

---------

Co-authored-by: ihmc-rosie-windows <rosie@ihmc.org>

Author: PotatoPeeler3000

ihmc-perception/src/main/generated-java/us/ihmc/perception/gpuMapping/HeightMapParameters.java

@@ -22,6 +22,7 @@ public class HeightMapParameters extends StoredPropertySet implements HeightMapP
    public static final BooleanStoredPropertyKey flyingPointsFilter = keys.addBooleanKey("Flying points filter");
    public static final BooleanStoredPropertyKey enableChunkedMap = keys.addBooleanKey("Enable chunked map");
    public static final BooleanStoredPropertyKey logHeightMap = keys.addBooleanKey("Log height map");
+   public static final DoubleStoredPropertyKey minDepthToAccept = keys.addDoubleKey("Min Depth To Accept");
    public static final DoubleStoredPropertyKey minHeightRegistration = keys.addDoubleKey("Min height registration");
    public static final DoubleStoredPropertyKey maxHeightRegistration = keys.addDoubleKey("Max height registration");
    public static final DoubleStoredPropertyKey kalmanFilterPredictionNoise = keys.addDoubleKey("Kalman filter prediction noise");

ihmc-perception/src/main/generated-java/us/ihmc/perception/gpuMapping/HeightMapParametersBasics.java

@@ -28,6 +28,11 @@ default void setLogHeightMap(boolean logHeightMap)
       set(HeightMapParameters.logHeightMap, logHeightMap);
    }
 
+   default void setMinDepthToAccept(double minDepthToAccept)
+   {
+      set(HeightMapParameters.minDepthToAccept, minDepthToAccept);
+   }
+
    default void setMinHeightRegistration(double minHeightRegistration)
    {
       set(HeightMapParameters.minHeightRegistration, minHeightRegistration);

ihmc-perception/src/main/generated-java/us/ihmc/perception/gpuMapping/HeightMapParametersReadOnly.java

@@ -30,6 +30,11 @@ default boolean getLogHeightMap()
       return get(logHeightMap);
    }
 
+   default double getMinDepthToAccept()
+   {
+      return get(minDepthToAccept);
+   }
+
    default double getMinHeightRegistration()
    {
       return get(minHeightRegistration);

ihmc-perception/src/main/java/us/ihmc/perception/gpuMapping/HeightMapExtractor.java

@@ -418,7 +418,8 @@ public float[] populateParameterArray(HeightMapParameters parameters, CameraIntr
                           (float) parameters.getVariancePerMeter(),
                           (float) parameters.getVariancePerTranslationSpeed(),
                           (float) parameters.getVariancePerRotationSpeed(),
-                          (float) groundHeightGuess};
+                          (float) groundHeightGuess,
+                          (float) parameters.getMinDepthToAccept()};
    }
 
    public HeightMapData getHeightMapData()

ihmc-perception/src/main/resources/us/ihmc/perception/gpuMapping/HeightMapExtractor.cu

@@ -21,6 +21,7 @@ extern "C"
 #define VARIANCE_PER_TRANSLATION_SPEED 16
 #define VARIANCE_PER_ROTATION_SPEED 17
 #define GROUND_HEIGHT 18
+#define MIN_DEPTH_TO_ACCEPT 19
 
 __device__ float3 back_project_perspective(int2 pos, float Z, const float *params)
 {
@@ -69,7 +70,7 @@ __global__ void heightMapUpdateDataKernel(const unsigned short* __restrict__ dep
     float depth = rowPtr[xIndex] * 0.001f; // Scale to meters
 
     // Early exit for invalid depth
-    if (depth < 0.52f)
+    if (depth < params[MIN_DEPTH_TO_ACCEPT])
         return;
 
     // Back-project and transform

ihmc-perception/src/main/resources/us/ihmc/perception/gpuMapping/HeightMapParameters.json

@@ -4,6 +4,11 @@
   "Flying points filter": false,
   "Enable chunked map": false,
   "Log height map": false,
+  "Min Depth To Accept": {
+    "value": 0.88,
+    "lowerBound": 0.0,
+    "upperBound": 2.0
+  },
   "Min height registration": {
     "value": -1.5,
     "lowerBound": -10.0,

open-alexander/src/main/java/us/ihmc/openAlexander/AlexanderSensorInformation.java

@@ -39,8 +39,8 @@ public class AlexanderSensorInformation implements HumanoidRobotSensorInformatio
    private static final RigidBodyTransform D457_TO_CHEST_TRANSFORM = new RigidBodyTransform();
    static
    {
-      D457_TO_CHEST_TRANSFORM.getTranslation().set(0.081,  0.09,  0.087);
-      EuclidCoreMissingTools.setYawPitchRollDegrees(D457_TO_CHEST_TRANSFORM.getRotation(), 0.0, 28.0, 0.0);
+      D457_TO_CHEST_TRANSFORM.getTranslation().set(0.081,  0.04,  0.087);
+      EuclidCoreMissingTools.setYawPitchRollDegrees(D457_TO_CHEST_TRANSFORM.getRotation(), -0.8, 44.0, -1.0);
    }
 
    protected final SideDependentList<String> feetForceSensorNames = new SideDependentList<String>();

open-alexander/src/main/java/us/ihmc/openAlexander/parameters/controller/AlexanderSwingTrajectoryParameters.java

@@ -102,7 +102,7 @@ public double getDefaultSwingStepDownHeight()
    @Override
    public double getMinHeightDifferenceForStepUpOrDown()
    {
-      return 0.1;
+      return 0.05;
    }
 
    @Override