diff --git a/ada_feeding_perception/ada_feeding_perception/face_detection.py b/ada_feeding_perception/ada_feeding_perception/face_detection.py index 79ccd275..7b4307c9 100755 --- a/ada_feeding_perception/ada_feeding_perception/face_detection.py +++ b/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