Jupyter/CAMBOTv2_convert_GPS_to_camera_pos.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "b3a49088-15d2-4be1-815a-0493bd369fa3",
   "metadata": {},
   "source": [
    "## <div align=\"center\">Transformation of GPS antenna positions to camera sensor positions</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b6dd70e7-8735-4c20-bc67-034bdd7c8f7c",
   "metadata": {},
   "source": [
    "The ATM CAMBOTv2 L0 data product also provides position information for each L0 raw image frame. To use these GPS positions in ATM auxiliary files for processing with the Ames Stereo Pipeline ([ASP](https://stereopipeline.readthedocs.io/en/latest/index.html)) they have to be transformed from GPS antenna coordinates to the camera sensor's focal plane position. This is done by rotating and translating the antenna phase center coordinates using attitude ($p$ (pitch), $r$ (roll), and $h$ (heading)) information and the lever arm $r_{lever}$ between the GPS antenna's phase center and the camera's focal plane. All required information for this transformation is provided in the auxiliary navigation files. The auxiliary navigation files are included in NASA's Airborne Topographic Mapper (ATM) CAMBOTv2 L0 data distribution and are freely available from the National Snow and Ice Data Center (data set ID: IOCAM0):  \n",
    "https://nsidc.org/data/iocam0/versions/1  \n",
    "DOI: https://doi.org/10.5067/IOJH8A5F48J5  \n",
    "**NOTE:** An example auxiliary navigation file is included in this repository in the folder `data/nav` and is used in this notebook for demonstration."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2c227965-979d-4503-9605-dfb13a590fb6",
   "metadata": {},
   "source": [
    "## Calculation of the camera sensor's focal plane position\n",
    "\n",
    "The procedure for transforming the differential GPS antenna positions into camera sensor (focal plane) positions is detailed in the [CAMBOTv2](https://nsidc.org/data/iocam1b/versions/2) general resources document ([Kurtz & Harbeck, 2020](https://nsidc.org/sites/default/files/oib_cambot_geolocation.pdf)) and is summarized here. Some of the methodology was developed by [Harpold et al., 2016](http://dx.doi.org/10.1080/01431161.2015.1137989).\n",
    "\n",
    "First, the geodetic coordinates latitude $\\phi$, longitude $\\lambda$, and altitude $h_{elev}$ for the GPS antenna's phase center located on top of the aircraft's fuselage are converted to Earth-Centered, Earth-Fixed (ECEF) geocentric coordinates $r_{ant}(ECEF)$ using the Python™ [pyproj](https://github.com/pyproj4/pyproj) module. To transform the aircraft's internal Cartesian coordinate system, the three axes need to be rotated about $p$ (pitch), $r$ (roll), and $h$ (heading), respectively. The three rotation matrices can be consolidated into a single $3\\times3$ matrix $T(p,r,h)$ ([Harpold et al., 2016](http://dx.doi.org/10.1080/01431161.2015.1137989)):"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e3a08a6a-867c-4243-8327-f2082b79b499",
   "metadata": {},
   "source": [
    "$$\n",
    "T(p,r,h) = \\begin{bmatrix}\n",
    "{\\cos(h) \\cdot \\cos(p)}  &\n",
    "{\\hskip1em \\cos(h) \\cdot \\sin(p) \\cdot \\sin(r) - \\sin(h) \\cdot \\cos(r)} &\n",
    "{\\hskip1em \\cos(h) \\cdot \\sin(p) \\cdot \\cos(r) + \\sin(h) \\cdot \\sin(r)} \\\\\n",
    "\\sin(h) \\cdot \\cos(p) &\n",
    "\\hskip1em \\sin(h) \\cdot \\sin(p) \\cdot \\sin(r) + \\cos(h) \\cdot \\cos(r) &\n",
    "\\hskip1em \\sin(h) \\cdot \\sin(p) \\cdot \\cos(r) - \\cos(h) \\cdot \\sin(r) \\\\\n",
    "-\\sin(p) &\n",
    "\\hskip1em \\cos(p) \\cdot \\sin(r) &\n",
    "\\hskip1em \\cos(p) \\cdot \\cos(r) \n",
    "\\end{bmatrix}\n",
    "\\tag{1}\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ed230b75-26c0-4cca-8593-cdfbdd16defc",
   "metadata": {},
   "source": [
    "The matrix $R(\\phi,\\lambda)$ transforms the camera sensor's location to ECEF coordinates using the GPS antenna's phase center geodetic latitude $\\phi$ and longitude $\\lambda$: "
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4c3f11ec-11e2-4eb6-9626-54a0d3e85a2a",
   "metadata": {},
   "source": [
    "$$\n",
    "R(\\phi,\\lambda) = \\begin{bmatrix}\n",
    "{-\\sin\\phi \\cdot \\cos\\lambda} &\n",
    "{\\hskip1em -\\sin\\lambda} &\n",
    "{\\hskip1em -\\cos\\phi \\cdot \\cos\\lambda} \\\\\n",
    "{-\\sin\\phi \\cdot \\sin\\lambda} & \n",
    "{\\hskip1em \\phantom{-}\\cos\\lambda} &\n",
    "{\\hskip1em -\\cos\\phi \\cdot \\sin\\lambda} \\\\\n",
    "{\\hskip1em \\cos\\phi} & {\\hskip1em \\phantom{-}0} & {\\hskip1em -\\sin\\phi}\n",
    "\\end{bmatrix}\n",
    "\\tag{2}\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ae4a71af-06a6-4e9e-ae9d-0e3d370be5ae",
   "metadata": {},
   "source": [
    "The location of the camera sensor's focal plane $r_{camera}(ECEF)$ is then calculated using the lever arm $r_{lever}$ in the aircraft-based coordinate system:"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4849823c-0300-4dfd-b54e-02bac72ca18e",
   "metadata": {},
   "source": [
    "$$\n",
    "r_{camera}(ECEF) \\hskip1em = \\hskip1em r_{ant}(ECEF) \\hskip1em + \\hskip1em T(p,r,h) \\cdot R(\\phi,\\lambda) \\cdot r_{lever}\n",
    "\\tag{3}\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "dff49167-29de-4403-98e0-9422edc22a05",
   "metadata": {},
   "source": [
    "The camera's sensor position in ECEF coordinates is then transformed to geographic coordinates using the Python™ [pyproj](https://github.com/pyproj4/pyproj) module.  \n",
    "  \n",
    "***\n",
    "  \n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4e4dac14-9330-4bd8-864e-1eb09eb411bf",
   "metadata": {},
   "source": [
    "## Python™ code for converting GPS locations to sensor locations\n",
    "First, load the required modules and set up the input and output file names for conversion."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "48f0eac2-c36c-4295-9d33-88ce876079cb",
   "metadata": {},
   "outputs": [],
   "source": [
    "import pandas as pd\n",
    "import numpy as np\n",
    "import pyproj\n",
    "import re\n",
    "import os\n",
    "\n",
    "# set input directory with ATM aux navigation file for conversion\n",
    "f_dir_nav = r\"..\" + os.sep + \"data\" + os.sep + \"nav\"\n",
    "\n",
    "# set input and output file names\n",
    "f_name_aux = f_dir_nav + os.sep + \"IOCAM0_2019_GR_NASA_20190906_ancillary_data.csv\"\n",
    "f_name_asp = f_dir_nav + os.sep + \"IOCAM0_2019_GR_NASA_20190906_camera_positions.csv\"\n",
    "\n",
    "VERBOSE = False # displays extracted parameters from the header of ATM aux file if desired"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e698e559-4c3a-43d0-ad5e-cd18e27f26e9",
   "metadata": {},
   "source": [
    "ATM auxiliary (aux) file format:\n",
    "\n",
    ">`# CAMBOT positioning and installation info for 2019_GR_NASA on GV (N95NA)`  \n",
    ">`# Input ancillary file: 20190906_CAMBOT_ancillary_precise.csv, updated: Mon Jul 13 17:20:49 2020`  \n",
    ">`# Camera offset from GPS antenna (in meters) [x-forward, y-starboard, z-down]: -4.463, 0.092, 2.042`  \n",
    ">`# Camera angular mounting biases (in deg, to be added to attitude measurements) [pitch, roll, heading]: 0.0, 0.0, 0.0`  \n",
    ">`# Range bias (in m, already added to range measurement): 0`  \n",
    ">`# Time offset (in sec, to be added to image timestamp to get UTC): -0.4338`  \n",
    ">`#         ImageFilename,             Timestamp(UTC), PosixTime(UTC),     Lat(deg),     Lon(deg), AntAlt(m),    AGL(m), Roll(deg), Pitch(deg), Heading(deg)`\n",
    ">`IOCAM0_2019_GR_NASA_20190906-112100.4216.jpg, 2019-09-06T11:21:00.000000, 1567768860.000,    76.493496,   -68.125623,  1193.617, >-9999.000,     5.518,      3.786,       55.536`  \n",
    ">`IOCAM0_2019_GR_NASA_20190906-112100.9217.jpg, 2019-09-06T11:21:00.500000, 1567768860.500,    76.493816,   -68.123564,  1193.840, -9999.000,     6`  \n",
    "\n",
    "The ASP camera position format can be set with processing parameters: `--csv-format \"1:file 2:lon 3:lat 4:height_above_datum\"` in the `camera_solve` tool.\n",
    "\n",
    ">`# ID, longitude, latitude, elevation`  \n",
    ">`2009_11_05_00266, -62.73216039, -62.95223574, 10089`  \n",
    ">`2009_11_05_00267, -62.71861428, -62.95946623, 10088`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "ddd9967d-9f66-4fe7-a184-f8fb1f27fe15",
   "metadata": {},
   "outputs": [],
   "source": [
    "# parse header of ATM aux file and extract all information from header\n",
    "# this section of the code was written by C. Wayne Wright (https://github.com/lidar532)\n",
    "\n",
    "def parse_cambot_header(\n",
    "    file_name:str,    # CAMBOTv2 file to read and extract header info from.\n",
    "    max_chars=2000,   # Maximum number of characters to read in.\n",
    "    debug:bool=False  # True to get debuggin printout, False for no debug printout\n",
    "    ) -> object:      # A class containing the extracted data if it was found.\n",
    "  \"\"\"\n",
    "  Parse a CAMBOT header and extract the fwd, right, down, pitch, roll, heading, Time_Offset, and Range_Bias values.\n",
    "  and return a class with those values.\n",
    "  asof: 2024-0214.\n",
    "  \"\"\"\n",
    "\n",
    "  # regex expressions built with: https://regex101.com/\n",
    "  rx_offset       = r\"\\[x-forward, y-starboard, z-down\\]: (?P<fwd>-*\\d+\\.\\d*), (?P<right>-*\\d+\\.\\d+), *(?P<down>-*\\d*\\.\\d*)\"\n",
    "  rx_attitude     = r\"\\[pitch, roll, heading\\]: (?P<pitch>-*\\d+\\.\\d*), *(?P<roll>-*\\d*\\.\\d*), *(?P<heading>-*\\d\\.\\d*)\"\n",
    "  rx_time_offset  = r\"Time offset .*: *(?P<Time_Offset>-*\\d\\.\\d*)\"\n",
    "  rx_range_bias   = r\"Range bias.*: *(?P<Range_Bias>-*\\d*\\.*\\d)\"\n",
    "  rx_file_name    = r\"Input ancillary file: *(?P<aux_file_name>.*),\"\n",
    "  rx_header_row   = r\"# *(ImageFilename)\"\n",
    "  row_number      = 0\n",
    "\n",
    "  cb_fd = open(f_name_aux, 'r')\n",
    "  cb_header = cb_fd.readlines(max_chars)\n",
    "  if debug:\n",
    "    for line in cb_header:\n",
    "      print(f\"line={line}\", end=\"\")\n",
    "  cb_fd.close()\n",
    "\n",
    "  # define a class to return the data we are able to extract\n",
    "  class CB_HEADER():\n",
    "    def __init__(self):\n",
    "      self.header_row     = None\n",
    "      self.fwd            = None\n",
    "      self.right          = None\n",
    "      self.down           = None\n",
    "      self.pitch          = None\n",
    "      self.roll           = None\n",
    "      self.heading        = None\n",
    "      self.Time_Offset    = None\n",
    "      self.Range_Bias     = None\n",
    "      self.header_row     = None\n",
    "      self.data_start_row = None\n",
    "      self.headers        = None\n",
    "\n",
    "  # instantiate the return variable \"rv\" as a CB_HEADER class\n",
    "  rv = CB_HEADER()\n",
    "\n",
    "  # walk over the lines of the header\n",
    "  for line in cb_header:\n",
    "    if debug:\n",
    "      print(f'{row_number:3d}: {line}', end='')\n",
    "    x = re.search(rx_offset, line, flags=0)\n",
    "    if x:\n",
    "      rv.fwd   = float(x.group('fwd'))\n",
    "      rv.right = float(x.group('right'))\n",
    "      rv.down  = float(x.group('down') )\n",
    "      if debug:\n",
    "        print(f'        {rv.fwd = } {rv.right = } {rv.down = }')\n",
    "\n",
    "    x = re.search(rx_attitude, line, flags=0)\n",
    "    if x:\n",
    "      rv.pitch   = float(x.group('pitch'))\n",
    "      rv.roll    = float(x.group('roll'))\n",
    "      rv.heading = float(x.group('heading'))\n",
    "      if debug:\n",
    "        print(f'      {rv.pitch = } {rv.roll = } {rv.heading = }')\n",
    "\n",
    "    x = re.search(rx_time_offset, line, flags=0)\n",
    "    if x:\n",
    "      rv.Time_Offset = float(x.group('Time_Offset'))\n",
    "      if debug:\n",
    "        print(f'{rv.Time_Offset = }')\n",
    "\n",
    "    x = re.search(rx_range_bias, line, flags=0)\n",
    "    if x:\n",
    "      rv.Range_Bias = float(x.group('Range_Bias'))\n",
    "      if debug:\n",
    "        print(f' {rv.Range_Bias = }')\n",
    "\n",
    "    x = re.search(rx_file_name, line, flags=0)\n",
    "    if x:\n",
    "      rv.aux_file_name = x.group('aux_file_name')\n",
    "      if debug:\n",
    "        print(f' {rv.aux_file_name = }')\n",
    "\n",
    "    x = re.search(rx_header_row, line, flags=0)\n",
    "    if x:\n",
    "      rv.row_number = row_number\n",
    "      rv.header_row = line\n",
    "      if debug:\n",
    "        print(f' {row_number = }')\n",
    "    row_number += 1\n",
    "\n",
    "  if rv.header_row:\n",
    "    rv.data_start_row = rv.row_number + 1\n",
    "    rv.header_row = rv.header_row.strip()\n",
    "    rv.headers = rv.header_row.replace(\"(\", \"_\").replace(\")\", \"\").replace(\" \", \"\").replace(\"#\",\"\").replace(\">\",\"\").split(\",\")\n",
    "  return rv\n",
    "\n",
    "# read header from ATM aux file and extract parameters\n",
    "header_data = parse_cambot_header( f_name_aux , debug=False )\n",
    "if VERBOSE:\n",
    "    for k in header_data.__dict__:\n",
    "      print(f'{k:15s}: {header_data.__dict__[k]}' )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "c33958de-d1a4-4682-8bb0-a156341e8f94",
   "metadata": {},
   "outputs": [],
   "source": [
    "# extract lever arm parameters from class if they exist\n",
    "if (hasattr(header_data, 'fwd') &  hasattr(header_data, 'right') &  hasattr(header_data, 'down')):\n",
    "    lever_arm_sensor = np.array([header_data.fwd, header_data.right, header_data.down])\n",
    "else:\n",
    "    os.sys.exit(\"Unable to extract lever arm parameters from ATM aux file. Check input data.\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "6763a578-f5e7-4800-a9ce-e6b9ff46885d",
   "metadata": {},
   "outputs": [],
   "source": [
    "# read data from ATM aux file\n",
    "df = pd.read_csv(f_name_aux, skiprows=header_data.data_start_row, names=header_data.headers)\n",
    "if VERBOSE:\n",
    "    print(df.head())  "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "c24033ef-b5bf-4397-85ec-540f3a3bc6ee",
   "metadata": {},
   "outputs": [],
   "source": [
    "# populate numpy arrays with needed data\n",
    "\n",
    "ant_lon = np.asarray(df[\"Lon_deg\"])   # float64\n",
    "ant_lat = np.asarray(df[\"Lat_deg\"])   # float64\n",
    "ant_ele = np.asarray(df[\"AntAlt_m\"])  # float64\n",
    "\n",
    "pitch   = np.asarray(df[\"Pitch_deg\"]) # float64\n",
    "roll    = np.asarray(df[\"Roll_deg\"])  # float64\n",
    "yaw     = np.asarray(df[\"Heading_deg\"])  # float64\n",
    "\n",
    "agl     = np.asarray(df[\"AGL_m\"])   # float64\n",
    "\n",
    "# convert coordinate and attitude angles from degrees to radians and use vectorizationg for faster execution\n",
    "lon_rad = np.deg2rad(ant_lon)\n",
    "lat_rad = np.deg2rad(ant_lat)\n",
    "p_rad = np.deg2rad(pitch)\n",
    "r_rad = np.deg2rad(roll)\n",
    "y_rad = np.deg2rad(yaw)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "94936cc2-2060-42d3-b94c-c0d184376b9f",
   "metadata": {},
   "outputs": [],
   "source": [
    "# set up map projections for geodetic (latlong) to geocentric (ECEF) and back\n",
    "transformer_geo2ecef = pyproj.Transformer.from_crs(\n",
    "    {\"proj\":'latlong', \"ellps\":'WGS84', \"datum\":'WGS84'},\n",
    "    {\"proj\":'geocent', \"ellps\":'WGS84', \"datum\":'WGS84'},\n",
    "    )\n",
    "transformer_ecef2geo = pyproj.Transformer.from_crs(\n",
    "    {\"proj\":'geocent', \"ellps\":'WGS84', \"datum\":'WGS84'},\n",
    "    {\"proj\":'latlong', \"ellps\":'WGS84', \"datum\":'WGS84'},\n",
    "    )\n",
    "# convert phase center geodetic positions to Earth-centered, Earth-fixed coordinates (ECEF) \n",
    "x_ant_ecef, y_ant_ecef, z_ant_ecef = transformer_geo2ecef.transform(ant_lon,ant_lat,ant_ele,radians=False)\n",
    "del ant_lon, ant_lat"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0f8f9996-f865-4a5b-b4a5-67b5ecb343b6",
   "metadata": {},
   "source": [
    "The following function does the coordinate transformations described above. An equivalent MATLAB® implementation `atm_GPS_llh_2_sensor_ECEF.m` of this function is available from the GitHub repository [ATM-Bathymetry-Toolkit](https://github.com/mstudinger/ATM-Bathymetry-Toolkit)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "f95e7180-2d3c-4fc9-a5f0-54d2a920bc09",
   "metadata": {},
   "outputs": [],
   "source": [
    "# =============================================================================\n",
    "# helper function for converting antenna phase center ECEF coordinates\n",
    "# to sensor ECEF coordinates using lever arm and attitude information\n",
    "# =============================================================================\n",
    "\n",
    "def ecef_ant_pos_to_sensor_pos(ant_lon,ant_lat,x_ant_ecef, y_ant_ecef, z_ant_ecef,pitch, roll, yaw, lever_arm):\n",
    "\n",
    "    \"\"\"\n",
    "    SUMMARY:       The ecef_ant_pos_to_sensor_pos function calculates the location of an instrument's sensor using the geodetic coordinates of the \n",
    "                   phase center of the aircraft's GPS antenna, pitch, roll, and yaw (heading) and the instrument's lever arm measured in an\n",
    "                   aircraft-fixed cartesian coordinate system. The output is in a geocentric ECEF (Earth-centered, Earth-fixed) coordinate system \n",
    "    INPUT:  \n",
    "    ant_lon        array with geodetic longitudes of the GPS antenna's phase center in radians\n",
    "    ant_lat        array with geodetic latitudes  of the GPS antenna's phase center in radians\n",
    "    x_ant_ecef     array with geocentric ECEF x coordinates of the GPS antenna's phase center in meters\n",
    "    y_ant_ecef     array with geocentric ECEF y coordinates of the GPS antenna's phase center in meters\n",
    "    z_ant_ecef     array with geocentric ECEF z coordinates of the GPS antenna's phase center in meters\n",
    "    pitch          aircraft pitch angle in radians from IMU data. positive pitch = aircraft nose up. \n",
    "                   this parameter is also stored in /aircraft/pitch in ATM's waveform HDF5 files available from NSDIC (data products ILATMW1B and ILNSAW1B).\n",
    "    roll           aircraft roll angle in radians from IMU data. positive roll = starboard (right) wing down.\n",
    "                   this parameter is also stored in /aircraft/pitch in ATM's waveform HDF5 files available from NSDIC (data products ILATMW1B and ILNSAW1B).\n",
    "    yaw            aircraft heading/yaw angle in radians true north from IMU data. positive towards east. \n",
    "                   this parameter is also stored in /aircraft/heading in ATM's waveform HDF5 files available from NSDIC (data products ILATMW1B and ILNSAW1B).\n",
    "    lever_arm      3×1 displacement vector of the center of the camera's focal plane measured from the phase center\n",
    "                   of the aircraft's GPS antenna towards the camera in an aircraft-fixed cartesian coordinate system.\n",
    "                       x: positive forward\n",
    "                       y: positive starboard\n",
    "                       z: positive down\n",
    "                   lever_arm = [lever_arm_x, lever_arm_y, lever_arm_z]\n",
    "                   \n",
    "    OUTPUT:        vector with ECEF coordinates of the camera sensors's focal plane\n",
    "    SYNTAX:        sensor_ecef = ecef_ant_pos_to_sensor_pos(ant_ecef_x, ant_ecef_y, ant_ecef_z, pitch, roll, heading,lever_arm)\n",
    "    \"\"\"\n",
    "    \n",
    "    ant_ecef = np.array([x_ant_ecef, y_ant_ecef, z_ant_ecef])\n",
    "    \n",
    "    # set up rotation matrix to align sensor coordinate system with aircraft coordinate system with the x-axis in the direction of North\n",
    "    # the T(heading,pitch,roll) rotation aligns the sensor’s coordinate system with the aircraft coordinate system, with the x-axis in the direction of the North\n",
    "    cp = np.cos(pitch)\n",
    "    sp = np.sin(pitch)\n",
    "    cr = np.cos(roll)\n",
    "    sr = np.sin(roll)\n",
    "    ch = np.cos(yaw)\n",
    "    sh = np.sin(yaw)\n",
    "    \n",
    "    # equation (1) \n",
    "    T = np.array([ [ch*cp, ch*sp*sr-sh*cr, ch*sp*cr+sh*sr],\n",
    "          [sh*cp, sh*sp*sr+ch*cr, sh*sp*cr-ch*sr],\n",
    "          [-1.0*sp,          cp*sr,          cp*cr ]])\n",
    "    \n",
    "    # rotate the sensor coordinates into a local North, East, Down (NED) coordinate system\n",
    "    st = np.sin(ant_lat)\n",
    "    ct = np.cos(ant_lat)\n",
    "    sl = np.sin(ant_lon)\n",
    "    cl = np.cos(ant_lon)\n",
    "    \n",
    "    # equation (2) \n",
    "    NED_R = np.array([ [-1.0*st*cl, -1.0*sl, -1.0*ct*cl],\n",
    "                [-1.0*st*sl ,     cl , -1.0*ct*sl],\n",
    "                        [ct  ,     0  ,  -1.0*st] ])    \n",
    "    # equation (3) \n",
    "    sensor_ecef = NED_R @ T @ lever_arm_sensor + ant_ecef\n",
    " \n",
    "    return sensor_ecef"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "abfefdad-1e8c-4f07-9ac6-14bf87cabac3",
   "metadata": {},
   "outputs": [],
   "source": [
    "# allocate empty arrays for faster execution - NOTE: calculations need to be done with float64 to get the required accuracy\n",
    "sensor_ecef_x = np.empty(ant_ele.size) # float64\n",
    "sensor_ecef_y = np.empty(ant_ele.size) # float64\n",
    "sensor_ecef_z = np.empty(ant_ele.size) # float64\n",
    "\n",
    "for i in range(ant_ele.size):\n",
    "   sensor_ecef = ecef_ant_pos_to_sensor_pos(lon_rad[i],lat_rad[i],x_ant_ecef[i], y_ant_ecef[i], z_ant_ecef[i],p_rad[i], r_rad[i], y_rad[i], lever_arm_sensor)\n",
    "   sensor_ecef_x[i] = sensor_ecef[0]\n",
    "   sensor_ecef_y[i] = sensor_ecef[1]\n",
    "   sensor_ecef_z[i] = sensor_ecef[2]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "ee1d1db3-2151-4557-b645-bfa496a42344",
   "metadata": {},
   "outputs": [],
   "source": [
    "# transform sensor ECEF coordinates to geodetic\n",
    "lon_sen, lat_sen, alt_sen = transformer_ecef2geo.transform(sensor_ecef_x,sensor_ecef_y,sensor_ecef_z,radians=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "f4ff0fc5-3c64-47c9-b397-066185450472",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Converted 42691 phase center positions to camera focal plane positions: ..\\data\\nav\\IOCAM0_2019_GR_NASA_20190906_camera_positions.csv\n"
     ]
    }
   ],
   "source": [
    "# export new coordinates and also include attitude information\n",
    "f = open(f_name_asp, 'w')\n",
    "f.write(\"# ID, longitude_deg, latitude_deg, elevation_m, pitch_deg, roll_deg, yaw_deg\\n\")\n",
    "for i in range(ant_ele.size):\n",
    "    f.write(f'{df.iloc[i, 0]:s},{lon_sen[i]:25.20f},{lat_sen[i]:25.20f},{alt_sen[i]:10.4f} ,{pitch[i]:10.4f}, {roll[i]:10.4f},{yaw[i]%360:10.4f}\\n')\n",
    "f.close()\n",
    "\n",
    "print(f'Converted {ant_ele.size:d} phase center positions to camera focal plane positions: {f_name_asp:s}')"
   ]
  }
 ],
 "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.11.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}