공까지의 거리 추적하기(3)

2024.07.25(목) 세미나까지 아래의 distance 부분까지 계산한 내용을 발표했다.

참고로,

distance = focal_length * ( actual_area_meters / pixel_area)로 구했다.

머리속 이론이라 틀릴 수도 있지만,,,? 결과는 나름(?)잘 나오니 킵고잉,,,,,ㅎㅡㅎ

 

어쨌든 나는 distance가 아닌 실제로 이동해야할 거리를 구해야하는데,

이 문제는 간단한 삼각비로 해결할 수 있다.(로봇의 목 각도를 내가 알고 있다는 가정하에.)

 

결론은 아래 그림에서의 walk_distance가 필요한데, 계산 공식도 그림에 적었으니 참고 바람

import cv2
import numpy as np

# 모든 길이 단위는 m 기준
actual_diameter_meters = 0.05  # 공의 직경
actual_area_meters = np.pi * (actual_diameter_meters / 2) ** 2  # 공의 면적(직경으로 계산한 값)
focal_length = 403  # 초점 거리 값 # 캘리 매트릭스에서 ((fx+fy)/2)로 평균한 결과 # 509.0532269132008(새거)403.01356509306765(구)
robot_height = 0.4  # 로봇 높이 (실제론 카메라 높이로 변경해야함)
neck_angle = 60  # 임시 목 각도 (degree)

# 빨간 공 탐지 및 거리 추정 함수
def detect_and_calculate_distance(frame):
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    lower_red = np.array([170, 50, 70])
    upper_red = np.array([180, 255, 255])
    mask = cv2.inRange(hsv, lower_red, upper_red)

    contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
    if contours:
        largest_contour = max(contours, key=cv2.contourArea)
        ((x, y), radius) = cv2.minEnclosingCircle(largest_contour)

    if contours:
        largest_radius = 0
        largest_contour = None
        for contour in contours:
            ((x, y), radius) = cv2.minEnclosingCircle(contour)
            if radius > largest_radius:
                largest_radius = radius
                largest_contour = contour

        if largest_radius > 0 and largest_contour is not None:
            center = (int(x), int(y))
            radius = int(largest_radius)
            cv2.circle(frame, center, radius, (0, 255, 0), 2)
            cv2.drawContours(frame, [largest_contour], -1, (255, 0, 0), 2)

            # 중심점 표시는 내가 볼라고
            cv2.circle(frame, center, 5, (0, 0, 255), -1)
            cv2.putText(frame, "Center", (center[0] - 20, center[1] - 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)

            # 면적 계산
            pixel_area = np.pi * (radius ** 2)

            # 거리 계산
            distance = focal_length * np.sqrt(actual_area_meters / pixel_area)

            # walk_dist 계산
            neck_angle_radians = np.radians(neck_angle) # degree to radian
            walk_dist = distance * np.cos(np.pi/2 - neck_angle_radians)

            return frame, distance, center, pixel_area, walk_dist

    return frame, None, None, None, None

cap = cv2.VideoCapture(1)

while True:
    ret, frame = cap.read()
    if not ret:
        break
    
    frame, distance, center, pixel_area, walk_dist = detect_and_calculate_distance(frame)
    if distance:
        cv2.putText(frame, f"Distance: {distance:.2f} m", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
        if center:
            cv2.putText(frame, f"Center: {center}", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
        cv2.putText(frame, f"Area: {pixel_area:.2f} pixels^2", (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
        cv2.putText(frame, f"Walk Dist: {walk_dist:.2f} m", (10, 120), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)

    cv2.imshow("Frame", frame)
    
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

cap.release()
cv2.destroyAllWindows()