From e6013d37486d7a587ad290e0ee2b8b6aad8a4764 Mon Sep 17 00:00:00 2001 From: "A. Sriram" Date: Tue, 9 Apr 2024 19:33:34 -0400 Subject: [PATCH] uploaded distance tracking --- pp/__pycache__/config.cpython-39.pyc | Bin 0 -> 315 bytes pp/config.py | 6 ++ pp/distance_measurement.py | 94 +++++++++++++++++++++++++++ pp/main.py | 92 ++++++++++++++++++++++++++ 4 files changed, 192 insertions(+) create mode 100644 pp/__pycache__/config.cpython-39.pyc create mode 100644 pp/config.py create mode 100644 pp/distance_measurement.py create mode 100644 pp/main.py diff --git a/pp/__pycache__/config.cpython-39.pyc b/pp/__pycache__/config.cpython-39.pyc new file mode 100644 index 0000000000000000000000000000000000000000..65502e604ebc0ff4f50a5a65382cf34e5efc69de GIT binary patch literal 315 zcmYe~<>g`k0_T9Msf&R0V-N=!FatRbKwNA9BvKes7;_k+7*d#08L}8f7*d#}8B$mf z>@=n*rWDpF<`lLlmK4Td22J*tKwS)e=^hWOMAoT$IlSBg$cB-uUxR#WWB|bn^=-@i>V;I%vU$CdYUvPYYXN0Ry@GV8CjEAeIyGKa8qmzG_YrK1qf2f~JysvABYf$h?h9VB2 z3&6xLRcEW1(Bjmh;+Vvwq>RiMm;Ca)oczR;;+TShnB@Gtw9It9g34PQHo5sJr8%i~ RKvyypvjGVXW*J5nMgVxcQyBmN literal 0 HcmV?d00001 diff --git a/pp/config.py b/pp/config.py new file mode 100644 index 0000000..b885a3e --- /dev/null +++ b/pp/config.py @@ -0,0 +1,6 @@ +import math + + +CAMERA_FOV_RADIANS = (52.31 * math.pi / 180, 51.83 * math.pi / 180) +CAMERA_DIMENSIONS_PIXELS = (640, 480) +CAMERA_HEIGHT_ABOVE_GROUND_METERS = 0.263 diff --git a/pp/distance_measurement.py b/pp/distance_measurement.py new file mode 100644 index 0000000..417096a --- /dev/null +++ b/pp/distance_measurement.py @@ -0,0 +1,94 @@ +import numpy as np +import matplotlib.pyplot as plt +import math +import cv2 +from typing import List, Tuple + +CAMERA_FOV_RADIANS = (52.31 * math.pi / 180, 51.83 * math.pi / 180) +CAMERA_DIMENSIONS_PIXELS = (640, 480) +CAMERA_HEIGHT_ABOVE_GROUND_METERS = 0.263 + + +def conversion(x: int, c: int, theta: float): + return math.atan2((x - c / 2) * math.tan(theta / 2), c / 2) + + +def get_point_angle(point: Tuple[int, int], camera_dimension: Tuple[int, int], camera_fov: Tuple[float, float]) -> Tuple[float, float]: + return (conversion(point[0], camera_dimension[0], camera_fov[0]), + conversion(point[1], camera_dimension[1], camera_fov[1])) + + +def bounding_box_angle(box: List[Tuple[int, int]], camera_dimension: Tuple[int, int], camera_fov: Tuple[float, float]) -> List[Tuple[float, float]]: + return [get_point_angle(point, camera_dimension, camera_fov) for point in box] + + +def distance_estimation(angles: Tuple[float, float], h: float) -> Tuple[float, float]: + l = h / math.tan(angles[1]) + w = l * math.tan(angles[0]) + return w, l + + +def main(): + vid = cv2.VideoCapture(0) + + p = (1, 1) + target = 0.0775 + differential = 0.0001 + + while True: + ret, frame = vid.read() + if not ret: + print("Failed to capture frame") + break + + l, r, m = 0, math.pi, 0 + + while (r - l) > differential: + m = (l + r) / 2 + + m_angles = bounding_box_angle( + [p] * 4, + CAMERA_DIMENSIONS_PIXELS, (m, CAMERA_FOV_RADIANS[1]))[0] + + distances = distance_estimation(m_angles, CAMERA_HEIGHT_ABOVE_GROUND_METERS) + + distance = distances[0] + + if distance > target: + r = m - differential + elif distance < target: + l = m + differential + else: + break + + # Convert distance to string for display + distance_str = "Distance: {:.2f} meters".format(distance) + + # Convert pixel measurements to string for display + pixel_measure_str = "Pixel measurement: {} pixels".format(p) + + # Update text on the frame + cv2.putText(frame, distance_str, (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2) + cv2.putText(frame, pixel_measure_str, (20, 80), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2) + + # Display the frame + cv2.imshow('Webcam Feed', frame) + + # Wait for a small amount of time (1 millisecond) + # If 'q' is pressed, exit the loop + if cv2.waitKey(1) & 0xFF == ord('q'): + break + + # Read the next frame + ret, frame = vid.read() + + # Release the video capture object and close all windows + vid.release() + cv2.destroyAllWindows() + + +if __name__ == "__main__": + main() + +if __name__ == "__main__": + main() diff --git a/pp/main.py b/pp/main.py new file mode 100644 index 0000000..956026f --- /dev/null +++ b/pp/main.py @@ -0,0 +1,92 @@ +import numpy as np +import matplotlib.pyplot as plt +import math +import cv2 +from typing import List, Tuple + + + +CAMERA_FOV_RADIANS = (52.31 * math.pi / 180, 51.83 * math.pi / 180) +CAMERA_DIMENSIONS_PIXELS = (640, 480) +CAMERA_HEIGHT_ABOVE_GROUND_METERS = 0.263 + + + +def conversion(x: int, c: int, theta: float): + return math.atan2((x - c / 2) * math.tan(theta / 2), c / 2) + + +# width, height +def get_point_angle(point: Tuple[int, int], camera_dimension: Tuple[int, int], camera_fov: Tuple[float, float]) -> Tuple[float, float]: + + + return (conversion(point[0], camera_dimension[0], camera_fov[0]), + conversion(point[1], camera_dimension[1], camera_fov[1])) + + +def bounding_box_angle(box: List[Tuple[int, int]], camera_dimension: Tuple[int, int], camera_fov: Tuple[float, float]) -> List[Tuple[float, float]]: + return [get_point_angle(point, camera_dimension, camera_fov) for point in box] + + +def distance_estimation(angles: Tuple[float, float], h: float) -> Tuple[float, float]: + l = h / math.tan(angles[1]) + w = l * math.tan(angles[0]) + + return w, l + + +# 26.5cm + 35.3cm = 61.8cm + +def main(): + p = (400, 446) + + l, r, m = 0, math.pi, 0 + + # distance should be (0.0775, .835) + # calculated fov : (, 45.55) degrees + target = 0.0775 + differential = 0.0001 + + while (r - l) > differential: + m = (l + r) / 2 + + m_angles = bounding_box_angle( + [p] * 4, + CAMERA_DIMENSIONS_PIXELS, (m, CAMERA_FOV_RADIANS[1]))[0] + + distances = distance_estimation(m_angles, CAMERA_HEIGHT_ABOVE_GROUND_METERS) + + distance = distances[0] + + print(l, r, m, distances) + + if distance > target: + r = m - differential + elif distance < target: + l = m + differential + else: + break + + print(m) + + + + + vid = cv2.VideoCapture(0) + # + ret, frame = vid.read() + # + plt.imshow(frame) + plt.plot([0, CAMERA_DIMENSIONS_PIXELS[0]], [p[1]]*2, color='white', linewidth=3) + plt.plot([p[0]]*2, [0, CAMERA_DIMENSIONS_PIXELS[1]], color='white', linewidth=3) + # + plt.show() + # + print(frame.shape) + # + vid.release() + cv2.destroyAllWindows() + + +if __name__ == "__main__": + main()