{ "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "# Determining rigid body transformation using the SVD algorithm\n", "\n", "> Marcos Duarte \n", "> Laboratory of Biomechanics and Motor Control ([http://demotu.org/](http://demotu.org/)) \n", "> Federal University of ABC, Brazil" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Ideally, three non-collinear markers placed on a moving rigid body is everything we need to describe its movement (translation and rotation) in relation to a fixed coordinate system. However, in practical situations of human motion analysis, markers are placed on the soft tissue of a deformable body and this generates artifacts caused by muscle contraction, skin deformation, marker wobbling, etc. In this situation, the use of only three markers can produce unreliable results. It has been shown that four or more markers on the segment followed by a mathematical procedure to calculate the 'best' rigid-body transformation taking into account all these markers produces more robust results (Söderkvist & Wedin 1993; Challis 1995; Cappozzo et al. 1997). \n", "\n", "One mathematical procedure to calculate the transformation with three or more marker positions envolves the use of the [singular value decomposition](http://en.wikipedia.org/wiki/Singular_value_decomposition) (SVD) algorithm from linear algebra. The SVD algorithm decomposes a matrix $\\mathbf{M}$ (which represents a general transformation between two coordinate systems) into three simple transformations: a rotation $\\mathbf{V^T}$, a scaling factor $\\mathbf{S}$ along the rotated axes and a second rotation $\\mathbf{U}$: \n", "\n", "$$ \\mathbf{M}= \\mathbf{U\\;S\\;V^T}$$\n", "\n", "And the rotation matrix is given by:\n", "\n", "$$ \\mathbf{R}= \\mathbf{U\\:V^T}$$\n", "\n", "The matrices $\\mathbf{U}$ and $\\mathbf{V}$ are both orthonormal (det = $\\pm$1).\n", "\n", "For example, if we have registered the position of four markers placed on a moving segment in 100 different instants and the position of these same markers during, what is known in Biomechanics, a static calibration trial, we would use the SVD algorithm to calculate the 100 rotation matrices (between the static trials and the 100 instants) in order to find the Cardan angles for each instant. \n", "\n", "The function `svdt.py` (its code is shown at the end of this text) determines the rotation matrix ($R$) and the translation vector ($L$) for a rigid body after the following transformation: $B = R*A + L + err$. Where $A$ and $B$ represent the rigid body in different instants and err is an aleatory noise. $A$ and $B$ are matrices with the marker coordinates at different instants (at least three non-collinear markers are necessary to determine the 3D transformation). \n", "The matrix $A$ can be thought to represent a local coordinate system (but $A$ it's not a basis) and matrix $B$ the global coordinate system. The operation $P_g = R*P_l + L$ calculates the coordinates of the point $P_l$ (expressed in the local coordinate system) in the global coordinate system ($P_g$).\n", " \n", "Let's test the `svdt` function:" ] }, { "cell_type": "code", "execution_count": 2, "metadata": { "ExecuteTime": { "end_time": "2018-02-25T21:00:58.942260Z", "start_time": "2018-02-25T21:00:58.860203Z" } }, "outputs": [], "source": [ "# Import the necessary libraries\n", "import numpy as np\n", "import sys\n", "sys.path.insert(1, r'./../functions')" ] }, { "cell_type": "code", "execution_count": 3, "metadata": { "ExecuteTime": { "end_time": "2018-02-25T21:01:04.859690Z", "start_time": "2018-02-25T21:01:04.653910Z" } }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Rotation matrix:\n", " [[ 0. -1. 0.]\n", " [ 1. 0. 0.]\n", " [ 0. 0. 1.]]\n", "Translation vector:\n", " [0. 0. 0.]\n", "RMSE:\n", " 0.0\n", "Rotation matrix:\n", " [[ 0. -1. 0.]\n", " [ 1. 0. 0.]\n", " [ 0. 0. 1.]]\n", "Translation vector:\n", " [0. 0. 0.]\n", "RMSE:\n", " 0.0\n" ] } ], "source": [ "from svdt import svdt\n", "\n", "# markers in different columns (default):\n", "A = np.array([0,0,0, 1,0,0, 0,1,0, 1,1,0]) # four markers\n", "B = np.array([0,0,0, 0,1,0, -1,0,0, -1,1,0]) # four markers\n", "\n", "R, L, RMSE = svdt(A, B)\n", "\n", "print('Rotation matrix:\\n', np.around(R, 4))\n", "print('Translation vector:\\n', np.around(L, 4))\n", "print('RMSE:\\n', np.around(RMSE, 4))\n", "\n", "# markers in different rows:\n", "A = np.array([[0,0,0], [1,0,0], [ 0,1,0], [ 1,1,0]]) # four markers\n", "B = np.array([[0,0,0], [0,1,0], [-1,0,0], [-1,1,0]]) # four markers\n", "\n", "R, L, RMSE = svdt(A, B, order='row')\n", "\n", "print('Rotation matrix:\\n', np.around(R, 4))\n", "print('Translation vector:\\n', np.around(L, 4))\n", "print('RMSE:\\n', np.around(RMSE, 4))" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "For the matrix of a pure rotation around the z axis, the element in the first row and second column is $-sin\\gamma$, which means the rotation was $90^o$, as expected.\n", "\n", "A typical use of the `svdt` function is to calculate the transformation between $A$ and $B$ ($B = R*A + L$), where $A$ is the matrix with the markers data in one instant (the calibration or static trial) and $B$ is the matrix with the markers data of more than one instant (the dynamic trial). \n", "Input $A$ as a 1D array `[x1, y1, z1, ..., xn, yn, zn]` where `n` is the number of markers and $B$ a 2D array with the different instants as rows (like in $A$). \n", "The output $R$ has the shape `(3, 3, tn)`, where `tn` is the number of instants, $L$ the shape `(tn, 3)`, and $RMSE$ the shape `(tn)`. If `tn` is equal to one, the outputs have the same shape as in `svdt` (the last dimension of the outputs above is dropped). \n", "\n", "Let's show this case:" ] }, { "cell_type": "code", "execution_count": 4, "metadata": { "ExecuteTime": { "end_time": "2018-02-25T21:01:52.583228Z", "start_time": "2018-02-25T21:01:52.572738Z" } }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Rotation matrix:\n", " [[[-0. -1. -0.]\n", " [ 1. -0. -0.]\n", " [-0. -0. 1.]]\n", "\n", " [[-0. -1. -0.]\n", " [ 1. -0. -0.]\n", " [-0. -0. 1.]]]\n", "Translation vector:\n", " [[0. 0. 0.]\n", " [0. 0. 0.]]\n", "RMSE:\n", " [0. 0.]\n" ] } ], "source": [ "A = np.array([1,0,0, 0,1,0, 0,0,1])\n", "B = np.array([0,1,0, -1,0,0, 0,0,1])\n", "B = np.vstack((B, B)) # simulate two instants (two rows)\n", "\n", "R, L, RMSE = svdt(A, B)\n", "\n", "print('Rotation matrix:\\n', np.around(R, 4))\n", "print('Translation vector:\\n', np.around(L, 4))\n", "print('RMSE:\\n', np.around(RMSE, 4))" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## References\n", "\n", "- Cappozzo A, Cappello A, Della Croce U, Pensalfini F (1997) [Surface-marker cluster design criteria for 3-D bone movement reconstruction](http://www.ncbi.nlm.nih.gov/pubmed/9401217). IEEE Trans Biomed Eng., 44:1165-1174.\n", "- Challis JH (1995). [A procedure for determining rigid body transformation parameters](http://www.ncbi.nlm.nih.gov/pubmed/7601872). Journal of Biomechanics, 28, 733-737. \n", "- Söderkvist I, Wedin PA (1993) [Determining the movements of the skeleton using well-configured markers](http://www.ncbi.nlm.nih.gov/pubmed/8308052). Journal of Biomechanics, 26, 1473-1477." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Function `svdt.py`" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "ExecuteTime": { "end_time": "2018-02-26T00:27:42.371884Z", "start_time": "2018-02-26T00:27:42.368881Z" } }, "outputs": [], "source": [ "# %load ./../functions/svdt.py\n", "#!/usr/bin/env python\n", "\n", "\"\"\"Calculates the transformation between two coordinate systems using SVD.\"\"\"\n", "\n", "__author__ = \"Marcos Duarte, https://github.com/demotu/BMC\"\n", "__version__ = \"1.0.1\"\n", "__license__ = \"MIT\"\n", "\n", "import numpy as np\n", "\n", "\n", "def svdt(A, B, order='col'):\n", " \"\"\"Calculates the transformation between two coordinate systems using SVD.\n", "\n", " This function determines the rotation matrix (R) and the translation vector\n", " (L) for a rigid body after the following transformation [1]_, [2]_:\n", " B = R*A + L + err.\n", " Where A and B represents the rigid body in different instants and err is an\n", " aleatory noise (which should be zero for a perfect rigid body). A and B are\n", " matrices with the marker coordinates at different instants (at least three\n", " non-collinear markers are necessary to determine the 3D transformation).\n", "\n", " The matrix A can be thought to represent a local coordinate system (but A\n", " it's not a basis) and matrix B the global coordinate system. The operation\n", " Pg = R*Pl + L calculates the coordinates of the point Pl (expressed in the\n", " local coordinate system) in the global coordinate system (Pg).\n", "\n", " A typical use of the svdt function is to calculate the transformation\n", " between A and B (B = R*A + L), where A is the matrix with the markers data\n", " in one instant (the calibration or static trial) and B is the matrix with\n", " the markers data for one or more instants (the dynamic trial).\n", "\n", " If the parameter order='row', the A and B parameters should have the shape\n", " (n, 3), i.e., n rows and 3 columns, where n is the number of markers.\n", " If order='col', A can be a 1D array with the shape (n*3, like\n", " [x1, y1, z1, ..., xn, yn, zn] and B a 1D array with the same structure of A\n", " or a 2D array with the shape (ni, n*3) where ni is the number of instants.\n", " The output R has the shape (ni, 3, 3), L has the shape (ni, 3), and RMSE\n", " has the shape (ni,). If ni is equal to one, the outputs will have the\n", " singleton dimension dropped.\n", "\n", " Part of this code is based on the programs written by Alberto Leardini,\n", " Christoph Reinschmidt, and Ton van den Bogert.\n", "\n", " Parameters\n", " ----------\n", " A : Numpy array\n", " Coordinates [x,y,z] of at least three markers with two possible shapes:\n", " order='row': 2D array (n, 3), where n is the number of markers.\n", " order='col': 1D array (3*nmarkers,) like [x1, y1, z1, ..., xn, yn, zn].\n", "\n", " B : 2D Numpy array\n", " Coordinates [x,y,z] of at least three markers with two possible shapes:\n", " order='row': 2D array (n, 3), where n is the number of markers.\n", " order='col': 2D array (ni, n*3), where ni is the number of instants.\n", " If ni=1, B is a 1D array like A.\n", "\n", " order : string\n", " 'col': specifies that A and B are column oriented (default).\n", " 'row': specifies that A and B are row oriented.\n", "\n", " Returns\n", " -------\n", " R : Numpy array\n", " Rotation matrix between A and B with two possible shapes:\n", " order='row': (3, 3).\n", " order='col': (ni, 3, 3), where ni is the number of instants.\n", " If ni=1, R will have the singleton dimension dropped.\n", "\n", " L : Numpy array\n", " Translation vector between A and B with two possible shapes:\n", " order='row': (3,) if order = 'row'.\n", " order='col': (ni, 3), where ni is the number of instants.\n", " If ni=1, L will have the singleton dimension dropped.\n", "\n", " RMSE : array\n", " Root-mean-squared error for the rigid body model: B = R*A + L + err\n", " with two possible shapes:\n", " order='row': (1,).\n", " order='col': (ni,), where ni is the number of instants.\n", "\n", " See Also\n", " --------\n", " numpy.linalg.svd\n", "\n", " Notes\n", " -----\n", " The singular value decomposition (SVD) algorithm decomposes a matrix M\n", " (which represents a general transformation between two coordinate systems)\n", " into three simple transformations [3]_: a rotation Vt, a scaling factor S\n", " along the rotated axes and a second rotation U: M = U*S*Vt.\n", " The rotation matrix is given by: R = U*Vt.\n", "\n", " References\n", " ----------\n", " .. [1] Soderkvist, Kedin (1993) Journal of Biomechanics, 26, 1473-1477.\n", " .. [2] http://www.kwon3d.com/theory/jkinem/rotmat.html.\n", " .. [3] http://en.wikipedia.org/wiki/Singular_value_decomposition.\n", "\n", "\n", " Examples\n", " --------\n", " >>> import numpy as np\n", " >>> from svdt import svdt\n", " >>> A = np.array([0,0,0, 1,0,0, 0,1,0, 1,1,0]) # four markers\n", " >>> B = np.array([0,0,0, 0,1,0, -1,0,0, -1,1,0]) # four markers\n", " >>> R, L, RMSE = svdt(A, B)\n", " >>> B = np.vstack((B, B)) # simulate two instants (two rows)\n", " >>> R, L, RMSE = svdt(A, B)\n", " >>> A = np.array([[0,0,0], [1,0,0], [ 0,1,0], [ 1,1,0]]) # four markers\n", " >>> B = np.array([[0,0,0], [0,1,0], [-1,0,0], [-1,1,0]]) # four markers\n", " >>> R, L, RMSE = svdt(A, B, order='row')\n", " \"\"\"\n", "\n", " A, B = np.asarray(A), np.asarray(B)\n", " if order == 'row' or B.ndim == 1:\n", " if B.ndim == 1:\n", " A = A.reshape(int(A.size/3), 3)\n", " B = B.reshape(int(B.size/3), 3)\n", " R, L, RMSE = svd(A, B)\n", " else:\n", " A = A.reshape(int(A.size/3), 3)\n", " ni = B.shape[0]\n", " R = np.empty((ni, 3, 3))\n", " L = np.empty((ni, 3))\n", " RMSE = np.empty(ni)\n", " for i in range(ni):\n", " R[i, :, :], L[i, :], RMSE[i] = svd(A, B[i, :].reshape(A.shape))\n", "\n", " return R, L, RMSE\n", "\n", "\n", "def svd(A, B):\n", " \"\"\"Calculates the transformation between two coordinate systems using SVD.\n", "\n", " See the help of the svdt function.\n", "\n", " Parameters\n", " ----------\n", " A : 2D Numpy array (n, 3), where n is the number of markers.\n", " Coordinates [x,y,z] of at least three markers\n", " B : 2D Numpy array (n, 3), where n is the number of markers.\n", " Coordinates [x,y,z] of at least three markers\n", "\n", " Returns\n", " -------\n", " R : 2D Numpy array (3, 3)\n", " Rotation matrix between A and B\n", " L : 1D Numpy array (3,)\n", " Translation vector between A and B\n", " RMSE : float\n", " Root-mean-squared error for the rigid body model: B = R*A + L + err.\n", "\n", " See Also\n", " --------\n", " numpy.linalg.svd\n", " \"\"\"\n", "\n", " Am = np.mean(A, axis=0) # centroid of m1\n", " Bm = np.mean(B, axis=0) # centroid of m2\n", " M = np.dot((B - Bm).T, (A - Am)) # considering only rotation\n", " # singular value decomposition\n", " U, S, Vt = np.linalg.svd(M)\n", " # rotation matrix\n", " R = np.dot(U, np.dot(np.diag([1, 1, np.linalg.det(np.dot(U, Vt))]), Vt))\n", " # translation vector\n", " L = B.mean(0) - np.dot(R, A.mean(0))\n", " # RMSE\n", " err = 0\n", " for i in range(A.shape[0]):\n", " Bp = np.dot(R, A[i, :]) + L\n", " err += np.sum((Bp - B[i, :])**2)\n", " RMSE = np.sqrt(err/A.shape[0]/3)\n", "\n", " return R, L, RMSE\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [] } ], "metadata": { "hide_input": false, "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.7.2" }, "varInspector": { "cols": { "lenName": 16, "lenType": 16, "lenVar": 40 }, "kernels_config": { "python": { "delete_cmd_postfix": "", "delete_cmd_prefix": "del ", "library": "var_list.py", "varRefreshCmd": "print(var_dic_list())" }, "r": { "delete_cmd_postfix": ") ", "delete_cmd_prefix": "rm(", "library": "var_list.r", "varRefreshCmd": "cat(var_dic_list()) " } }, "types_to_exclude": [ "module", "function", "builtin_function_or_method", "instance", "_Feature" ], "window_display": false } }, "nbformat": 4, "nbformat_minor": 1 }