[openCV] Object height 알아내기(python)

 

 

카메라의 focal length, principal points, height이 주어졌다는 가정하에, 객체의 지면과의 contact point와 head point를 알고있다면 객체의 높이를 알 수 있다.

 

 

수식은 다음과 같다.

 

 

 

카메라의 외부, 내부 파라미터는 calibration을 수행해서 알아낼 수 있다.

calibration을 통해 알아낸 카메라 파라미터를 통해 객체의 높이를 알아내는 파이썬 코드는 다음과 같다.

 

 

p_x, p_y = pt[0]
p_x1, p_y1 = pt[1]
point_p = [p_x, p_y]
point_p1 = [p_x1, p_y1]

point_n = cv.undistortPoints(np.array([point_p], dtype=np.float64), K, distort).squeeze(axis=1)
point_n1 = cv.undistortPoints(np.array([point_p1], dtype=np.float64), K, distort).squeeze(axis=1)

point_c = np.matmul(cam_ori, np.append(point_n, 1))
point_c1 = np.matmul(cam_ori, np.append(point_n1, 1))

Z = (offset - cam_pos[1]) / point_c[1] * point_c[2]  # L/y_bar
H = (point_c[1] / point_c[2] - point_c1[1] / point_c1[2]) * Z
print(H)

 

 

여기서 contact point는 pt[0]에 해당하고 head point는 pt[1]에 해당한다. 또한 K는 camera matrix, distort는 왜곡 계수를 나타낸다.

 

 

 

 

camera matrix, distortion coefficient를 사용해서 openCV undistortPoints 함수를 통해 [ (x-Cx)/fx,(x-Cy)/fy ] 좌표를 얻어내고 이렇게 구해낸 좌표를 camera orientation matrix R과 행렬곱셈을 한다. 이 값을 아까의 수식에 집어 넣어주면 객체의 높이를 알 수 있다.

 

undistortPoints 함수에 대한 정보는 공식 문서를 참고하면 좋을 것 같다.

https://docs.opencv.org/3.4/da/d54/group__imgproc__transform.html#ga55c716492470bfe86b0ee9bf3a1f0f7e

OpenCV: Geometric Image Transformations

 

 

이미지에 키를 나타내는 코드

import numpy as np
import cv2 as cv

image = cv.imread("이미지이름.jpg")
idx = 0
# pts : head point, contact point가 들어있는 리스트
# ex) points = [[[714, 449], [715, 402]],[[726, 446], [727, 403]],[[737, 447], [737, 399]]]
for pt in pts:
    p_x, p_y = pt[0]
    p_x1, p_y1 = pt[1]
    point_p = [p_x, p_y]
    point_p1 = [p_x1, p_y1]

    point_n = cv.undistortPoints(np.array([point_p], dtype=np.float64), K, distort).squeeze(axis=1)
    point_n1 = cv.undistortPoints(np.array([point_p1], dtype=np.float64), K, distort).squeeze(axis=1)
    point_c = np.matmul(cam_ori, np.append(point_n, 1))
    point_c1 = np.matmul(cam_ori, np.append(point_n1, 1))

    Z = (offset - cam_pos[1]) / point_c[1] * point_c[2]  # L/y_bar
    H = (point_c[1] / point_c[2] - point_c1[1] / point_c1[2]) * Z
    print(H)
    cv.circle(image, point_p, 4, (0, 0, 255), 1)
    cv.circle(image, point_p1, 4, (0, 0, 255), 1)
    px = [point_p[0],round((point_p[1]+point_p1[1])/2)+20*idx]
    cv.putText(image,str(round(H)),px,cv.FONT_HERSHEY_PLAIN, 1, (0,255,0))
    idx += 1

cv.imshow('object height',image)
cv.waitKey(0)
cv.destroyAllWindows()