tuto_slam_elements.ipynb

{
 "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Graph-SLAM types and helper methods\n",
    "\n",
    "This tuto introduces you to the variable types that we defined for our SLAM problem, and also to some methods that we pre-programmed for you.\n",
    "\n",
    "## Overview\n",
    "\n",
    "Our SLAM system consists of a **single monocular camera**, and some markers or **tags from the AprilTag** family, which we will consider as our landmarks.\n",
    "\n",
    "Our **factor graph** will have _keyframes_ and _landmarks_, and _factors_ relating them.\n",
    "\n",
    "Geometrically, each **keyframe** in the graph will be a **3D pose**.\n",
    "\n",
    "Also, each **landmark** in the graph will be a **3D pose**.\n",
    "\n",
    "**Factors** in the graph will be of three different types:\n",
    "\n",
    "- **Motion factor**, from one keyframe to the next.\n",
    "\n",
    "- **Landmark factor**, from one keyframe to one landmark.\n",
    "\n",
    "- **Prior factor**, from one keyframe to some absolute value, or from one landmark to some absolute value.\n",
    "  **NOTE:** you can have a prior in the first keyframe, or a prior in one of the landmarks. You CANNOT impose both priors, since they would most certainly conflict with each other!\n",
    "\n",
    "Each factor will be associated with a particular **cost function**.\n",
    "\n",
    "The **total cost** to be minimized by the solver will be the sum of the costs of each one of the factors.\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "## Variable types\n",
    "\n",
    "### A note on 3D orientations in estimation and optimization\n",
    "\n",
    "The orientation part of the pose deserves special attention. \n",
    "3D orientations do not form a linear space, but a Lie group. One can represent such rotations in different ways:\n",
    "- Euler angles\n",
    "- Rotation matrix\n",
    "- Quaternion\n",
    "- Axis-Angle\n",
    "- Rotation vector, or Angle-vector\n",
    "\n",
    "All have their pros and cons:\n",
    "- Euler angles: \n",
    "\n",
    "    - :-)   intuitive, 3 parameters\n",
    "\n",
    "    - :-(   non-continuous, singularities, 12 different conventions!\n",
    "\n",
    "- Rotation matrix: \n",
    "\n",
    "    - :-)   easy to operate with\n",
    "\n",
    "    - :-(   9 parameters, highly redundant\n",
    "\n",
    "- Quaternion: \n",
    "\n",
    "    - :-)   fairly easy to operate\n",
    "\n",
    "    - :-(   4 parameters, some redundancy\n",
    "\n",
    "- Axis-Angle: \n",
    "\n",
    "    - :-)   intuitive\n",
    "\n",
    "    - :-(   difficult to operate, 4 parameters, redundant\n",
    "\n",
    "- Rotation vector, or Angle-vector: \n",
    "\n",
    "    - :-)   3 parameters, no singularities\n",
    "\n",
    "    - :-(   difficult to operate\n",
    "\n",
    "Since our solver IPOPT only understands variables in vector spaces and does not like redundant representations, we resort to the **rotation vector** representation.\n",
    "\n",
    "We circumvent the stated difficulty of operation by converting the rotation vector to a rotation matrix every time we need it.\n",
    "\n",
    "A rotation vector $w$ relates to its equivalent rotation matrix $R$ through the exponential map (or Rodrigues formula)\n",
    "$$\n",
    "R = \\exp(w_\\times) \\\\\n",
    "w_\\times = \\log(R)\n",
    "$$\n",
    "where $w_\\times$ is a skew-symmetric matrix built from the vector $w$, and $\\exp,\\log$ are the exponential map defined in SO(3).\n",
    "\n",
    "For this tuto, it suffices to say that our variable to estimate is $w$, and that we can go back and forth to its matrix equivalent $R$ using the exponential map.\n",
    "\n",
    "See the exponential map in action using Pinocchio for normal numeric variables:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import pinocchio as pin\n",
    "\n",
    "w = np.array([.1,.2,.3])\n",
    "\n",
    "# go from vector to SO(3)\n",
    "R = pin.exp(w)\n",
    "\n",
    "print('rot vector, w = ', w)\n",
    "print('rot matrix, R = \\n', R)\n",
    "\n",
    "# go from SO(3) to vector\n",
    "u = pin.log(R)\n",
    "\n",
    "print('rot vector, u = ', u)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And using Casadi for symbolic computations used by the optimizer:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import casadi\n",
    "from pinocchio import casadi as cpin\n",
    "from gslam_april_tools import *\n",
    "\n",
    "# Create the casadi optimization problem\n",
    "opti = casadi.Opti()\n",
    "\n",
    "# The optimization variable is the angle-vector w and the associated rotation R=exp(w)\n",
    "w = opti.variable(3)\n",
    "R = exp3(w)\n",
    "u = log3(R)\n",
    "\n",
    "print('rot vector w = ', w)\n",
    "print('rot matrix R = ', R)\n",
    "print('rot vector u = ', u)\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "\n",
    "### The class `OptiVariablePose3`\n",
    "\n",
    "This is a Pose3 object that can be used in Casadi for symbolic computations. Our keyframes and landmarks will be of this type.\n",
    "\n",
    "The class basically contains:\n",
    "- an **ID**\n",
    "- a **position** : a symbolic 3-vector \n",
    "- an **orientation** : a symbolic 3-vector, i.e. the rotation vector\n",
    "- a method to recover the rotation matrix\n",
    "- a constructor that accepts initial values for position and orientation\n",
    "\n",
    "This is the prototype and constructor:\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "\n",
    "class OptiVariablePose3:\n",
    "    def __init__(self, opti, id, position, anglevector):\n",
    "        self.id             = id\n",
    "        self.position       = opti.variable(3)\n",
    "        self.anglevector    = opti.variable(3)\n",
    "        self.R              = exp3(self.anglevector)\n",
    "        opti.set_initial(self.position, position)\n",
    "        opti.set_initial(self.anglevector, anglevector)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To create a keyframe, or a landmark, simply do:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# some necesary variables\n",
    "opti        = casadi.Opti()\n",
    "pos_example = np.array([1,2,3])\n",
    "ori_example = np.array([4,5,6])\n",
    "\n",
    "# create one keyframe!\n",
    "kf_id    = 1\n",
    "keyframe = OptiVariablePose3(opti, kf_id, pos_example, ori_example)\n",
    "\n",
    "# create one landmark!\n",
    "lmk_id   = 11\n",
    "landmark = OptiVariablePose3(opti, lmk_id, pos_example, ori_example)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Keyframes and landmarks are stored in `Dictionnaries`, indexed by their ID. Insertion is trivial:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "keyframes = dict()\n",
    "landmarks = dict()\n",
    "\n",
    "keyframes[kf_id]  = keyframe\n",
    "landmarks[lmk_id] = landmark"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, having an ID, one can retrieve the KF or the lmk easily, so access is also trivial:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "lmk = landmarks[lmk_id]\n",
    "kf  = keyframes[kf_id]"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Adding new keyframes (or landmarks) is easy, just remember to increment their ID each time:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# increment keyframe ID and create new keyframe:\n",
    "kf_id += 1\n",
    "keyframe = OptiVariablePose3(opti, kf_id, pos_example, ori_example)\n",
    "keyframes[kf_id]  = keyframe\n",
    "\n",
    "# recover landmark ID and create new landmark:\n",
    "lmk_id = 12  # = detection.tag_id  <-- this comes from the Apriltag detection, see tuto_apriltag.ipynb for details\n",
    "landmark = OptiVariablePose3(opti, lmk_id, pos_example, ori_example)\n",
    "landmarks[lmk_id]  = landmark"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can now see all KFs and lmks in your SLAM problem:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# will show IDs used as keys and stored in objects -- they must match!\n",
    "for kid in keyframes:\n",
    "    keyframe = keyframes[kid]\n",
    "    print('KF  # :', kid, keyframe.id)\n",
    "for lid in landmarks:\n",
    "    landmark = landmarks[lid]\n",
    "    print('lmk # :', lid, landmark.id)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### The class `Factor`\n",
    "\n",
    "Factors in our example are fairly simple. There are three types, but all can be stored in a single class:\n",
    "\n",
    "- Prior factor: concerns a single KF or a single Lmk\n",
    "\n",
    "- Motion factor: concerns two consecutive KFs\n",
    "\n",
    "- Landmark factor: concerns one KF and one lmk\n",
    "\n",
    "We define the class `Factor` as follows:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class Factor:\n",
    "    def __init__(self, type, id, i, j, meas, sqrt_info):\n",
    "        self.type       = type\n",
    "        self.id         = id\n",
    "        self.i          = i\n",
    "        self.j          = j\n",
    "        self.meas       = meas\n",
    "        self.sqrt_info  = sqrt_info"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The fields of `Factor` are explained as follows:\n",
    "- `type`: string with values in `{'prior_keyframe', 'prior_landmark', 'motion', 'landmark'}`\n",
    "- `id`: integer\n",
    "- `i`: the ID of the first concerned object\n",
    "- `j`: the ID of the second concerned object\n",
    "- `meas`: the measurement\n",
    "- `sqrt_info`: the square-root of the information matrix\n",
    "\n",
    "So to create a factor, you need to know each of these fields.\n",
    "\n",
    "Factors are stored in a `Dictionary`, indexed by their own ID.\n",
    "\n",
    "#### Prior factors\n",
    "\n",
    "One type of prior factor constrains one keyframe:\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Dictionary to store all factors\n",
    "factors = dict()\n",
    "\n",
    "# Factor details\n",
    "fac_id      = 0\n",
    "kf_id       = 1   # Id of the constrained KF\n",
    "dummy_id    = 0   # ignored in priors since only one state is constrained\n",
    "meas        = np.array([0,0,0,0,0,0])\n",
    "sqrt_info   = np.eye(6) / 1e-3 # 1mm, 1mrad, std deviation errors\n",
    "\n",
    "# Construct the factor!\n",
    "factor      = Factor('prior_keyframe', fac_id, kf_id, dummy_id, meas, sqrt_info)\n",
    "\n",
    "# Add it to the Dictionary!\n",
    "factors[fac_id] = factor\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A similar prior factor is used to constrain one landmark:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "fac_id      += 1\n",
    "lmk_id      = 11  # ID of the constrained landmark\n",
    "dummy_id    = 0   # ignored in priors since only one state is constrained\n",
    "meas        = np.array([0,0,0,0,0,0])\n",
    "sqrt_info   = np.eye(6) / 1e-3 # 1mm, 1mrad, std deviation errors\n",
    "\n",
    "factor      = Factor('prior_landmark', fac_id, lmk_id, dummy_id, meas, sqrt_info)\n",
    "\n",
    "factors[fac_id] = factor\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Motion factor\n",
    "\n",
    "It relates two consecutive keyframes:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "kf1_id      = 1   # ID of the first keyframe\n",
    "kf2_id      = 2   # ID of the second keyframe\n",
    "meas        = np.array([1,2,3,  3,2,1])  # motion measurement, translation and rotation\n",
    "sqrt_info   = np.eye(6) / 1e-3 # 1mm, 1mrad, std deviation errors\n",
    "\n",
    "fac_id += 1   # increase factor ID counter\n",
    "factor      = Factor('motion', fac_id, kf1_id, kf2_id, meas, sqrt_info)\n",
    "\n",
    "factors[fac_id] = factor\n",
    "\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Landmark factor\n",
    "\n",
    "It relates a keyframe and a landmark measured from that keyframe:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "kf_id       = 1      # ID of the keyframe\n",
    "lmk_id      = 11 # ID of the landmark\n",
    "meas        = np.array([1,2,3,  3,2,1])  # measured relative pose of tag wrt. camera (see tuto_apriltag for details)\n",
    "sqrt_info   = np.eye(6) / 1e-3 # 1mm, 1mrad, std deviation errors\n",
    "\n",
    "fac_id += 1\n",
    "factor = Factor('landmark', fac_id, kf_id, lmk_id, meas, sqrt_info)\n",
    "\n",
    "factors[fac_id] = factor\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Knowing what kfs or lmks are referred to by the factor is easy: check the factor's `type` and recover the appropriate objects accordingly:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for fid in factors:\n",
    "    \n",
    "    factor = factors[fid]\n",
    "\n",
    "    if factor.type == 'prior_keyframe':\n",
    "        keyframe = keyframes[factor.i]\n",
    "        print('factor ', factor.id, 'type', factor.type, '\\tkf ', factor.i)\n",
    "        # do stuff with this KF\n",
    "\n",
    "    elif factor.type == 'prior_landmark':\n",
    "        landmark = landmarks[factor.i]\n",
    "        print('factor ', factor.id, 'type', factor.type, '\\tlmk', factor.i)\n",
    "        # do stuff with this lmk\n",
    "\n",
    "    elif factor.type == 'motion':\n",
    "        keyframe1 = keyframes[factor.i]\n",
    "        keyframe2 = keyframes[factor.j]\n",
    "        print('factor ', factor.id, 'type', factor.type, '\\t\\tkf1', factor.i, '\\tkf2', factor.j)\n",
    "        # do stuff with these KFs\n",
    "\n",
    "    elif factor.type == 'landmark':\n",
    "        keyframe = keyframes[factor.i]\n",
    "        landmark = landmarks[factor.j]\n",
    "        print('factor ', factor.id, 'type', factor.type, '\\tkf ', factor.i, '\\tlmk', factor.j)\n",
    "        # do stuff with these KF and lmk\n",
    "\n",
    "    else:\n",
    "        print(\"Unrecognized factor type! Must be one in \\{'prior_keyframe', 'prior_landmark', 'motion', landmark'\\}\")\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We use this mechanism to compute the total cost to minimize:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# define dummy cost functions for each type of factor -- se further down for real implementations\n",
    "def cost_constant_position (sqrt_info, keyframe1, keyframe2):\n",
    "    return 10\n",
    "def cost_landmark (measurement, sqrt_info, keyframe, landmark):\n",
    "    return 20\n",
    "def cost_keyframe_prior (measurement, sqrt_info, keyframe):\n",
    "    return 30\n",
    "def cost_landmark_prior (measurement, sqrt_info, keyframe):\n",
    "    return 40\n",
    "\n",
    "# define a method to compute the total cost\n",
    "def computeTotalCost(factors, keyframes, landmarks):  # see that we will use all Dictionaries in the SLAM system!\n",
    "\n",
    "    totalcost = 0\n",
    "\n",
    "    for fid in factors:\n",
    "        factor = factors[fid]\n",
    "        i = factor.i\n",
    "        j = factor.j\n",
    "        measurement = factor.meas\n",
    "        sqrt_info   = factor.sqrt_info\n",
    "\n",
    "        if factor.type == \"motion\":\n",
    "            totalcost += cost_constant_position (sqrt_info, keyframes[i], keyframes[j])\n",
    "\n",
    "        elif factor.type == \"landmark\":\n",
    "            totalcost += cost_landmark (measurement, sqrt_info, keyframes[i], landmarks[j])\n",
    "\n",
    "        elif factor.type == \"prior_keyframe\":\n",
    "            totalcost += cost_keyframe_prior (measurement, sqrt_info, keyframes[i])\n",
    "\n",
    "        elif factor.type == \"prior_landmark\":\n",
    "            totalcost += cost_landmark_prior (measurement, sqrt_info, landmarks[i])\n",
    "\n",
    "        else:\n",
    "            print('Error in the factor type: type not known')\n",
    "    return totalcost\n",
    "\n",
    "# do compute total cost!\n",
    "totalcost = computeTotalCost(factors, keyframes, landmarks)\n",
    "\n",
    "print('total cost = ', totalcost)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "where the cost of each type of factor is computed by a dedicated cost function (in the example above, the cost functions are defined as dummy functions returnning fixed dummy costs). \n",
    "\n",
    "We study the real implementation of these cost functions below."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Cost functions\n",
    "\n",
    "Each factor stores the necessary information for computing a cost function.\n",
    "\n",
    "A cost function compares the actual measurement against a prediction of that measurement made from the current states associated to the factor.\n",
    "\n",
    "### Cost per factor\n",
    "\n",
    "The cost function is responsible for computing the cost of a given factor. \n",
    "\n",
    "Since a factor involves a measurement and some states, we begin by computing the expected value $e$ of the measurement, given the states:\n",
    "$$\n",
    "e = h(x_1, x_2)\n",
    "$$\n",
    "where $h()$ is a particular observation model, and $x_i$ are the states involved in the factor.\n",
    "\n",
    "Then we compare our true measurement $y$ against the expected value $e$, obtaining what we call the prediction error $z$:\n",
    "$$\n",
    "z = y \\ominus e = y \\ominus h(x_1,x_2)\n",
    "$$\n",
    "\n",
    "**NOTE:** see that we compare with a generic operator $\\ominus$ and not with a simple subtraction $-$. This allows us to compare magnitudes that do not belong to vector spaces, such as orientations in 3D. Read on to know how we deal with them!\n",
    "\n",
    "This error $z$ is then weighted by the square root of the information matrix, $W$, and then taken the squared norm. This yields the cost $c$ of the factor:\n",
    "$$\n",
    "c = || W \\cdot z ||^2\n",
    "$$\n",
    "\n",
    "Since factors are all of different nature and relate different states, it is necessary to index all variables correctly in formulas.\n",
    "Overall, the cost we have computed for a given factor $k$ is the following:\n",
    "$$\n",
    "c_k = ||W_k(y_k\\ominus h_k(x_{i_k},x_{j_k}))||^2\n",
    "$$\n",
    "where:\n",
    "- $k$ is the factor index\n",
    "- $i_k$ and $j_k$ are the indices of the two states associated to this factor\n",
    "- $h_k$ is the particular observation model for this factor\n",
    "- $y_k$ and $W_k$ are the measurement and sqrt-info-matrix of this factor\n",
    "\n",
    "### Total cost\n",
    "\n",
    "The total cost for $K$ factors is the sum of the costs of all factors\n",
    "$$\n",
    "c = \\sum_{k=0}^K c_k = \\sum_{k=0}^K ||W_k(y_k \\ominus h_k(x_{i_k},x_{j_k}))||^2\n",
    "$$\n",
    "This is the cost to be minimized by the solver.\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "### Landmark observation cost function\n",
    "\n",
    "We can observe the computation of one of such costs for our SLAM. \n",
    "We study the case of a landmark observation from a keyframe. \n",
    "\n",
    "We note the following facts:\n",
    "\n",
    "1. The keyframe and landmark states are expressed in position and orientation:\n",
    "   $$\n",
    "   \\textrm{keyframe: } x_i = \\begin{bmatrix}T_i\\\\R_i\\end{bmatrix} \\\\\n",
    "   \\textrm{landmark: } x_j = \\begin{bmatrix}T_j\\\\R_j\\end{bmatrix}\n",
    "   $$\n",
    "\n",
    "   1. one of the states $x_i$ is a particular keyframe, indexed by field `i` in the factor\n",
    "\n",
    "   1. the other state $x_j$ is a landmark, indexed by field `j` in the factor\n",
    "\n",
    "1. The measurement $y$ is that obtained by the Apriltag detector, in the form of a translation vector $T_y$ and a rotation vector $w_y$, stacked in a 6-vector,\n",
    "   $$\n",
    "   y = \\begin{bmatrix}T_y \\\\ w_y\\end{bmatrix}\n",
    "   $$\n",
    "\n",
    "1. The error $z$ is computed according to a chain of operations:\n",
    "   \n",
    "   1. The expected measurement $e=h(x_i,x_j)$ is a 3D composition of keyframe and landmark states. It is therefore a 3d pose with translation $T_e$ and rotation matrix $R_e$\n",
    "      $$\n",
    "      e = \\begin{bmatrix}T_e \\\\ R_e\\end{bmatrix} = \\begin{bmatrix}R_i^\\top(T_j - T_i) \\\\ R_i^\\top R_j\\end{bmatrix}\n",
    "      $$\n",
    "      \n",
    "   1. The error is the \"difference\", expressed by $\\ominus$, between the expectation above and the measurement\n",
    "      $$\n",
    "      z = y \\ominus e\n",
    "      $$\n",
    "      This operator $\\ominus$ is implemented as follows:\n",
    "\n",
    "      1. For the translational part, the difference is a mere subtraction\n",
    "         $$\n",
    "         T_z = T_y - T_e\n",
    "         $$\n",
    "\n",
    "      1. For the rotational part, this \"difference\" is done in the SO3 manifold, with,\n",
    "         $$z_w = \\log(R_e^\\top \\cdot \\exp(w_y))$$\n",
    "      so that we have the error:\n",
    "      $$\n",
    "      z = y \\ominus e = \\begin{bmatrix} T_y - T_e \\\\ \\log(R_e^\\top \\cdot \\exp(w_y)) \\end{bmatrix}\n",
    "      $$\n",
    "\n",
    "Yep, it seems complicated. A glipmse at the code may clarify things up:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# landmark observations\n",
    "def cost_landmark(meas, sqrt_info, keyframe_i, landmark_j):\n",
    "    # predict: landmark pose wrt KF pose\n",
    "    ppred = keyframe_i.R.T @ (landmark_j.position - keyframe_i.position)\n",
    "    Rpred = keyframe_i.R.T @ landmark_j.R\n",
    "    # you can also use the method betweenPoses() to do this\n",
    "\n",
    "    # error:\n",
    "    perr = meas[0:3] - ppred          # use regular subtraction for the translation\n",
    "    Rerr = Rpred.T @ exp3(meas[3:6])  # use SO(3) tools for the orientation part\n",
    "    werr = log3(Rerr)                 # this is a 3-vector\n",
    "\n",
    "    # stack position and orientation errors into a 6-vector\n",
    "    err = casadi.vertcat(perr, werr)\n",
    " \n",
    "    # apply weight and compute squared norm\n",
    "    cost = casadi.sumsqr(sqrt_info @ err) \n",
    "    return cost\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Motion cost function: costant position model\n",
    "\n",
    "Since we have no motion sensor, we assume the motion is null at each step. This is known as a constant-position motion model. What we do is:\n",
    "\n",
    "- predict the motion: compose $x_1$ and $x_2$ so that you obtain a prediction that corresponds to the frame `keyframe2` with respect to `keyframe1`.\n",
    "\n",
    "- compare this prediction with the null motion, that is, $y = 0_{6\\times1}$\n",
    "\n",
    "Since $y=0$, we can write the function prototype for the constant-position motion model with no measurement input. To allow for real motion, we have to provide an information matrix that is small.\n",
    "\n",
    "Now, define a similar cost function for the motion, that is, between two keyframes.\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def cost_constant_position(sqrt_info, keyframe_i, keyframe_j):\n",
    "    # write your code here and change the return line below\n",
    "    return 0  "
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Prior keyframe cost function\n",
    "\n",
    "Now, write a cost function to apply a prior pose to one keyframe:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def cost_keyframe_prior(meas, sqrt_info, keyframe_i):\n",
    "    # write your code here and change the return line below\n",
    "    return 0"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Prior landmark cost function\n",
    "\n",
    "Finally, write a cost function to apply a prior pose to a landmark:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def cost_landmark_prior(meas, sqrt_info, landmark):\n",
    "    # write your code here and change the return line below\n",
    "    return 0"
   ]
  }
 ],
 "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.9.6 (default, Oct 18 2022, 12:41:40) \n[Clang 14.0.0 (clang-1400.0.29.202)]"
  },
  "orig_nbformat": 4,
  "vscode": {
   "interpreter": {
    "hash": "31f2aee4e71d21fbe5cf8b01ff0e069b9275f58929596ceb00d14d90e3e16cd6"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}