TimeOfArrivalExample.ipynb

{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "arMydWfHifPF"
      },
      "source": [
        "# Localization with time of arrival measurements\n",
        "\n",
        "<a href=\"https://colab.research.google.com/github/gtbook/gtsam-examples/blob/main/TimeOfArrivalExample.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "7ER921sUifPI"
      },
      "source": [
        "## Introduction\n",
        "\n",
        "Acoustic sensors can be used as a mean to track a moving object. Typically, acoustic measurements are characteristic of the distance between the receiver and an acoustic source. Hence, they convey information on the position of the latter, which can be leveraged for localization. In the next, we will see how gtsam can be used in such a scenario. \n",
        "\n",
        "The following example considers an omnidirectional sound source that periodically emits an acoustic signal while moving. The signals are received by a fixed array of microphones that can synchronously record the signals from which Time of Arrival (ToA) measurements are extracted. The objective is to recover, from the ToA measurements, the trajectory of the moving agent. In this simple example, no odometry information is used, but odometry factors can seamlessly be integrated."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "JoW4C_OkOMhe",
        "tags": [
          "remove-cell"
        ],
        "trusted": true
      },
      "outputs": [],
      "source": [
        "# Install the pre-requisites\n",
        "%pip -q install gtbook "
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "id": "10-snNDwOSuC",
        "tags": [
          "remove-cell"
        ],
        "trusted": true
      },
      "outputs": [],
      "source": [
        "import numpy as np\n",
        "import matplotlib.pyplot as plt\n",
        "import gtsam\n",
        "\n",
        "from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams,\n",
        "                   NonlinearFactorGraph, Point3, Values, noiseModel)\n",
        "from gtsam_unstable import Event, TimeOfArrival, TOAFactor"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "nAvx4-UCNzt2",
        "tags": []
      },
      "source": [
        "## Example trajectory\n",
        "\n",
        "We start by defining the microphones position and create the ground truth trajectory along with sound events. We first define useful units."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "id": "X0BsBHPGifPN",
        "trusted": true
      },
      "outputs": [],
      "source": [
        "MS = 1e-3 # 1 millisecond in s\n",
        "CM = 1e-2 # 1 centimeter in m"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "QpwKGbRylV-7"
      },
      "source": [
        " Next, we istantiate a functor with a value $v = 330 m.s^{-1}$ for the speed of sound."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {
        "id": "xirrSUgtifPP",
        "trusted": true
      },
      "outputs": [],
      "source": [
        "TIME_OF_ARRIVAL = TimeOfArrival(330)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Ykno7J8MifPP"
      },
      "source": [
        "We define the positions of four microphones that we constrain to lie in a horizontal plane (*i.e* with constant $z$ coordinate)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "jZE77ghYifPQ",
        "outputId": "50d4d96f-55fb-4b3d-ae08-d7508737725d",
        "trusted": true
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "mic 0 = [0.  0.  0.5]\n",
            "mic 1 = [4.03 0.   0.5 ]\n",
            "mic 2 = [4.03 4.03 0.5 ]\n",
            "mic 3 = [0.   4.03 1.  ]\n"
          ]
        }
      ],
      "source": [
        "height = 0.5\n",
        "microphones = []\n",
        "microphones.append(Point3(0, 0, height))\n",
        "microphones.append(Point3(403 * CM, 0, height))\n",
        "microphones.append(Point3(403 * CM, 403 * CM, height))\n",
        "microphones.append(Point3(0, 403 * CM, 2 * height))\n",
        "\n",
        "K = len(microphones)\n",
        "for i in range(K):\n",
        "    print(\"mic {} = {}\".format(i, microphones[i]))"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "c0Q43BDxifPT"
      },
      "source": [
        "We subsequently create a ground truth trajectory for the moving sound emitter which follows a straight line, and specify the times at which a sound is emitted."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "metadata": {
        "id": "wO5_ocxiifPU",
        "tags": [],
        "trusted": true
      },
      "outputs": [],
      "source": [
        "n = 5\n",
        "groundTruth = []\n",
        "timeOfEvent = 10\n",
        "\n",
        "# simulate emitting a sound every second while moving on straight line\n",
        "for key in range(n):\n",
        "    groundTruth.append(\n",
        "        Event(timeOfEvent, 245 * CM + key * 1.0, 201.5 * CM, (212 - 45) * CM))\n",
        "    timeOfEvent += 1\n",
        "        \n",
        "#for event in groundTruth:\n",
        "#    event.print()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "NvXX8a2YifPU"
      },
      "source": [
        "For visualization, we display the agent positions corresponding to an excitation event, along with the microphone array."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 7,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 279
        },
        "id": "L_tLoHdOifPV",
        "outputId": "9b0e3587-b124-439d-bb41-ed46f2b49370"
      },
      "outputs": [
        {
          "data": {
            "image/png": "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",
            "text/plain": [
              "<Figure size 432x288 with 1 Axes>"
            ]
          },
          "metadata": {
            "needs_background": "light"
          },
          "output_type": "display_data"
        }
      ],
      "source": [
        "fig = plt.figure()\n",
        "plt.plot([mic[0] for mic in microphones], [mic[1] for mic in microphones], \"b.\", markersize=10, label=\"Microphones\")\n",
        "plt.plot([ev.location()[0] for ev in groundTruth], [ev.location()[1] for ev in groundTruth], \"s\", color=\"r\", markersize=5, linestyle=\"-\", lw=0.5, label=\"Source positions\")\n",
        "plt.xlabel(\"x [m]\"); plt.ylabel(\"y [m]\")\n",
        "plt.legend(); fig.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "MkerMqa9ifPW"
      },
      "source": [
        "We can now simulate the time of arrival measurements based on the distance between each microphone and the moving sound source. To determine the (noise-free) ToA between microphone $j$ for the $i$-th source position, one may simply use the formula:\n",
        "\n",
        "$$ z_{i, j} = \\frac{d_{i, j}}{v}, $$\n",
        "\n",
        "where $d_{i, j}$ is the distance between them. Otherwise, the function TIME_OF_ARRIVAL.measure can take care of it."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 8,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "EdsVLXlXifPW",
        "outputId": "cd736783-df1a-4f26-abe2-ad897ea8bd24",
        "trusted": true
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "z_00 = 10010.245662478956 ms\n",
            "z_01 = 10008.531002716338 ms\n",
            "z_02 = 10008.531002716338 ms\n",
            "z_03 = 10009.824738271 ms\n",
            "z_10 = 11012.615535847513 ms\n",
            "z_11 = 11007.276214441754 ms\n",
            "z_12 = 11007.276214441754 ms\n",
            "z_13 = 11012.276140565378 ms\n",
            "z_20 = 12015.22154271807 ms\n",
            "z_21 = 12007.174542408371 ms\n",
            "z_22 = 12007.174542408371 ms\n",
            "z_23 = 12014.941460610633 ms\n",
            "z_30 = 13017.961192990088 ms\n",
            "z_31 = 13008.268633130547 ms\n",
            "z_32 = 13008.268633130547 ms\n",
            "z_33 = 13017.724455875657 ms\n",
            "z_40 = 14020.78169909914 ms\n",
            "z_41 = 14010.17998044382 ms\n",
            "z_42 = 14010.17998044382 ms\n",
            "z_43 = 14020.577436670008 ms\n"
          ]
        }
      ],
      "source": [
        "def simulate_one_toa(microphones, event):\n",
        "    \"\"\"Simulate time-of-arrival measurements for a single event.\"\"\"\n",
        "    return [TIME_OF_ARRIVAL.measure(event, microphones[i])\n",
        "            for i in range(len(microphones))]\n",
        "\n",
        "simulatedTOA = [simulate_one_toa(microphones, event)\n",
        "        for event in groundTruth]\n",
        "\n",
        "for key in range(n):\n",
        "    for i in range(K):\n",
        "        print(\"z_{}{} = {} ms\".format(key, i, simulatedTOA[key][i] / MS))"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "qR0JNEZlifPX"
      },
      "source": [
        "## Creation of the nonlinear factor graph\n",
        "\n",
        "As always, we create a factor graph to solve the problem at hand. "
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 9,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "rJ0D8cGFifPX",
        "outputId": "6480b693-c47a-4b46-df73-e74b95628cf0",
        "trusted": true
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "  keys = { 0 }\n",
            "isotropic dim=1 sigma=0.0005\n",
            "ExpressionFactor with measurement:  10.0102\n",
            "\n"
          ]
        }
      ],
      "source": [
        "graph = NonlinearFactorGraph()\n",
        "\n",
        "# Create a noise model for the TOA error\n",
        "model = noiseModel.Isotropic.Sigma(1, 0.5 * MS)\n",
        "\n",
        "K = len(microphones)\n",
        "key = 0\n",
        "for toa in simulatedTOA:\n",
        "    for i in range(K):\n",
        "        factor = TOAFactor(key, microphones[i], toa[i], model)\n",
        "        graph.push_back(factor)\n",
        "    key += 1\n",
        "        \n",
        "print(graph.at(0))"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "t3FYzhipifPX"
      },
      "source": [
        "## Optimization and results\n",
        "\n",
        "The optimizer needs to be provided an initial trajectory estimate."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 10,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "Tk5NVy_7ifPY",
        "outputId": "b9981d9c-ed84-450b-f1de-08a73ac42305"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Values with 5 values:\n",
            "Value 0: (gtsam::Event)\n",
            "{'time':0, 'location': 0 0 0}\n",
            "Value 1: (gtsam::Event)\n",
            "{'time':0, 'location': 0 0 0}\n",
            "Value 2: (gtsam::Event)\n",
            "{'time':0, 'location': 0 0 0}\n",
            "Value 3: (gtsam::Event)\n",
            "{'time':0, 'location': 0 0 0}\n",
            "Value 4: (gtsam::Event)\n",
            "{'time':0, 'location': 0 0 0}\n",
            "\n"
          ]
        }
      ],
      "source": [
        "initial_estimate = Values()\n",
        "zero = Event()\n",
        "for key in range(n):\n",
        "    TOAFactor.InsertEvent(key, zero, initial_estimate)\n",
        "print(initial_estimate)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "cCTqrJa1ifPY"
      },
      "source": [
        "The graph is optimized using the Levenberg-Marquardt optimizer."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 11,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "7VNSYxFOifPa",
        "outputId": "3c3c432d-16b3-4f8c-9a04-ea8a1d8997c9"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Final Result:\n",
            " Values with 5 values:\n",
            "Value 0: (gtsam::Event)\n",
            "{'time':10, 'location':  2.45 2.015  1.67}\n",
            "Value 1: (gtsam::Event)\n",
            "{'time':11, 'location':  3.45 2.015  1.67}\n",
            "Value 2: (gtsam::Event)\n",
            "{'time':12, 'location':  4.45 2.015  1.67}\n",
            "Value 3: (gtsam::Event)\n",
            "{'time':13, 'location':  5.45 2.015  1.67}\n",
            "Value 4: (gtsam::Event)\n",
            "{'time':14, 'location':  6.45 2.015  1.67}\n",
            "\n"
          ]
        }
      ],
      "source": [
        "params = LevenbergMarquardtParams()\n",
        "params.setAbsoluteErrorTol(1e-10)\n",
        "params.setVerbosityLM(\"SUMMARY\")\n",
        "optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params)\n",
        "result = optimizer.optimize()\n",
        "print(\"Final Result:\\n\", result)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "kmxKML2RifPa"
      },
      "source": [
        "The visual representation of the result shows the excelent matching between estimated and ground truth positions."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 12,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 279
        },
        "id": "fZDgkEfTifPa",
        "outputId": "73e33986-88f9-4b70-abf4-68fa5517c949"
      },
      "outputs": [
        {
          "data": {
            "image/png": "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",
            "text/plain": [
              "<Figure size 432x288 with 1 Axes>"
            ]
          },
          "metadata": {
            "needs_background": "light"
          },
          "output_type": "display_data"
        }
      ],
      "source": [
        "# parsing the result to retrieve the estimated locations\n",
        "L = result.__repr__().split('\\n'); L = [L[ind] for ind in range(2, len(L), 2)]\n",
        "L = [l.split('location')[1].split()[1:] for l in L]\n",
        "estimated_trajectory = [ [float(coord.split('}')[0]) for coord in l] for l in L]\n",
        "\n",
        "# Display\n",
        "fig = plt.figure()\n",
        "plt.plot([mic[0] for mic in microphones], [mic[1] for mic in microphones], \"b.\", markersize=10, label=\"Microphones\")\n",
        "plt.plot([ev.location()[0] for ev in groundTruth], [ev.location()[1] for ev in groundTruth], \"s\", color=\"r\", markersize=5, linestyle=\"-\", lw=0.5, label=\"Source positions\")\n",
        "plt.plot([x[0] for x in estimated_trajectory], [x[1] for x in estimated_trajectory], \"s\", color=\"g\", markersize=5, linestyle=\"-\", lw=0.5, label=\"Estimated positions\")\n",
        "plt.xlabel(\"x [m]\"); plt.ylabel(\"y [m]\")\n",
        "plt.legend(); fig.show()"
      ]
    }
  ],
  "metadata": {
    "colab": {
      "collapsed_sections": [],
      "name": "TimeOfArrivalExample.ipynb",
      "provenance": []
    },
    "interpreter": {
      "hash": "c6e4e9f98eb68ad3b7c296f83d20e6de614cb42e90992a65aa266555a3137d0d"
    },
    "kernelspec": {
      "display_name": "Python 3",
      "name": "python3"
    },
    "language_info": {
      "name": "python"
    },
    "latex_metadata": {
      "affiliation": "International Research Lab 2958 Georgia Tech-Cnrs",
      "author": "Othmane-Latif Ouabi",
      "title": "Localization_with_time_of_arrival_measurements"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}