{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Ops Tutorial"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "SymForce uses [concepts](https://en.wikipedia.org/wiki/Concept_(generic_programming)) as an underlying mechanism. A concept is a specification of supported operations, including syntax and semantics, but does not require a subtype relationship. This means that a set of heterogenous types can be operated on in a homogenous way, i.e. types that are external and don't share a base class, like Python floats treated as scalars.\n",
    "\n",
    "There are three core concepts, each of which is a superset of the previous. The core routines use these ops interfaces rather than calling methods on types directly. The [ops package](../api/symforce.ops.html) docs provide much more detail and each op is tested on each type, but examples are given here.\n",
    "\n",
    "In the case of different containers, such as lists, dataclasses, or Values objects (see the [Values tutorial](../tutorials/values_tutorial.html)), we perform operations recursively over their elements."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Setup\n",
    "import symforce\n",
    "\n",
    "symforce.set_symbolic_api(\"sympy\")\n",
    "symforce.set_log_level(\"warning\")\n",
    "\n",
    "import symforce.symbolic as sf\n",
    "from symforce.notebook_util import display\n",
    "from symforce.ops import GroupOps\n",
    "from symforce.ops import LieGroupOps\n",
    "from symforce.ops import StorageOps\n",
    "from symforce.values import Values"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## StorageOps"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "[StorageOps](../api/symforce.ops.storage_ops.html): **Data type that can be serialized to and from a vector of scalar quantities.**\n",
    "\n",
    "Methods: `.storage_dim()`, `.to_storage()`, `.from_storage()`, `.symbolic()`, `.evalf()`, `.subs()`, `.simplify()`\n",
    "\n",
    "Storage operations are used extensively for marshalling and for operating on each scalar in a type."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Number of scalars used to represent a Pose3 (4 quaternion + 3 position)\n",
    "display(StorageOps.storage_dim(sf.Pose3))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Because we are using concepts, we can operate on types that aren't subtypes\n",
    "# of symforce\n",
    "display(StorageOps.storage_dim(float))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Element-wise operations on lists of objects\n",
    "display(StorageOps.storage_dim([sf.Pose3, sf.Pose3]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Element-wise operations on Values object with multiple types of elements\n",
    "values = Values(\n",
    "    pose=sf.Pose3(),\n",
    "    scalar=sf.Symbol(\"x\"),\n",
    ")\n",
    "display(StorageOps.storage_dim(values))  # 4 quaternion + 3 position + 1 scalar"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Serialize scalar\n",
    "display(StorageOps.to_storage(5))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Serialize vector/matrix\n",
    "display(StorageOps.to_storage(sf.V3(sf.Symbol(\"x\"), 5.2, sf.sqrt(5))))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Serialize geometric type and reconstruct\n",
    "T = sf.Pose3.symbolic(\"T\")\n",
    "T_serialized = StorageOps.to_storage(T)\n",
    "T_recovered = StorageOps.from_storage(sf.Pose3, T_serialized)\n",
    "display(T_serialized)\n",
    "display(T_recovered)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## GroupOps"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "[GroupOps](../api/symforce.ops.group_ops.html): **Mathematical group that implements closure, associativity, identity and invertibility.**\n",
    "\n",
    "Methods: `.identity()`, `.inverse()`, `.compose()`, `.between()`\n",
    "\n",
    "Group operations provide the core methods to compare and combine types."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Identity of a pose\n",
    "display(GroupOps.identity(sf.Pose3))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Identity of a scalar (under addition)\n",
    "display(GroupOps.identity(float))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Inverse of a vector\n",
    "display(GroupOps.inverse(sf.V3(1.2, -3, 2)).T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Compose two vectors (under addition)\n",
    "display(GroupOps.compose(sf.V2(1, 2), sf.V2(3, -5)))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Compose a rotation and its inverse to get identity\n",
    "R1 = sf.Rot3.from_angle_axis(\n",
    "    angle=sf.Symbol(\"theta1\"),\n",
    "    axis=sf.V3(0, 0, 1),\n",
    ")\n",
    "display(StorageOps.simplify(GroupOps.compose(R1, R1.inverse()).simplify()))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Relative rotation using `.between()`\n",
    "R2 = sf.Rot3.from_angle_axis(\n",
    "    angle=sf.Symbol(\"theta2\"),\n",
    "    axis=sf.V3(0, 0, 1),\n",
    ")\n",
    "R_delta = GroupOps.between(R1, R2)\n",
    "display(R2)\n",
    "display(StorageOps.simplify(GroupOps.compose(R1, R_delta)))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## LieGroupOps"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "[LieGroupOps](../api/symforce.ops.lie_group_ops.html): **Group that is also a differentiable manifold, such that calculus applies.**\n",
    "\n",
    "Methods: `.tangent_dim()`, `.from_tangent()`, `to_tangent()`, `.retract()`, `.local_coordinates()`, `.storage_D_tangent()`\n",
    "\n",
    "Lie group operations provide the core methods for nonlinear optimization.  \n",
    "Familiarity is not expected for all users, but learning is encouraged!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Underlying dimension of a 3D rotation's tangent space\n",
    "display(LieGroupOps.tangent_dim(sf.Rot3))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Exponential map (tangent space vector wrt identity element) for a 2D rotation\n",
    "angle = sf.Symbol(\"theta\")\n",
    "rot2 = LieGroupOps.from_tangent(sf.Rot2, [angle])\n",
    "display(rot2.to_rotation_matrix())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Logarithmic map (tangent space wrt identity element -> element) of the rotation\n",
    "display(LieGroupOps.to_tangent(rot2))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Exponential map of a vector type is a no-op\n",
    "display(LieGroupOps.from_tangent(sf.V5(), [1, 2, 3, 4, 5]).T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Retract perturbs the given element in the tangent space and returns the\n",
    "# updated element\n",
    "rot2_perturbed = LieGroupOps.retract(rot2, [sf.Symbol(\"delta\")])\n",
    "display(rot2_perturbed.to_rotation_matrix())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Local coordinates compute the tangent space perturbation between one element\n",
    "# and another\n",
    "display(StorageOps.simplify(LieGroupOps.local_coordinates(rot2, rot2_perturbed)))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# storage_D_tangent computes the jacobian of the storage space of an object with\n",
    "# respect to the tangent space around the element.\n",
    "\n",
    "# A 2D rotation is represented by a complex number, so storage_D_tangent\n",
    "# represents how that complex number will change given an infinitesimal\n",
    "# perturbation in the tangent space\n",
    "display(LieGroupOps.storage_D_tangent(rot2))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Using symbolic geometric types and concepts is already very powerful for development and analysis of robotics, but operating on symbolic objects at runtime is much too slow for most applications. However, symbolic expressions can be beautifully set to fast runtime code."
   ]
  }
 ],
 "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.8.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}