Fix face detection out-of-bounds bug

ada_feeding_perception/ada_feeding_perception/face_detection.py

@@ -327,10 +327,27 @@ def depth_callback(self, msg: CompressedImage):
                     desired_encoding="passthrough",
                 )
 
-                # Retrieve the depth value averaged over all mouth coordinates
+                # Retrieve the depth value averaged over all mouth coordinates. Note that
+                # if the detected face is partially out of the camera frame, the detected
+                # mouth coordinates may be out of bounds of the depth image.
                 depth_sum = 0
+                num_points_out_of_bounds = 0
                 for point in img_mouth_points:
-                    depth_sum += closest_depth[int(point[1])][int(point[0])]
+                    x, y = int(point[0]), int(point[1])
+                    if (
+                        x < 0
+                        or x >= closest_depth.shape[1]
+                        or y < 0
+                        or y >= closest_depth.shape[0]
+                    ):
+                        num_points_out_of_bounds += 1
+                        continue
+                    depth_sum += closest_depth[y, x]
+                # If more than half of the points we are using to determine depth are out-of-bounds
+                # we won't have a reliable depth estimate. In this case, we will not publish a
+                # mouth location.
+                if num_points_out_of_bounds > 0.5 * len(img_mouth_points):
+                    return
                 depth = depth_sum / float(len(img_mouth_points))
 
                 # Create target 3d point, with mm measurements converted to m