line_Procrustes.ipynb

{
  "nbformat": 4,
  "nbformat_minor": 0,
  "metadata": {
    "colab": {
      "name": "Untitled11.ipynb",
      "provenance": [],
      "authorship_tag": "ABX9TyM3+LyXC0fPPW+lxh0nGkSY",
      "include_colab_link": true
    },
    "kernelspec": {
      "name": "python3",
      "display_name": "Python 3"
    },
    "language_info": {
      "name": "python"
    }
  },
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "view-in-github",
        "colab_type": "text"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/chene/ARQOPUS/blob/main/line_Procrustes.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "sr5aKeWtO1G2"
      },
      "source": [
        "# Line-based Orthogonal Procrustes Analysis\n",
        "\n",
        "In line-based Procrustes Analysis, we are interested in register a set of points to a set of lines, where the correspondence between point-line is explicitly known. Within the context of computer-assisted interventions (CAI), we are typically interested in **scaling**, followed by **rotation** and **translation** of a set of 3D *points* so they are registered (aligned) to a set of 3D *lines*. Again, Orthogonal implies that there is no searing. We also assume there is a 1:1 correspondence between these two 3D datasets.\n",
        "\n",
        "The immediate application for line-based Procrustes Analysis in CAI are:\n",
        "\n",
        "Camera Hand-Eye Calibration:\n",
        "```\n",
        "@article{Morgan2017,\n",
        "author    = {Morgan, Isabella and Jayarathne, Uditha L. and Rankin, Adam and Peters, Terry M. and Chen, Elvis C. S.},\n",
        "doi       = {10.1007/s11548-017-1590-9},\n",
        "journal   = {International journal of computer assisted radiology and surgery},\n",
        "number    = {7},\n",
        "pages     = {1141--1149},\n",
        "pmid      = {28425030},\n",
        "title     = {Hand-eye calibration for surgical cameras: a Procrustean Perspective-n-Point solution},\n",
        "volume    = {12},\n",
        "year      = {2017}, }\n",
        "```\n",
        "\n",
        "and Ultrasound Probe Calibration:\n",
        "```\n",
        "@article{Chen2016,\n",
        "author    = {Chen, Elvis C. S. and Peters, Terry M. and Ma, Burton},\n",
        "doi       = {10.1007/s11548-016-1390-7},\n",
        "journal   = {International Journal of Computer Assisted Radiology and Surgery},\n",
        "number    = {6},\n",
        "pages     = {889--898},\n",
        "pmid      = {27038966},\n",
        "title     = {Guided ultrasound calibration: where, how, and how many calibration fiducials},\n",
        "volume    = {11},\n",
        "year      = {2016}, }\n",
        "```\n",
        "\n",
        "I first introduced the Matlab code in the following:\n",
        "```\n",
        "@inproceedings{Chen2017,\n",
        "author    = {Chen, Elvis C. S. and Peters, Terry M. and Ma, Burton},\n",
        "booktitle = {Proc. SPIE 10135, Medical Imaging 2017: Image-Guided Procedures, Robotic Interventions, and Modeling},\n",
        "doi       = {10.1117/12.2253692},\n",
        "editor    = {Webster, Robert J. and Fei, Baowei},\n",
        "pages     = {1013509},\n",
        "title     = {Which point-line registration?},\n",
        "volume    = {1013509},\n",
        "year      = {2017}, }\n",
        "```"
      ]
    },
    {
      "cell_type": "markdown",
      "source": [
        "We address the problem of line-based Procrustean Analysis using an iterative algorithm. It works in a similar manner to the popular [ICP](https://ieeexplore.ieee.org/document/121791) algorithm, where the corresponding point for **a point** along **a given line** is searched first, followed by [the point-based Procrustean Analysis](https://github.com/chene/ARQOPUS/blob/main/point_Procrustes.ipynb). \n",
        "\n",
        "The Matlab code is:\n",
        "```Matlab\n",
        "function [ R, t, A ] = p2l( X, Y, D, tol )\n",
        "%\n",
        "% INPUT:\n",
        "%        X : matrix (3xn) 3D coordinates\n",
        "%        Y : matrix (3xn) 3D coordinates, line origin\n",
        "%        D : matrix (3xn) 3D vector, line orientation\n",
        "%        tol: exit condition\n",
        "%\n",
        "% OUTPUT:\n",
        "%         solves the registration between points (X) and line (Y+tD)\n",
        "%\n",
        "%        Q : 3x3 rotation\n",
        "%        t : 3x1 translation\n",
        "%        A : 3x3 diagonal scales\n",
        "%\n",
        "% Elvis C.S. Chen\n",
        "% chene AT robarts DOT ca\n",
        "\n",
        "[m,n] = size(X); e = ones(1,n); err = +Inf;\n",
        "E_old = 1000*ones( m,n ); Q = Y; Dir = normc(D);\n",
        "while err > tol\n",
        "    [ R, t, A ] = AOPA_Major( X, Q, tol);                % pont-based solution\n",
        "    E = Q - R*A*X-t*e;                                   % correspondence\n",
        "    Q = Y + Dir .* repmat( dot(R*A*X+t*e-Y,Dir),[m,1] ); % reprojection\n",
        "    err = norm(E-E_old,'fro'); E_old = E;                % residual error\n",
        "end\n",
        "```\n",
        "where a line is represented as a line origin ($Y$, 3D points) and its unit orientation ($D$)."
      ],
      "metadata": {
        "id": "zVseiJnZgqOb"
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "Alternatively, the line parameterization $t$ can be solved independently, which provides point correspondence:\n",
        "```Matlab\n",
        "function [R, t, A] = g2Fiore(X, O, D, tol)\n",
        "%\n",
        "% INPUT:\n",
        "%        X : matrix (3xn) 3D coordinates\n",
        "%        Y : matrix (3xn) 3D coordinates, line origin\n",
        "%        D : matrix (3xn) 3D vector, line orientation\n",
        "%        tol: exit condition\n",
        "%\n",
        "% OUTPUT:\n",
        "%         solves the registration between points (X) and line (Y+tD)\n",
        "%\n",
        "%        Q : 3x3 rotation\n",
        "%        t : 3x1 translation\n",
        "%        A : 3x3 diagonal scales\n",
        "%\n",
        "% Elvis C.S. Chen\n",
        "% chene AT robarts DOT ca\n",
        "n = size(D, 2); e = ones( 1, n );\n",
        "M = [X; e]; [~,~,V] = svd(M); V2 = V(:, rank(M)+1:end);\n",
        "D = ( (D'*D) .*(V2*V2') ); b = -diag( V2*V2'*O'*D );\n",
        "v = S\\b; Z = diag(v);\n",
        "[R, t, A] = AOPA_major(X, D*Z+O, tol);\n",
        "```\n",
        "refer to my [SPIE paper](https://www.spiedigitallibrary.org/conference-proceedings-of-spie/10135/1013509/Which-point-line-registration/10.1117/12.2253692.full?SSO=1) for derivation.  For both of these solutions, a minimum of $6$ paired point-line are required. Note that when the cardinality of the dataset is large, the $g2Fiore$ algorithm may be computationally expensive and at times unstable due to the use of matrix left-division."
      ],
      "metadata": {
        "id": "tbjVzYUs6te7"
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "In my experience, the $p2l$ algorithm works very well and almost always converge to the correct answer **provided** that\n",
        "- there are **8** or more paired point-line fiducials, and\n",
        "- the line orientations are not parallel among themselves.\n",
        "\n",
        "The Python code corresponding to the Matlab code above is:"
      ],
      "metadata": {
        "id": "h5DLHn30Ay4r"
      }
    },
    {
      "cell_type": "code",
      "source": [
        "import numpy as np\n",
        "import numpy.matlib\n",
        "import math\n",
        "\n",
        "def p2l(X, Y, D, tol):\n",
        "    \"\"\"\n",
        "    Computes the Procrustean point-line registration between X and Y+nD with \n",
        "    anisotropic Scaling,\n",
        "        \n",
        "    where X is a mxn matrix, m is typically 3\n",
        "          Y is a mxn matrix denoting line origin, same dimension as X\n",
        "          D is a mxn normalized matrix denoting line direction\n",
        "          \n",
        "          R is a mxm rotation matrix,\n",
        "          A is a mxm diagonal scaling matrix, and\n",
        "          t is a mx1 translation vector\n",
        "          Q is a mxn fiducial on line that is closest to X after registration\n",
        "          fre is the fiducial localization error\n",
        "          \n",
        "    based on the Majorization Principle\n",
        "    \"\"\"\n",
        "    [m,n] = X.shape\n",
        "    err = np.Infinity\n",
        "    E_old = 1000000 * np.ones((m,n))\n",
        "    e = np.ones((1,n))\n",
        "    # intialization\n",
        "    Q = Y\n",
        "    # normalize the line orientation just in case\n",
        "    Dir = D/np.linalg.norm(D, ord=2,axis=0,keepdims=True)\n",
        "    while err > tol:\n",
        "        [R, t, A] = AOPA_Major(X, Q, tol)\n",
        "        E  = Q-np.matmul(R,np.matmul(A,X))-np.matmul(t,e)\n",
        "        # project point to line\n",
        "        Q = Y+Dir*np.matlib.repmat(np.einsum('ij,ij->j',np.matmul(R,np.matmul(A,X))+np.matmul(t,e)-Y,Dir),m,1)       \n",
        "        err = np.linalg.norm(E-E_old)\n",
        "        E_old = E\n",
        "    E = Q - np.matmul(R, np.matmul(A,X)) - np.matmul(t,e)\n",
        "    \n",
        "    # calculate fiducial registration error\n",
        "    fre = np.sum(np.linalg.norm(E,ord=2,axis=0,keepdims=True))/X.shape[1]\n",
        "    return[R,t,A,Q,fre]"
      ],
      "metadata": {
        "id": "fhIiHekBBKxa"
      },
      "execution_count": null,
      "outputs": []
    }
  ]
}