{"cells":[{"metadata":{"_uuid":"8f2839f25d086af736a60e9eeb907d3b93b6e0e5","_cell_guid":"b1076dfc-b9ad-4769-8c92-a6c4dae69d19","trusted":true},"cell_type":"code","source":"import numpy as np # linear algebra\nimport pandas as pd # data processing, CSV file I/O (e.g. pd.read_csv)\nimport cv2\nimport os\nimport json\nfrom math import sin, cos\nfrom scipy.spatial.transform import Rotation as R\nimport matplotlib.pyplot as plt","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"im_color = cv2.applyColorMap(np.arange(256).astype('uint8') , cv2.COLORMAP_HSV)[:,0,:]\nCV_PI = np.pi\n\ndef rotateImage(img, alpha=0, beta=0, gamma=0):\n    fx, dx = 2304.5479, 1686.2379\n    fy, dy = 2305.8757, 1354.9849\n    # get width and height for ease of use in matrices\n    h, w = img.shape[:2]\n    # Projection 2D -> 3D matrix\n    A1 = np.array([[1/fx, 0, -dx/fx],\n                   [0, 1/fx, -dy/fx],\n                   [0, 0,    1],\n                   [0, 0,    1]])\n    \n    # Rotation matrices around the X, Y, and Z axis\n    RX = np.array([[1,          0,           0, 0],\n             [0, cos(alpha), -sin(alpha), 0],\n             [0, sin(alpha),  cos(alpha), 0],\n             [0,          0,           0, 1]])\n    \n    RY = np.array([[cos(beta), 0, -sin(beta), 0],\n              [0, 1,          0, 0],\n              [sin(beta), 0,  cos(beta), 0],\n              [0, 0,          0, 1]])\n    RZ = np.array([[cos(gamma), -sin(gamma), 0, 0],\n              [sin(gamma),  cos(gamma), 0, 0],\n              [0,          0,           1, 0],\n              [0,          0,           0, 1]])\n    # Composed rotation matrix with (RX, RY, RZ)\n    R = np.dot(RZ, np.dot(RX, RY))\n\n    # 3D -> 2D matrix\n    A2 = np.array([[fx, 0, dx, 0],\n                   [0, fy, dy, 0],\n                   [0, 0,   1, 0]])\n    # Final transformation matrix\n    trans = np.dot(A2,np.dot(R, A1))\n    # Apply matrix transformation\n    return cv2.warpPerspective(img, trans, (w,h), flags=cv2.INTER_LANCZOS4), trans, R\n\ndef draw_car(yaw, pitch, roll, x, y, z, overlay, color=(0,0,255)):\n    yaw, pitch, roll = -pitch, -yaw, -roll\n    Rt = np.eye(4)\n    t = np.array([x, y, z])\n    Rt[:3, 3] = t\n    Rt[:3, :3] = euler_to_Rot(yaw, pitch, roll).T\n    Rt = Rt[:3, :]\n    P = np.ones((vertices.shape[0],vertices.shape[1]+1))\n    P[:, :-1] = vertices\n    P = P.T\n    img_cor_points = np.dot(k, np.dot(Rt, P))\n    img_cor_points = img_cor_points.T\n    img_cor_points[:, 0] /= img_cor_points[:, 2]\n    img_cor_points[:, 1] /= img_cor_points[:, 2]\n    draw_obj(overlay, img_cor_points, triangles, color)\n    return overlay\n\ndef draw_obj(image, vertices, triangles, color):\n    for t in triangles:\n        coord = np.array([vertices[t[0]][:2], vertices[t[1]][:2], vertices[t[2]][:2]], dtype=np.int32)\n#         cv2.fillConvexPoly(image, coord, (0,0,255))\n        cv2.polylines(image, np.int32([coord]), 1, color)\n\n# Load a 3D model of a car\nwith open('../input/pku-autonomous-driving/car_models_json/mazida-6-2015.json') as json_file:\n    data = json.load(json_file)\nvertices = np.array(data['vertices'])\nvertices[:, 1] = -vertices[:, 1]\ntriangles = np.array(data['faces']) - 1\n\ndef euler_to_Rot(yaw, pitch, roll):\n    Y = np.array([[cos(yaw), 0, sin(yaw)],\n                  [0, 1, 0],\n                  [-sin(yaw), 0, cos(yaw)]])\n    P = np.array([[1, 0, 0],\n                  [0, cos(pitch), -sin(pitch)],\n                  [0, sin(pitch), cos(pitch)]])\n    R = np.array([[cos(roll), -sin(roll), 0],\n                  [sin(roll), cos(roll), 0],\n                  [0, 0, 1]])\n    return np.dot(Y, np.dot(P, R))\n\nk = np.array([[2304.5479, 0,  1686.2379],\n           [0, 2305.8757, 1354.9849],\n           [0, 0, 1]], dtype=np.float32)","execution_count":null,"outputs":[]},{"metadata":{"_uuid":"d629ff2d2480ee46fbb7e2d37f6b5fab8052498a","_cell_guid":"79c7e3d0-c299-4dcb-8224-4455121ee9b0","trusted":true},"cell_type":"code","source":"train = pd.read_csv(\"../input/pku-autonomous-driving/train.csv\")","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def draw_rot(img_name = 'ID_aa6ffba0a', alpha = 0, beta = 0, gamma = 0):\n    img = cv2.imread('../input/pku-autonomous-driving/train_images/%s.jpg'%img_name)[:,:,::-1]\n    pred_string = train[train.ImageId == img_name].PredictionString.iloc[0]\n    items = pred_string.split(' ')\n    items = np.array(items, dtype='float')\n    model_types, yaws, pitches, rolls, xs, ys, zs = [items[i::7] for i in range(7)]\n\n    alpha = alpha*CV_PI/180.\n    beta = beta*CV_PI/180.\n    gamma = gamma*CV_PI/180.\n\n    dst, Mat,Rot = rotateImage(img, alpha, beta, gamma)\n    overlay = np.zeros_like(dst)\n\n    for yaw, pitch, roll, x, y, z in zip(yaws, pitches, rolls, xs, ys, zs):\n\n        x,y,z,_ = np.dot(Rot,[x, y, z, 1])\n\n        r1 = R.from_euler('xyz', [-pitch, -yaw, -roll], degrees=False)\n        r2 = R.from_euler('xyz', [beta, -alpha, -gamma], degrees=False)\n\n        pitch2, yaw2, roll2 = (r2*r1).as_euler('xyz')*(-1)\n\n        color = im_color[np.random.randint(256)].tolist()\n        overlay = draw_car(yaw2, pitch2, roll2, x, y, z, overlay, color)\n\n        #print(np.array([yaw, pitch, roll, yaw2, pitch2, roll2]))\n        #break\n\n    plt.figure(figsize=(20,20))\n    plt.imshow((dst*(np.sum(overlay, axis=-1)[:,:,np.newaxis]==0)+overlay), interpolation='lanczos')","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"#original\ndraw_rot(img_name = 'ID_aa6ffba0a', alpha = 0, beta = 0, gamma = 0)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"draw_rot(img_name = 'ID_aa6ffba0a', alpha = 10, beta = 30, gamma = -10)","execution_count":null,"outputs":[]}],"metadata":{"kernelspec":{"language":"python","display_name":"Python 3","name":"python3"},"language_info":{"pygments_lexer":"ipython3","nbconvert_exporter":"python","version":"3.6.4","file_extension":".py","codemirror_mode":{"name":"ipython","version":3},"name":"python","mimetype":"text/x-python"}},"nbformat":4,"nbformat_minor":1}