Registering LiDAR and UAV data using on ice GPS and total station observations

lidar_auv.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Coregistration of LiDAR and AUV draft observations\n",
    "### Using on ice GPS and total station observations as keys\n",
    "\n",
    "Adam Steer, February 2016, adam.d.steer@gmail.com"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "On SIPEX-2 (2012 - see http://seaice.acecrc.org.au/sipex2012) airborne LiDAR was flown over\n",
    "a small area designated for intensive sampling, taking sea ice height measurements.\n",
    "An AUV swam underneath, taking observations of sea ice draft.\n",
    "To look at how well estimates of ice thickness using altimetry and some empirical models work\n",
    "we need to coregister airborne and AUV data.\n",
    "How?\n",
    "\n",
    "On the ice floe a local coordinate system was deployed using at least one pair of GPS receivers\n",
    "and a robotic total station. Locating beacons for the AUV were registered in this local\n",
    "system, so UAV drafts are in the local 'floe north' system already.\n",
    "\n",
    "But LiDAR was captured in geographic coordinates! Using the on-ice GPS pair, we track ice drift\n",
    "and ice floe rotation, and ue these to adjust the aircraft trajectory. We know that one of the GPS stations (ice1) has a local coordinate of [0,0]. We also know that the azimuth between the two GPS stations gives us the rotation between 'map north' and 'floe north', since 'floe north' is the baseline between the reciever pair.\n",
    "\n",
    "We also know that we can work just on the aircraft trajectory, and then regenerate a LiDAR point cloud, rather than loading up and rotating N * 10^^6 points!\n",
    "\n",
    "So first the trajectory is translated to local coordinates, and then a rotated to align the aicraft trajectory 'north' to 'floe north'. It should probably be the other way (rotation -> translation), but it's not complex, operating only in two dimensions, so I think order doesn't matter here. Here's the code:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "\n",
    "#perform rotation of an array of 2D coordinates (x,y) about Z axis\n",
    "def rotate_z(coord, angle):\n",
    "    r_angle = angle * np.pi/180\n",
    "    rotmatrix = np.array(((np.cos(r_angle), np.sin(r_angle)), (np.sin(-r_angle), np.cos(r_angle))))\n",
    "    new_coord = coord * rotmatrix\n",
    "    return new_coord\n",
    "\n",
    "def trim_timeseries(t1, t2):\n",
    "#trim a long timeseries to match a shoreter one.\n",
    "#t1 is the long series, t2 is the short sries\n",
    "#returns a set of indices to apply to the longer\n",
    "#timeseries dataset\n",
    "    start = np.min(t2)\n",
    "    end = np.max(t2)\n",
    "    idx = (t1>=start)*(t1<=end)\n",
    "    the_inds = np.where(idx)\n",
    "    return np.asarray(the_inds), start, end\n",
    "\n",
    "def find_nearest_vector(array, value):\n",
    "#http://stackoverflow.com/questions/2566412/find-nearest-value-in-numpy-array\n",
    "  idx = np.array([np.linalg.norm(x+y) for (x,y) in array-value]).argmin()\n",
    "  return idx"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "OK, functions are declared. next is some messy data gathering (maybe implement using Pandas\n",
    "                                                              to tidy up next time):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "\n",
    "traj_dir = '/media/adam/data/is6_f11/lidar.matlab/trajectory/'\n",
    "trajfile = 'is6_f11_pass1.3dp'\n",
    "\n",
    "base_dir = '/media/adam/data/is6_f11/lidar.matlab/base/'\n",
    "basefile = 'ice1_utm51s_grafnav_ppp_nohead.txt'\n",
    "\n",
    "rots_dir = '/media/adam/data/is6_f11/lidar.matlab/base/'\n",
    "baserots = 'ice1_ice2_relaz.txt'\n",
    "\n",
    "outfilename = '/media/adam/data/is6_f11/lidar.matlab/trajectory/is6_f11_pass1_local_ice_rot.3dp'\n",
    "\n",
    "#aim here  is to rotate the trajectory at each epoch by the azimuth\n",
    "# between two GPS stations on the ice\n",
    "# this *should* end up with a trajectory using 'ice floe' north.\n",
    "# Also, translating such that the GPS on ice is point (0,0)\n",
    "\n",
    "trajectory = np.loadtxt(traj_dir + trajfile)\n",
    "base_pos = np.loadtxt(base_dir + basefile)\n",
    "base_rots = np.loadtxt(rots_dir + baserots)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So we have collected a number of data sources here:\n",
    "    - the '.3dp' file is the output of RT post-process, already reprojected to UTM. This has aircraft position and attitude from GPS+IMU,at 250 Hz.\n",
    "    - two ice base station files are used - first is the base position in space and time, and the second is the relative azimuth from base1 -> base2. This second file describes the rotation between UTM north and floe north.\n",
    "    \n",
    "The first job we do is collect the time vector from all three datasets\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "\n",
    "t1 = base_pos[:,0]\n",
    "t2 = trajectory[:,0]\n",
    "t3 = base_rots[:,0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "In the next block, we make sure all the three data sources have the same time range"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "base_timeinds,start,end = trim_timeseries(t1,t2)\n",
    "base_timeinds = base_timeinds[0,:]\n",
    "base_pos = base_pos[base_timeinds,:]\n",
    "t1 = base_pos[:,0]\n",
    "\n",
    "rots_timeinds,start,end = trim_timeseries(t3,t2)\n",
    "rots_timeinds = rots_timeinds[0,:]\n",
    "base_rots = base_rots[rots_timeinds,:]\n",
    "t3 = base_rots[:,0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "...and then we interpolate from GPS base frequency (2 Hz) to aicraft data frequency (250 Hz)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "\n",
    "interp_base_x = np.interp(t2,t1,base_pos[:,1])\n",
    "interp_base_y = np.interp(t2,t1,base_pos[:,2])\n",
    "interp_rot = np.interp(t2,t3,base_rots[:,1])\n",
    "\n",
    "local_x = trajectory[:,1] - interp_base_x\n",
    "local_y = trajectory[:,2] - interp_base_y\n",
    "\n",
    "local_xy = np.column_stack((local_x, local_y))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Notice in the step above the planar motion of the ice floe was subtracted from the aircraft\n",
    "trajectory. This is done for each timeseries step - not just one correction for the whole\n",
    "position vector, because the ice is always moving!\n",
    "\n",
    "Next, we find the closest point in our local coordinate vector to the origin. This index\n",
    "tells us where to find a rotation angle. Here, a single angle is applied as a sort of 'snapshot'. \n",
    "(why not a timeseries of angles? because it messes up aicraft heading required to make LiDAR)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "\n",
    "\n",
    "#find rotation angle closest to origin\n",
    "\n",
    "angle_idx = find_nearest_vector(local_xy, [0, 0])\n",
    "the_angle = interp_rot[angle_idx]\n",
    "print('rotating by:')\n",
    "print(the_angle)\n",
    "print('degrees')\n",
    "\n",
    "#rotate the xy array by the rotation closest to origin\n",
    "rot_xy = rotate_z(np.matrix(local_xy), the_angle)\n",
    "\n",
    "adjusted_heading = trajectory[:,6] - the_angle\n",
    "\n",
    "out_trajectory = np.column_stack((t2, rot_xy[:,0],rot_xy[:,1], trajectory[:,3], trajectory[:,4], trajectory[:,5], adjusted_heading, \\\n",
    "                                  trajectory[:,7], trajectory[:,8], trajectory[:,9], trajectory[:,10], trajectory[:,11], \\\n",
    "                                  trajectory[:,12]))\n",
    "\n",
    "np.savetxt(outfilename, out_trajectory, fmt='%.5f')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "That's it! The new trajectory is saved, and now we can use it make LiDAR points in the same coordinate\n",
    "system as the UAV."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.5.1"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}