{"cells":[{"metadata":{},"cell_type":"markdown","source":"# Question on ego pose translations"},{"metadata":{"_uuid":"8f2839f25d086af736a60e9eeb907d3b93b6e0e5","_cell_guid":"b1076dfc-b9ad-4769-8c92-a6c4dae69d19","trusted":true},"cell_type":"code","source":"# Operating system\nimport sys\nimport os\nfrom pathlib import Path\n\n# math\nimport numpy as np\n\n# data analysis\nimport pandas as pd\n\n#plotting 2D\nimport matplotlib.pyplot as plt\nfrom matplotlib.axes import Axes\nfrom matplotlib import animation, rc\n","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"\n# Lyft dataset SDK\n!pip install lyft-dataset-sdk\nfrom lyft_dataset_sdk.lyftdataset import LyftDataset\nfrom lyft_dataset_sdk.utils.data_classes import LidarPointCloud, Box, Quaternion\nfrom lyft_dataset_sdk.utils.geometry_utils import view_points, transform_matrix\n","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"!ln -s /kaggle/input/3d-object-detection-for-autonomous-vehicles/train_images images\n!ln -s /kaggle/input/3d-object-detection-for-autonomous-vehicles/train_maps maps\n!ln -s /kaggle/input/3d-object-detection-for-autonomous-vehicles/train_lidar lidar","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"lyft_dataset =  LyftDataset(data_path='.', json_path='/kaggle/input/3d-object-detection-for-autonomous-vehicles/train_data', verbose=True)\n","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"## Select a trip with data on all sensors"},{"metadata":{"trusted":true},"cell_type":"code","source":"log_df = pd.DataFrame(lyft_dataset.log)\n# log_df = log_df[log_df['vehicle'].str.match('a101')]\n#da4ed9e02f64c544f4f1f10c6738216dcb0e6b0d50952e\nscene_df =  pd.DataFrame(lyft_dataset.scene)\nscene_df = pd.merge(log_df, scene_df, left_on='token', right_on='log_token',how='inner')\n\n# scene_df.head()\nsample_df = pd.DataFrame(lyft_dataset.sample)\nsample_df = pd.merge(scene_df[['log_token', 'date_captured', 'vehicle', 'token_y']], sample_df, left_on='token_y', right_on='scene_token',how='inner')\n# sample_df.head()\n\nsampledata_df = pd.DataFrame(lyft_dataset.sample_data)\nsampledata_df = pd.merge(sample_df[['log_token', 'date_captured', 'token', 'vehicle']], sampledata_df, left_on='token', right_on='sample_token',how='inner')\n# sampledata_df.head()\ncounts = sampledata_df.groupby(['vehicle','date_captured'])['channel'].value_counts().unstack().fillna(0)\n\ncounts","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"### a102 on 2019-05-24 looks good"},{"metadata":{},"cell_type":"markdown","source":"## Prepare data for plotting the trip"},{"metadata":{"_uuid":"d629ff2d2480ee46fbb7e2d37f6b5fab8052498a","_cell_guid":"79c7e3d0-c299-4dcb-8224-4455121ee9b0","trusted":true},"cell_type":"code","source":"# join log, scene, sample, data, ego pose and filter for car a102's ride on 2019-05-24\nlog_df = pd.DataFrame(lyft_dataset.log)\nlog_df = log_df[log_df['date_captured'].str.match('2019-05-24')]\nlog_df = log_df[log_df['vehicle'].str.match('a102')]\n\n\nscene_df =  pd.DataFrame(lyft_dataset.scene)\nscene_df = pd.merge(log_df, scene_df, left_on='token', right_on='log_token',how='inner')\n\nsample_df = pd.DataFrame(lyft_dataset.sample)\nsample_df = pd.merge(sample_df, scene_df[['vehicle', 'token_y']], left_on='scene_token', right_on='token_y',how='inner')\n\nsampledata_df = pd.DataFrame(lyft_dataset.sample_data)\nsampledata_df = pd.merge(sample_df[['token', 'vehicle']], sampledata_df, left_on='token', right_on='sample_token',how='inner')\n\nego_pose_df = pd.DataFrame(lyft_dataset.ego_pose)\n\nego_pose_df = pd.merge(sampledata_df[['token_x','ego_pose_token', 'channel','vehicle' ,'calibrated_sensor_token']], \n                                   ego_pose_df, left_on='ego_pose_token', right_on='token',how='inner')\n\nego_pose_df = ego_pose_df.drop(['token'], axis=1)\nego_pose_df.rename(columns={'token_x':'token'}, inplace=True)\n\n\ncalibrated_sensor_df = pd.DataFrame(lyft_dataset.calibrated_sensor)\n# calibrated_sensor_df.head()\ncalibrated_sensor_df.rename(columns={\n    'token':'calibrated_sensor_token',\n    'rotation':'calibrated_sensor_rotation',\n    'translation':'calibrated_sensor_translation'    \n                                    }, inplace=True)\n\n\n\nego_pose_df = pd.merge(ego_pose_df, \n                      calibrated_sensor_df[['calibrated_sensor_token', 'calibrated_sensor_rotation', 'calibrated_sensor_translation']],\n                      left_on='calibrated_sensor_token',\n                      right_on='calibrated_sensor_token',\n                      how='inner')\n# ego_pose_df = ego_pose_df[ego_pose_df['vehicle'].str.match('a101')]\n# ego_pose_df.sort_values(by=['token','timestamp'])\nego_pose_df.sort_values(by=['token'])\nego_pose_df['timestamp'] = ego_pose_df['timestamp'].astype(str)\n\n# pivot on sample token to spread channel translations across columns\npivot_df = ego_pose_df.pivot(index ='token', columns ='channel', values = ['translation','rotation','calibrated_sensor_translation','calibrated_sensor_rotation']).reset_index()\n","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"ego_pose_df[ego_pose_df['token'].str.match('a1e8c14fe99d3543b54adfefafc8207cb1c34a80afde92')]","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"pivot_df.head()","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"## Is the car moving sideways?"},{"metadata":{"trusted":true},"cell_type":"code","source":"\n# Camera front\nx = pivot_df.iloc[:,4].map(lambda t: t[0])\ny = pivot_df.iloc[:,4].map(lambda t: t[1])\n\n\n# Camera front left\nx0 = pivot_df.iloc[:,5].map(lambda t: t[0])\ny0 = pivot_df.iloc[:,5].map(lambda t: t[1])\n\n# Camera front right\nx1 = pivot_df.iloc[:,6].map(lambda t: t[0])\ny1 = pivot_df.iloc[:,6].map(lambda t: t[1])\n\n\n#LIDAR top\nx2 = pivot_df.iloc[:,10].map(lambda t: t[0])\ny2 = pivot_df.iloc[:,10].map(lambda t: t[1])\n\n\n#Camera Back\nx3 = pivot_df.iloc[:,1].map(lambda t: t[0])\ny3 = pivot_df.iloc[:,1].map(lambda t: t[1])\n\n\n","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"fig = plt.figure(figsize=(16,8))\n        \nax = fig.add_subplot(111)\nax.set(xlim=(2110, 2130), ylim=(1020, 1035))\n\nax.scatter(x, y,s=50, c='r', label='Camera Front')\nax.scatter(x0, y0,s=50, c='g', label='Camera Front Left')\nax.scatter(x1, y1,s=50, c='orange', label='Camera Front Right')\nax.scatter(x2, y2,s=50, c='b', label='LIDAR Top')\nax.scatter(x3, y3,s=50, c='y', label='Camera Back')\n\nax.legend()\nplt.show()","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"## Tried applying rotation after translation - still wierd"},{"metadata":{"trusted":true},"cell_type":"code","source":"# Camera front\nreal_camera_front_coords =  pivot_df.apply(lambda row: Quaternion(row.iloc[14]).rotate(row.iloc[4]), axis=1)\nx_rot = real_camera_front_coords.map(lambda t: t[0])\ny_rot = real_camera_front_coords.map(lambda t: t[1])\n\n\n# Camera front left\nreal_camera_front_coords_0 =  pivot_df.apply(lambda row: Quaternion(row.iloc[15]).rotate(row.iloc[5]), axis=1)\nx0_rot = real_camera_front_coords_0.map(lambda t: t[0])\ny0_rot = real_camera_front_coords_0.map(lambda t: t[1])\n\n\n# Camera front right\nreal_camera_front_coords_1 =  pivot_df.apply(lambda row: Quaternion(row.iloc[16]).rotate(row.iloc[6]), axis=1)\nx1_rot = real_camera_front_coords_1.map(lambda t: t[0])\ny1_rot = real_camera_front_coords_1.map(lambda t: t[1])\n\n\n\n#LIDAR top\nreal_camera_front_coords_2 =  pivot_df.apply(lambda row: Quaternion(row.iloc[20]).rotate(row.iloc[10]), axis=1)\nx2_rot = real_camera_front_coords_2.map(lambda t: t[0])\ny2_rot = real_camera_front_coords_2.map(lambda t: t[1])\n\n\n\n#Camera Back\nreal_camera_front_coords_3 =  pivot_df.apply(lambda row: Quaternion(row.iloc[11]).rotate(row.iloc[1]), axis=1)\nx3_rot = real_camera_front_coords_3.map(lambda t: t[0])\ny3_rot = real_camera_front_coords_3.map(lambda t: t[1])\n\n\n","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"row = pivot_df.iloc[0,[1,11,21,31]]\nrow","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"row = pivot_df.iloc[0,[4,14,24,34]]\nrow\n#row.iloc[0], row.iloc[1],Quaternion(row.iloc[1]).rotate(row.iloc[0])","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"pivot_df.iloc[0,[5,15,25,35]]","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"pivot_df.iloc[0,[6,16,26,36]]","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"pivot_df.iloc[0,[1,11,21,31]]","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"pivot_df.iloc[0,[10,20,30,40]]","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"row = pivot_df.iloc[0,[5,15]]\nrow.iloc[0], row.iloc[1],Quaternion(row.iloc[1]).rotate(row.iloc[0])","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"row = pivot_df.iloc[0,[6,16]]\nrow.iloc[0], row.iloc[1],Quaternion(row.iloc[1]).rotate(row.iloc[0])","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"row = pivot_df.iloc[0,[1,11]]\nrow.iloc[0], row.iloc[1],Quaternion(row.iloc[1]).rotate(row.iloc[0])","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"pivot_df.iloc[0,[10,20]]","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"row = pivot_df.iloc[0,[10,20]]\nrow.iloc[0], row.iloc[1],Quaternion(row.iloc[1]).rotate(row.iloc[0])","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"fig = plt.figure(figsize=(16,8))\n        \n\nax = fig.add_subplot(111)\nax.set(xlim=(2610, 2630), ylim=(-1270, -1225))\n\nax.scatter(x_rot, y_rot,s=50, c='r', label='Camera Front')\nax.scatter(x0_rot, y0_rot,s=50, c='g', label='Camera Front Left')\nax.scatter(x1_rot, y1_rot,s=50, c='orange', label='Camera Front Right')\nax.scatter(x2_rot, y2_rot,s=50, c='b', label='LIDAR Top')\nax.scatter(x3_rot, y3_rot,s=50, c='y', label='Camera Back')\n\nax.legend()\n\nplt.show()","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"## This approach appears to be right. Needs confirmation from an expert though..."},{"metadata":{"trusted":true},"cell_type":"code","source":"## Rotate transform sensor's coordinates in car frame (using the rotation specs of back camera as it is closest to back frame)\n## Then position transform the latter in world frame\ndef sensor_coords_in_world (row, i):\n    rot = row.iloc[11];\n    return  np.add(row.iloc[1],Quaternion(rot).rotate(row.iloc[i+20] ))\n","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"pivot_df.iloc[0].iloc[10]","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"pivot_df.iloc[0].iloc[1],pivot_df.iloc[0].iloc[21], sensor_coords_in_world(pivot_df.iloc[0],1)\n","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"pivot_df.iloc[0].iloc[4],pivot_df.iloc[0].iloc[24], sensor_coords_in_world(pivot_df.iloc[0],4)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"pivot_df.iloc[0].iloc[5],pivot_df.iloc[0].iloc[25], sensor_coords_in_world(pivot_df.iloc[0],5)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"pivot_df.iloc[0].iloc[6], pivot_df.iloc[0].iloc[26], sensor_coords_in_world(pivot_df.iloc[0],6)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"pivot_df.iloc[0].iloc[10], pivot_df.iloc[0].iloc[30], sensor_coords_in_world(pivot_df.iloc[0],10)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"\n\n# Camera front\nreal_camera_front_coords =  pivot_df.apply(lambda row: sensor_coords_in_world(row,4), axis=1)\nx_rot = real_camera_front_coords.map(lambda t: t[0])\ny_rot = real_camera_front_coords.map(lambda t: t[1])\n\n\n# Camera front left\nreal_camera_front_coords_0 =  pivot_df.apply(lambda row: sensor_coords_in_world(row,5), axis=1)\nx0_rot = real_camera_front_coords_0.map(lambda t: t[0])\ny0_rot = real_camera_front_coords_0.map(lambda t: t[1])\n\n\n# Camera front right\nreal_camera_front_coords_1 =  pivot_df.apply(lambda row: sensor_coords_in_world(row,6), axis=1)\nx1_rot = real_camera_front_coords_1.map(lambda t: t[0])\ny1_rot = real_camera_front_coords_1.map(lambda t: t[1])\n\n\n\n#LIDAR top\nreal_camera_front_coords_2 =  pivot_df.apply(lambda row: sensor_coords_in_world(row,10), axis=1)\nx2_rot = real_camera_front_coords_2.map(lambda t: t[0])\ny2_rot = real_camera_front_coords_2.map(lambda t: t[1])\n\n\n\n#Camera Back\nreal_camera_front_coords_3 =  pivot_df.apply(lambda row: sensor_coords_in_world(row,1), axis=1)\nx3_rot = real_camera_front_coords_3.map(lambda t: t[0])\ny3_rot = real_camera_front_coords_3.map(lambda t: t[1])\n\n\n\n","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"fig = plt.figure(figsize=(16,8))\n        \n\nax = fig.add_subplot(111)\nax.set(xlim=(2120, 2145), ylim=(1010, 1027))\n\nax.scatter(x_rot, y_rot,s=50, c='r', label='Camera Front')\nax.scatter(x0_rot, y0_rot,s=50, c='g', label='Camera Front Left')\nax.scatter(x1_rot, y1_rot,s=50, c='orange', label='Camera Front Right')\nax.scatter(x2_rot, y2_rot,s=50, c='b', label='LIDAR Top')\nax.scatter(x3_rot, y3_rot,s=50, c='y', label='Camera Back')\n\nax.legend()\nplt.show()","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}