.. DO NOT EDIT. .. THIS FILE WAS AUTOMATICALLY GENERATED BY SPHINX-GALLERY. .. TO MAKE CHANGES, EDIT THE SOURCE PYTHON FILE: .. "imu/imu_integrator_tutorial.py" .. LINE NUMBERS ARE GIVEN BELOW. .. only:: html .. note:: :class: sphx-glr-download-link-note Click :ref:`here ` to download the full example code .. rst-class:: sphx-glr-example-title .. _sphx_glr_imu_imu_integrator_tutorial.py: IMU Integrator Tutorial ======================== .. GENERATED FROM PYTHON SOURCE LINES 7-9 Uncomment this if you're using google colab to run this script .. GENERATED FROM PYTHON SOURCE LINES 9-13 .. code-block:: default # !pip install pypose # !pip install pykitti .. GENERATED FROM PYTHON SOURCE LINES 14-41 In this tutorial, we will be doing IMU integration using the ``pypose.module.IMUPreintegrator`` module. 1. What is IMU integration -------------------------- An Inertial Measurement Unit (IMU) is a device that can measure accelaration and angular velocity. An IMU typically consists of: * Gyroscopes: providing a measure of angular velocity * Accelerometers: providing a measure of acceleration With acceleration and angular velocity, we can get velocity and position using basic kinetics: * The first integral of acceleration over time is the change in velocity. * The second integral of acceleration over time is the change in position. This process is called the IMU preintegration, often used in applications in robotics like SLAM (Simultaneous Localization and Mapping). Uncertainty ~~~~~~~~~~~~~ However, IMU measurements contains very big noise. For example, if we put an IMU sensor in a static position, the measurements will jump around zero. That's why, the more we integrate, the more uncertain we are. This uncertainty can also be measured mathematically. Please refer the `doc `_ for the math. We will see below in a simple example, how we can get the IMU integrated position and the uncertainty with ``pypose.module.IMUPreintegrator``. .. GENERATED FROM PYTHON SOURCE LINES 41-56 .. code-block:: default import os import argparse import torch import pykitti import numpy as np import pypose as pp from datetime import datetime import torch.utils.data as Data import matplotlib.pyplot as plt from matplotlib.patches import Ellipse from matplotlib.collections import PatchCollection .. GENERATED FROM PYTHON SOURCE LINES 57-79 2. Dataset Defination -------------------------- First we will define the ``KITTI_IMU`` dataset as a ``data.Dataset`` in torch, for easy usage. We're using the ``pykitti`` package. This package provides a minimal set of tools for working with the KITTI datasets. To access a data sequence, use: :: dataset = pykitti.raw(root, dataname, drive) Some of the data attributes we used below are: * ``dataset.timestamps``: Timestamps are parsed into a list of datetime objects * ``dataset.oxts``: List of OXTS packets and 6-dof poses as named tuples For more details about the data format, please refer to their github page `here `_. A sequence will be seperated into many segments. The number of segments is controlled by ``step_size``. Each segment of the sequence will return the measurements like ``dt``, ``acc``, and ``gyro`` for a few frames, defined by duration. .. GENERATED FROM PYTHON SOURCE LINES 79-146 .. code-block:: default class KITTI_IMU(Data.Dataset): def __init__(self, root, dataname, drive, duration=10, step_size=1, mode='train'): super().__init__() self.duration = duration self.data = pykitti.raw(root, dataname, drive) self.seq_len = len(self.data.timestamps) - 1 assert mode in ['evaluate', 'train', 'test'], "{} mode is not supported.".format(mode) self.dt = torch.tensor([datetime.timestamp(self.data.timestamps[i+1]) - datetime.timestamp(self.data.timestamps[i]) for i in range(self.seq_len)]) self.gyro = torch.tensor([[self.data.oxts[i].packet.wx, self.data.oxts[i].packet.wy, self.data.oxts[i].packet.wz] for i in range(self.seq_len)]) self.acc = torch.tensor([[self.data.oxts[i].packet.ax, self.data.oxts[i].packet.ay, self.data.oxts[i].packet.az] for i in range(self.seq_len)]) self.gt_rot = pp.euler2SO3(torch.tensor([[self.data.oxts[i].packet.roll, self.data.oxts[i].packet.pitch, self.data.oxts[i].packet.yaw] for i in range(self.seq_len)])) self.gt_vel = self.gt_rot @ torch.tensor([[self.data.oxts[i].packet.vf, self.data.oxts[i].packet.vl, self.data.oxts[i].packet.vu] for i in range(self.seq_len)]) self.gt_pos = torch.tensor( np.array([self.data.oxts[i].T_w_imu[0:3, 3] for i in range(self.seq_len)])) start_frame = 0 end_frame = self.seq_len if mode == 'train': end_frame = np.floor(self.seq_len * 0.5).astype(int) elif mode == 'test': start_frame = np.floor(self.seq_len * 0.5).astype(int) self.index_map = [i for i in range( 0, end_frame - start_frame - self.duration, step_size)] def __len__(self): return len(self.index_map) def __getitem__(self, i): frame_id = self.index_map[i] end_frame_id = frame_id + self.duration return { 'dt': self.dt[frame_id: end_frame_id], 'acc': self.acc[frame_id: end_frame_id], 'gyro': self.gyro[frame_id: end_frame_id], 'gyro': self.gyro[frame_id: end_frame_id], 'gt_pos': self.gt_pos[frame_id+1: end_frame_id+1], 'gt_rot': self.gt_rot[frame_id+1: end_frame_id+1], 'gt_vel': self.gt_vel[frame_id+1: end_frame_id+1], 'init_pos': self.gt_pos[frame_id][None, ...], # TODO: the init rotation might be used in gravity compensation 'init_rot': self.gt_rot[frame_id: end_frame_id], 'init_vel': self.gt_vel[frame_id][None, ...], } def get_init_value(self): return {'pos': self.gt_pos[:1], 'rot': self.gt_rot[:1], 'vel': self.gt_vel[:1]} .. GENERATED FROM PYTHON SOURCE LINES 147-151 3. Utility Functions -------------------------- These are several utility functions. You can skip to the parameter definations and come back when necessary. .. GENERATED FROM PYTHON SOURCE LINES 153-157 ``imu_collate`` ~~~~~~~~~~~~~~~~ ``imu_collate`` is used in batch operation, to stack data in multiple frames together. .. GENERATED FROM PYTHON SOURCE LINES 157-187 .. code-block:: default def imu_collate(data): acc = torch.stack([d['acc'] for d in data]) gyro = torch.stack([d['gyro'] for d in data]) gt_pos = torch.stack([d['gt_pos'] for d in data]) gt_rot = torch.stack([d['gt_rot'] for d in data]) gt_vel = torch.stack([d['gt_vel'] for d in data]) init_pos = torch.stack([d['init_pos'] for d in data]) init_rot = torch.stack([d['init_rot'] for d in data]) init_vel = torch.stack([d['init_vel'] for d in data]) dt = torch.stack([d['dt'] for d in data]).unsqueeze(-1) return { 'dt': dt, 'acc': acc, 'gyro': gyro, 'gt_pos': gt_pos, 'gt_vel': gt_vel, 'gt_rot': gt_rot, 'init_pos': init_pos, 'init_vel': init_vel, 'init_rot': init_rot, } .. GENERATED FROM PYTHON SOURCE LINES 188-192 ``move_to`` ~~~~~~~~~~~~~~~~ ``move_to`` used to move different object to CUDA device. .. GENERATED FROM PYTHON SOURCE LINES 192-210 .. code-block:: default def move_to(obj, device): if torch.is_tensor(obj): return obj.to(device) elif isinstance(obj, dict): res = {} for k, v in obj.items(): res[k] = move_to(v, device) return res elif isinstance(obj, list): res = [] for v in obj: res.append(move_to(v, device)) return res else: raise TypeError("Invalid type for move_to", obj) .. GENERATED FROM PYTHON SOURCE LINES 211-216 ``plot_gaussian`` ~~~~~~~~~~~~~~~~~~ ``plot_gaussian`` used to plot an ellipse measuring uncertainty, bigger ellipse means bigger uncertainty. .. GENERATED FROM PYTHON SOURCE LINES 216-230 .. code-block:: default def plot_gaussian(ax, means, covs, color=None, sigma=3): ''' Set specific color to show edges, otherwise same with facecolor.''' ellipses = [] for i in range(len(means)): eigvals, eigvecs = np.linalg.eig(covs[i]) axis = np.sqrt(eigvals) * sigma slope = eigvecs[1][0] / eigvecs[1][1] angle = 180.0 * np.arctan(slope) / np.pi ellipses.append(Ellipse(means[i, 0:2], axis[0], axis[1], angle=angle)) ax.add_collection(PatchCollection(ellipses, edgecolors=color, linewidth=1)) .. GENERATED FROM PYTHON SOURCE LINES 231-235 4. Define Parameters ---------------------- Here we define all the parameters we will use. See the help message for the usage of each parameter. .. GENERATED FROM PYTHON SOURCE LINES 235-280 .. code-block:: default parser = argparse.ArgumentParser(description='IMU Preintegration') parser.add_argument("--device", type=str, default='cpu', help="cuda or cpu") parser.add_argument("--batch-size", type=int, default=1, help="batch size, only support 1 now") #why? parser.add_argument("--step-size", type=int, default=2, help="the size of the integration for one interval") parser.add_argument("--save", type=str, default='../dataset/save/', help="location of png files to save") parser.add_argument("--dataroot", type=str, default='../dataset/', help="dataset location downloaded") parser.add_argument("--dataname", type=str, default='2011_09_26', help="dataset name") parser.add_argument("--datadrive", nargs='+', type=str, default=["0001", "0002", "0005", "0009", "0011", "0013", "0014", "0015", "0017", "0018", "0019", "0020", "0022"], help="data sequences") parser.add_argument('--plot3d', dest='plot3d', action='store_true', help="plot in 3D space, default: False") parser.set_defaults(plot3d=False) args, unknown = parser.parse_known_args() print(args) os.makedirs(os.path.join(args.save), exist_ok=True) torch.set_default_tensor_type(torch.DoubleTensor) .. rst-class:: sphx-glr-script-out .. code-block:: none Namespace(device='cpu', batch_size=1, step_size=2, save='../dataset/save/', dataroot='../dataset/', dataname='2011_09_26', datadrive=['0001', '0002', '0005', '0009', '0011', '0013', '0014', '0015', '0017', '0018', '0019', '0020', '0022'], plot3d=False) .. GENERATED FROM PYTHON SOURCE LINES 281-296 5. Perform Integration ---------------------- With everything set up, we will perform the core operation of IMU integration. There are mainly 5 steps in the codes below: #. **Step 1**: Define dataloader using the ``KITTI_IMU`` class we defined above #. **Step 2**: Get the initial position, rotation and velocity, all 0 here #. **Step 3**: Define the IMUPreintegrator #. **Step 4**: Perform integration: After running the forward function of the ``integrator``, the result is stored in ``state``, where ``state['pos']`` is the integrated position, and ``state['cov']`` is the uncertainty measurements. Note that ``state['cov']`` is a 9x9 matrix in the order of rotation, velocity, and position. That's why in visualization we are using ``covs[:, 6:8, 6:8]`` here: they are the covariance matrix of ``x`` and ``y`` position. #. **Step 5**: Visualization .. GENERATED FROM PYTHON SOURCE LINES 296-356 .. code-block:: default for drive in args.datadrive: # Step 1: Define dataloader using the ``KITTI_IMU`` class we defined above dataset = KITTI_IMU(args.dataroot, args.dataname, drive, duration=args.step_size, step_size=args.step_size, mode='evaluate') loader = Data.DataLoader(dataset=dataset, batch_size=args.batch_size, collate_fn=imu_collate, shuffle=False) # Step 2: Get the initial position, rotation and velocity, all 0 here init = dataset.get_init_value() # Step 3: Define the IMUPreintegrator. integrator = pp.module.IMUPreintegrator(init['pos'], init['rot'], init['vel'], reset=False).to(args.device) # Step 4: Perform integration poses, poses_gt = [init['pos']], [init['pos']] covs = [torch.zeros(9, 9)] for idx, data in enumerate(loader): data = move_to(data, args.device) state = integrator(dt=data['dt'], gyro=data['gyro'], acc=data['acc'], rot=data['init_rot']) poses_gt.append(data['gt_pos'][..., -1, :].cpu()) poses.append(state['pos'][..., -1, :].cpu()) covs.append(state['cov'][..., -1, :, :].cpu()) poses = torch.cat(poses).numpy() poses_gt = torch.cat(poses_gt).numpy() covs = torch.stack(covs, dim=0).numpy() # Step 5: Visualization plt.figure(figsize=(5, 5)) if args.plot3d: ax = plt.axes(projection='3d') ax.plot3D(poses[:, 0], poses[:, 1], poses[:, 2], 'b') ax.plot3D(poses_gt[:, 0], poses_gt[:, 1], poses_gt[:, 2], 'r') else: ax = plt.axes() ax.plot(poses[:, 0], poses[:, 1], 'b') ax.plot(poses_gt[:, 0], poses_gt[:, 1], 'r') plot_gaussian(ax, poses[:, 0:2], covs[:, 6:8, 6:8]) plt.title("PyPose IMU Integrator") plt.legend(["PyPose", "Ground Truth"]) figure = os.path.join(args.save, args.dataname+'_'+drive+'.png') plt.savefig(figure) print("Saved to", figure) .. rst-class:: sphx-glr-horizontal * .. image-sg:: /imu/images/sphx_glr_imu_integrator_tutorial_001.png :alt: PyPose IMU Integrator :srcset: /imu/images/sphx_glr_imu_integrator_tutorial_001.png :class: sphx-glr-multi-img * .. image-sg:: /imu/images/sphx_glr_imu_integrator_tutorial_002.png :alt: PyPose IMU Integrator :srcset: /imu/images/sphx_glr_imu_integrator_tutorial_002.png :class: sphx-glr-multi-img * .. image-sg:: /imu/images/sphx_glr_imu_integrator_tutorial_003.png :alt: PyPose IMU Integrator :srcset: /imu/images/sphx_glr_imu_integrator_tutorial_003.png :class: sphx-glr-multi-img * .. image-sg:: /imu/images/sphx_glr_imu_integrator_tutorial_004.png :alt: PyPose IMU Integrator :srcset: /imu/images/sphx_glr_imu_integrator_tutorial_004.png :class: sphx-glr-multi-img * .. image-sg:: /imu/images/sphx_glr_imu_integrator_tutorial_005.png :alt: PyPose IMU Integrator :srcset: /imu/images/sphx_glr_imu_integrator_tutorial_005.png :class: sphx-glr-multi-img * .. image-sg:: /imu/images/sphx_glr_imu_integrator_tutorial_006.png :alt: PyPose IMU Integrator :srcset: /imu/images/sphx_glr_imu_integrator_tutorial_006.png :class: sphx-glr-multi-img * .. image-sg:: /imu/images/sphx_glr_imu_integrator_tutorial_007.png :alt: PyPose IMU Integrator :srcset: /imu/images/sphx_glr_imu_integrator_tutorial_007.png :class: sphx-glr-multi-img * .. image-sg:: /imu/images/sphx_glr_imu_integrator_tutorial_008.png :alt: PyPose IMU Integrator :srcset: /imu/images/sphx_glr_imu_integrator_tutorial_008.png :class: sphx-glr-multi-img * .. image-sg:: /imu/images/sphx_glr_imu_integrator_tutorial_009.png :alt: PyPose IMU Integrator :srcset: /imu/images/sphx_glr_imu_integrator_tutorial_009.png :class: sphx-glr-multi-img * .. image-sg:: /imu/images/sphx_glr_imu_integrator_tutorial_010.png :alt: PyPose IMU Integrator :srcset: /imu/images/sphx_glr_imu_integrator_tutorial_010.png :class: sphx-glr-multi-img * .. image-sg:: /imu/images/sphx_glr_imu_integrator_tutorial_011.png :alt: PyPose IMU Integrator :srcset: /imu/images/sphx_glr_imu_integrator_tutorial_011.png :class: sphx-glr-multi-img * .. image-sg:: /imu/images/sphx_glr_imu_integrator_tutorial_012.png :alt: PyPose IMU Integrator :srcset: /imu/images/sphx_glr_imu_integrator_tutorial_012.png :class: sphx-glr-multi-img * .. image-sg:: /imu/images/sphx_glr_imu_integrator_tutorial_013.png :alt: PyPose IMU Integrator :srcset: /imu/images/sphx_glr_imu_integrator_tutorial_013.png :class: sphx-glr-multi-img .. rst-class:: sphx-glr-script-out .. code-block:: none Saved to ../dataset/save/2011_09_26_0001.png Saved to ../dataset/save/2011_09_26_0002.png Saved to ../dataset/save/2011_09_26_0005.png Saved to ../dataset/save/2011_09_26_0009.png Saved to ../dataset/save/2011_09_26_0011.png Saved to ../dataset/save/2011_09_26_0013.png Saved to ../dataset/save/2011_09_26_0014.png Saved to ../dataset/save/2011_09_26_0015.png Saved to ../dataset/save/2011_09_26_0017.png Saved to ../dataset/save/2011_09_26_0018.png Saved to ../dataset/save/2011_09_26_0019.png Saved to ../dataset/save/2011_09_26_0020.png Saved to ../dataset/save/2011_09_26_0022.png .. GENERATED FROM PYTHON SOURCE LINES 357-362 We can see that, in some of the sequences, the integrated position drifts away from the groundtruth, also the uncertainty grows very big. This shows the noisy nature of the IMU sensor. In the IMUCorrector tutorial, we will see an example of how we can correct this. .. rst-class:: sphx-glr-timing **Total running time of the script:** ( 0 minutes 5.191 seconds) .. _sphx_glr_download_imu_imu_integrator_tutorial.py: .. only:: html .. container:: sphx-glr-footer sphx-glr-footer-example .. container:: sphx-glr-download sphx-glr-download-python :download:`Download Python source code: imu_integrator_tutorial.py ` .. container:: sphx-glr-download sphx-glr-download-jupyter :download:`Download Jupyter notebook: imu_integrator_tutorial.ipynb ` .. only:: html .. rst-class:: sphx-glr-signature `Gallery generated by Sphinx-Gallery `_