{"cells":[{"metadata":{},"cell_type":"markdown","source":"* **Visualize the location and 3d bounding box of the car**"},{"metadata":{"_uuid":"8f2839f25d086af736a60e9eeb907d3b93b6e0e5","_cell_guid":"b1076dfc-b9ad-4769-8c92-a6c4dae69d19","trusted":true,"_kg_hide-output":true},"cell_type":"code","source":"# This Python 3 environment comes with many helpful analytics libraries installed\n# It is defined by the kaggle/python docker image: https://github.com/kaggle/docker-python\n# For example, here's several helpful packages to load in \n\nimport numpy as np # linear algebra\nimport pandas as pd # data processing, CSV file I/O (e.g. pd.read_csv)\n\n# Input data files are available in the \"../input/\" directory.\n# For example, running this (by clicking run or pressing Shift+Enter) will list all files under the input directory\n\nimport os\nfor dirname, _, filenames in os.walk('/kaggle/input'):\n    for filename in filenames:\n        print(os.path.join(dirname, filename))\n\n# Any results you write to the current directory are saved as output.","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"import numpy as np\nimport matplotlib.pylab as plt\nimport pandas as pd\nfrom keras.preprocessing.image import load_img\nfrom math import sin, cos\nfrom PIL import ImageDraw, Image\nimport cv2","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"**Example**"},{"metadata":{"trusted":true},"cell_type":"code","source":"train = pd.read_csv('../input/pku-autonomous-driving/train.csv')\n# k is camera instrinsic matrix\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":{"trusted":true},"cell_type":"code","source":"plt.rcParams[\"axes.grid\"] = False\nimg_name = train.loc[10]['ImageId']\npred_string = train.loc[10]['PredictionString']\nfig, ax = plt.subplots(figsize=(15, 15))\nimg = load_img('../input/pku-autonomous-driving/train_images/' + img_name + '.jpg')\nplt.imshow(img)\nplt.show()","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"items = pred_string.split(' ')\nmodel_types, yaws, pitches, rolls, xs, ys, zs = [items[i::7] for i in range(7)]","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"# convert euler angle to rotation matrix\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))","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def draw_line(image, points):\n    color = (255, 0, 0)\n    cv2.line(image, tuple(points[1][:2]), tuple(points[2][:2]), color, 16)\n    cv2.line(image, tuple(points[1][:2]), tuple(points[4][:2]), color, 16)\n\n    cv2.line(image, tuple(points[1][:2]), tuple(points[5][:2]), color, 16)\n    cv2.line(image, tuple(points[2][:2]), tuple(points[3][:2]), color, 16)\n    cv2.line(image, tuple(points[2][:2]), tuple(points[6][:2]), color, 16)\n    cv2.line(image, tuple(points[3][:2]), tuple(points[4][:2]), color, 16)\n    cv2.line(image, tuple(points[3][:2]), tuple(points[7][:2]), color, 16)\n\n    cv2.line(image, tuple(points[4][:2]), tuple(points[8][:2]), color, 16)\n    cv2.line(image, tuple(points[5][:2]), tuple(points[8][:2]), color, 16)\n\n    cv2.line(image, tuple(points[5][:2]), tuple(points[6][:2]), color, 16)\n    cv2.line(image, tuple(points[6][:2]), tuple(points[7][:2]), color, 16)\n    cv2.line(image, tuple(points[7][:2]), tuple(points[8][:2]), color, 16)\n    return image\n\n\ndef draw_points(image, points):\n    image = np.array(image)\n    for (p_x, p_y, p_z) in points:\n        # print(\"p_x, p_y\", p_x, p_y)\n        cv2.circle(image, (p_x, p_y), 5, (255, 0, 0), -1)\n    return image","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"# image coordinate to world coordinate\ndef img_cor_2_world_cor():\n    x_img, y_img, z_img = img_cor_points[0]\n    xc, yc, zc = x_img*z_img, y_img*z_img, z_img\n    p_cam = np.array([xc, yc, zc])\n    xw, yw, zw = np.dot(np.linalg.inv(k), p_cam)\n    print(xw, yw, zw)\n    print(x, y, z)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"\nx_l = 1.02\ny_l = 0.80\nz_l = 2.31\nfor yaw, pitch, roll, x, y, z in zip(yaws, pitches, rolls, xs, ys, zs):\n    yaw, pitch, roll, x, y, z = [float(x) for x in [yaw, pitch, roll, x, y, z]]\n    # I think the pitch and yaw should be exchanged\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.array([[0, 0, 0, 1],\n                  [x_l, y_l, -z_l, 1],\n                  [x_l, y_l, z_l, 1],\n                  [-x_l, y_l, z_l, 1],\n                  [-x_l, y_l, -z_l, 1],\n                  [x_l, -y_l, -z_l, 1],\n                  [x_l, -y_l, z_l, 1],\n                  [-x_l, -y_l, z_l, 1],\n                  [-x_l, -y_l, -z_l, 1]]).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    # call this function before chage the dtype\n    img_cor_2_world_cor()\n    img_cor_points = img_cor_points.astype(int)\n    img = draw_points(img, img_cor_points)\n    img = draw_line(img, img_cor_points)\n    \nimg = Image.fromarray(img)\nplt.imshow(img)\nplt.show()","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"","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}