{"cells":[{"metadata":{},"cell_type":"markdown","source":"some code is from [here](https://www.kaggle.com/bvictor/pku-data-ground-truth-geometry-analysis)\n\nPlease upvote, if you like it.😄"},{"metadata":{},"cell_type":"markdown","source":"# These are original pose information from train.csv"},{"metadata":{"_uuid":"8f2839f25d086af736a60e9eeb907d3b93b6e0e5","_cell_guid":"b1076dfc-b9ad-4769-8c92-a6c4dae69d19","trusted":true,"_kg_hide-input":true},"cell_type":"code","source":"import numpy as np\nimport pandas as pd\nimport matplotlib.pylab as plt\nfrom math import sin, cos\nimport cv2\nimport os\n\ntrain = pd.read_csv('/kaggle/input/pku-autonomous-driving/train.csv')\n\ncamera_matrix = np.array([[2304.5479, 0,  1686.2379],\n                          [0, 2305.8757, 1354.9849],\n                          [0, 0, 1]], dtype=np.float32)\n\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\n\ndef str2coords(s, names=['id', 'yaw', 'pitch', 'roll', 'x', 'y', 'z']):\n    '''\n    Input:\n        s: PredictionString (e.g. from train dataframe)\n        names: array of what to extract from the string\n    Output:\n        list of dicts with keys from `names`\n    '''\n    coords = []\n    for l in np.array(s.split()).reshape([-1, 7]):\n        coords.append(dict(zip(names, l.astype('float'))))\n        if 'id' in coords[-1]:\n            coords[-1]['id'] = int(coords[-1]['id'])\n    return coords\n\n\ndef get_img_coords(s):\n    '''\n    Input is a PredictionString (e.g. from train dataframe)\n    Output is two arrays:\n        xs: x coordinates in the image\n        ys: y coordinates in the image\n    '''\n    coords = str2coords(s)\n    xs = [c['x'] for c in coords]\n    ys = [c['y'] for c in coords]\n    zs = [c['z'] for c in coords]\n    P = np.array(list(zip(xs, ys, zs))).T\n    img_p = np.dot(camera_matrix, P).T\n    img_p[:, 0] /= img_p[:, 2]\n    img_p[:, 1] /= img_p[:, 2]\n    img_xs = img_p[:, 0]\n    img_ys = img_p[:, 1]\n    return img_xs, img_ys\n\n\ndef draw_points(image, points):\n    for (p_x, p_y, p_z) in points:\n        cv2.circle(image, (p_x, p_y), 10, (0, 255, 0), -1)\n    return image\n\ndef visualize(img, coords, pose_name):\n    x_l = 1.02\n    y_l = 0.80\n    z_l = 2.31\n    \n    img = img.copy()\n    for point in coords:\n        # Get values\n        x, y, z = point['x'], point['y'], point['z']\n        yaw, pitch, roll = point['yaw'], point['pitch'], point['roll']\n        # Math\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([[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                      [0, 0, 0, 1]]).T\n        img_cor_points = np.dot(camera_matrix, 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        img_cor_points = img_cor_points.astype(int)\n        # Drawing\n        img = draw_points(img, img_cor_points[-1:])\n        if pose_name == 'X':\n            cv2.putText(img, str(int(x)), (img_cor_points[-1,0]-60, img_cor_points[-1,1]-20), cv2.FONT_HERSHEY_PLAIN, 6, (0, 255, 0), 3, cv2.LINE_AA)\n        elif pose_name == 'Y':\n            cv2.putText(img, str(int(y)), (img_cor_points[-1,0]-60, img_cor_points[-1,1]-20), cv2.FONT_HERSHEY_PLAIN, 6, (0, 255, 0), 3, cv2.LINE_AA)\n        elif pose_name == 'Z':\n            cv2.putText(img, str(int(z)), (img_cor_points[-1,0]-60, img_cor_points[-1,1]-20), cv2.FONT_HERSHEY_PLAIN, 6, (0, 255, 0), 3, cv2.LINE_AA)\n        elif pose_name == 'Yaw':\n            cv2.putText(img, str(round(yaw,1)), (img_cor_points[-1,0]-60, img_cor_points[-1,1]-20), cv2.FONT_HERSHEY_PLAIN, 6, (0, 255, 0), 3, cv2.LINE_AA)\n        elif pose_name == 'Pitch':\n            cv2.putText(img, str(round(pitch,1)), (img_cor_points[-1,0]-60, img_cor_points[-1,1]-20), cv2.FONT_HERSHEY_PLAIN, 6, (0, 255, 0), 3, cv2.LINE_AA)\n        elif pose_name == 'Roll':\n            cv2.putText(img, str(round(roll,1)), (img_cor_points[-1,0]-60, img_cor_points[-1,1]-20), cv2.FONT_HERSHEY_PLAIN, 6, (0, 255, 0), 3, cv2.LINE_AA)\n        else:\n            print('Please input right pose name (X, Y, Z, Yaw, Pitch, Roll)')\n            \n    return img\n\ndef display_pose(i, pose):\n    img = cv2.imread('/kaggle/input/pku-autonomous-driving/train_images/' + train.iloc[i,0] + '.jpg')\n    img = visualize(img, str2coords(train.iloc[i,1]), pose)\n    img = cv2.cvtColor(img,cv2.COLOR_BGR2RGB)\n    \n    plt.figure(figsize=(20,20))\n    plt.imshow(img)\n    plt.title(pose)\n    plt.show()\n    \ndef display(i):\n    for pose in ['Yaw', 'Pitch', 'Roll', 'X', 'Y', 'Z']:\n        display_pose(i, pose)","execution_count":null,"outputs":[]},{"metadata":{"_uuid":"d629ff2d2480ee46fbb7e2d37f6b5fab8052498a","_cell_guid":"79c7e3d0-c299-4dcb-8224-4455121ee9b0","trusted":true},"cell_type":"code","source":"index = 20\ndisplay(index)","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"It seems the Yaw and Pitch are messed up, but it's right in another perspective.\n\nSee more detail in [here](https://www.kaggle.com/c/pku-autonomous-driving/discussion/123385)."}],"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}