107 lines
2.6 KiB
Python
Executable File
107 lines
2.6 KiB
Python
Executable File
#!/usr/bin/env python
|
|
|
|
import numpy as np
|
|
import math
|
|
import sys
|
|
import cv2
|
|
|
|
# copied from common.transformations/camera.py
|
|
eon_focal_length = 910.0 # pixels
|
|
eon_dcam_focal_length = 860.0 # pixels
|
|
|
|
# adjust these for your specific camera specs if known
|
|
cam_width = 1280
|
|
cam_height = 720
|
|
cam_fov = 80
|
|
cam_focal_length = cam_width/(2*math.tan(cam_fov/2/180*math.pi))
|
|
|
|
# EON intrinsics
|
|
eon_intrinsics = np.array([
|
|
[eon_focal_length, 0., 1164/2.],
|
|
[ 0., eon_focal_length, 874/2.],
|
|
[ 0., 0., 1.]])
|
|
|
|
# TODO: implement driver cam
|
|
eon_dcam_intrinsics = np.array([
|
|
[eon_dcam_focal_length, 0, 1152/2.],
|
|
[ 0, eon_dcam_focal_length, 864/2.],
|
|
[ 0, 0, 1]])
|
|
|
|
final_warp = []
|
|
|
|
if __name__ == "__main__":
|
|
in_cmd = sys.argv
|
|
cam_number = 0
|
|
|
|
if (len(in_cmd) < 3) or ("-h" in in_cmd)or ("--help" in in_cmd):
|
|
print("USAGE: ./warp_vis.py [camera_number] [path_to_png.png]")
|
|
print("-h or --help to display this dialogue")
|
|
exit()
|
|
else:
|
|
print("CONTROLS:\n\
|
|
q - zoom in\n\
|
|
e - zoom out\n\
|
|
w - shift up in\n\
|
|
s - shift down\n\
|
|
a - shift left\n\
|
|
d - shift right\n\
|
|
x - exit and print warp mesh\n\
|
|
")
|
|
cam_number = in_cmd[1]
|
|
background = cv2.imread(in_cmd[2])
|
|
|
|
cap = cv2.VideoCapture("/dev/video" + cam_number)
|
|
cap.set(cv2.CAP_PROP_FRAME_WIDTH, cam_width)
|
|
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, cam_height)
|
|
|
|
while (True):
|
|
|
|
k = cv2.waitKey(1)
|
|
|
|
if (k == ord('x')):
|
|
break
|
|
if (k == ord('q')):
|
|
cam_focal_length += 1
|
|
if (k == ord('e')):
|
|
cam_focal_length -= 1
|
|
if (k == ord('a')):
|
|
cam_width += 1
|
|
if (k == ord('d')):
|
|
cam_width -= 1
|
|
if (k == ord('w')):
|
|
cam_height += 1
|
|
if (k == ord('s')):
|
|
cam_height -= 1
|
|
|
|
ret, img = cap.read()
|
|
|
|
if ret:
|
|
cam_intrinsics = np.array([
|
|
[cam_focal_length, 0., cam_width/2.],
|
|
[ 0., cam_focal_length, cam_height/2.],
|
|
[ 0., 0., 1]])
|
|
rear_warp = np.dot(eon_intrinsics, np.linalg.inv(cam_intrinsics))
|
|
|
|
cv2.imshow('original',img)
|
|
img = cv2.warpPerspective(img, rear_warp, (1164,874), borderMode=cv2.BORDER_CONSTANT, borderValue=0)
|
|
# img = cv2.addWeighted(background,0.8,img,1,0)
|
|
cv2.imshow('preview', img)
|
|
|
|
print("FINAL WARP: ")
|
|
final_warp = rear_warp.flatten()
|
|
|
|
# string format to make copying into camera_webcam.cc easier
|
|
warp_string = " float ts[9] = {"
|
|
cnt_fmt = 0
|
|
for item in final_warp:
|
|
warp_string += str(item)
|
|
cnt_fmt += 1
|
|
if cnt_fmt != 9:
|
|
if cnt_fmt % 3 == 0:
|
|
warp_string += ",\n "
|
|
else:
|
|
warp_string += ", "
|
|
warp_string += "};"
|
|
|
|
print(warp_string)
|