diff --git a/panda b/panda index 7aa6e7ae..d527579b 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 7aa6e7ae1f831b99ba8a94032495882f8cf68ba4 +Subproject commit d527579b41293680ad2124970fbb605b3502072b diff --git a/tools/webcam/warp_vis_interactive.py b/tools/webcam/warp_vis_interactive.py new file mode 100755 index 00000000..d5efd6e7 --- /dev/null +++ b/tools/webcam/warp_vis_interactive.py @@ -0,0 +1,106 @@ +#!/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)