{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "from nuscenes.can_bus.can_bus_api import NuScenesCanBus\n",
    "nusc_can = NuScenesCanBus(dataroot='can_bus')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "tags": []
   },
   "source": [
    "The CAN bus is a vehicle bus over which information such as position, velocity, acceleration, steering, lights, battery and many more are submitted. We recommend you start by reading the [README](https://github.com/nutonomy/nuscenes-devkit/tree/master/python-sdk/nuscenes/can_bus/README.md)\n",
    "In BEVFormer, we only use the `pose` fields."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "scene_name = 'scene-0001'\n",
    "pose_list = nusc_can.get_messages(scene_name, 'pose')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Each value of `pose_list` contains: \n",
    "- `orientation`: a Quaternion representation of orientation\n",
    "- `pos`: a global postion of ego-car\n",
    "- `vel`: the velocity of ego-car\n",
    "- `rotation_rate`: rotation rate"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "{'accel': [0.05252802768009661, 0.9291747528573647, 9.513756543139106],\n",
       " 'orientation': [0.7479305678167669, 0.0, 0.0, 0.6637769698666026],\n",
       " 'pos': [1010.1436201720262, 610.8882352282457, 0.0],\n",
       " 'rotation_rate': [0.040320225059986115,\n",
       "  -0.002563952235504985,\n",
       "  0.28492140769958496],\n",
       " 'utime': 1531883530467511,\n",
       " 'vel': [4.1688763951334185, 0.0, 0.0]}"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "pose_list[0] # one example"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "dict_keys(['accel', 'orientation', 'pos', 'rotation_rate', 'utime', 'vel'])"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "pose_list[0].keys()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In [data_converter](https://github.com/zhiqi-li/BEVFormer/blob/master/tools/data_converter/nuscenes_converter.py), we use the following function to obatain the can bus information for each sample."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def _get_can_bus_info(nusc, nusc_can_bus, sample):\n",
    "    scene_name = nusc.get('scene', sample['scene_token'])['name']\n",
    "    sample_timestamp = sample['timestamp']\n",
    "    try:\n",
    "        pose_list = nusc_can_bus.get_messages(scene_name, 'pose')\n",
    "    except:\n",
    "        return np.zeros(18)  # serveral scenes do not have can bus information.\n",
    "    can_bus = []\n",
    "    # during each scene, the first timestamp of can_bus may be large than the first sample's timestamp\n",
    "    last_pose = pose_list[0]\n",
    "    for i, pose in enumerate(pose_list):\n",
    "        if pose['utime'] > sample_timestamp:\n",
    "            break\n",
    "        last_pose = pose # we obtain the can_bus information which is recorded before the sample recorded.\n",
    "        \n",
    "    _ = last_pose.pop('utime')  # useless\n",
    "    pos = last_pose.pop('pos') \n",
    "    rotation = last_pose.pop('orientation')\n",
    "    \n",
    "    # one can_bus record contains 18 numbers\n",
    "    can_bus.extend(pos) # [0:3] is the position\n",
    "    can_bus.extend(rotation) # [3:7] is the orientation\n",
    "    \n",
    "    for key in last_pose.keys():\n",
    "        can_bus.extend(pose[key])  # accel: [7, 10], rotation_rate: [10: 13], velocity: [13: 16]\n",
    "    \n",
    "    # the last two numbers are reserved for later calculation of rotation angle.\n",
    "    can_bus.extend([0., 0.])\n",
    "    \n",
    "    \n",
    "    return np.array(can_bus)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In [dataset](https://github.com/zhiqi-li/BEVFormer/blob/master/projects/mmdet3d_plugin/datasets/nuscenes_dataset.py#L174), we reorganize the can_bus."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "        # actually, the nuScenes provides the rotation and translation of each sample, which is more accurate than we obtained from can bus. \n",
    "        rotation = Quaternion(input_dict['ego2global_rotation'])\n",
    "        translation = input_dict['ego2global_translation']\n",
    "        \n",
    "        can_bus = input_dict['can_bus']\n",
    "        can_bus[:3] = translation # We use the provided translation and rotation to repalce the original translation and rotation in can bus\n",
    "        can_bus[3:7] = rotation\n",
    "        \n",
    "        patch_angle = quaternion_yaw(rotation) / np.pi * 180 # we get the yaw angle of ego car\n",
    "        can_bus[-2] = patch_angle / 180 * np.pi # this angle is kept unchanged.\n",
    "        can_bus[-1] = patch_angle # this angle is used to compute the detal of adjacent timestamps."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In [dataset](https://github.com/zhiqi-li/BEVFormer/blob/master/projects/mmdet3d_plugin/datasets/nuscenes_dataset.py#L93), we compute the delta orientation and position of adjacent timestamps"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "        prev_pos = None\n",
    "        prev_angle = None\n",
    "        for i, each in enumerate(queue):\n",
    "            metas_map[i] = each['img_metas'].data\n",
    "            if i == 0:\n",
    "                metas_map[i]['prev_bev'] = False\n",
    "                prev_pos = copy.deepcopy(metas_map[i]['can_bus'][:3])\n",
    "                prev_angle = copy.deepcopy(metas_map[i]['can_bus'][-1])\n",
    "                metas_map[i]['can_bus'][:3] = 0\n",
    "                metas_map[i]['can_bus'][-1] = 0\n",
    "            else:\n",
    "                metas_map[i]['prev_bev'] = True\n",
    "                tmp_pos = copy.deepcopy(metas_map[i]['can_bus'][:3])\n",
    "                tmp_angle = copy.deepcopy(metas_map[i]['can_bus'][-1])\n",
    "                metas_map[i]['can_bus'][:3] -= prev_pos\n",
    "                metas_map[i]['can_bus'][-1] -= prev_angle\n",
    "                prev_pos = copy.deepcopy(tmp_pos)\n",
    "                prev_angle = copy.deepcopy(tmp_angle)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}