├── SO3_transformations.pdf ├── .gitignore ├── README.md └── so3_log_map_analysis.ipynb /SO3_transformations.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nurlanov-zh/so3_log_map/HEAD/SO3_transformations.pdf -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 95 | __pypackages__/ 96 | 97 | # Celery stuff 98 | celerybeat-schedule 99 | celerybeat.pid 100 | 101 | # SageMath parsed files 102 | *.sage.py 103 | 104 | # Environments 105 | .env 106 | .venv 107 | env/ 108 | venv/ 109 | ENV/ 110 | env.bak/ 111 | venv.bak/ 112 | 113 | # Spyder project settings 114 | .spyderproject 115 | .spyproject 116 | 117 | # Rope project settings 118 | .ropeproject 119 | 120 | # mkdocs documentation 121 | /site 122 | 123 | # mypy 124 | .mypy_cache/ 125 | .dmypy.json 126 | dmypy.json 127 | 128 | # Pyre type checker 129 | .pyre/ 130 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SO3 Lie group transformations and Jacobian of log map 2 | The analysis and implementation of `SO3` `log` map, and its Jacobian (in a numerical sense). The special focus on edge cases, i.e. angles close to $0$ and $\pi$. 3 | 4 | For details see the pdf report: [Report](SO3_transformations.pdf). 5 | 6 | Find corresponding [Sophus](https://github.com/strasdat/Sophus) PRs [here](https://github.com/strasdat/Sophus/pull/269) and [here](https://github.com/strasdat/Sophus/pull/281) 7 | 8 | ## Contribution 9 | * Compared 3 different formulas of log map: `log_baseline`, `log_pi`, `log_quaternion` = `log_sophus` (see jupyter notebook for details). 10 | * Tested the above formulas of logarithmic mapping. 11 | * Derived Jacobians of log map, i.e. `dlog(R)/dR` or `dlog(q)/dq`, in a numerical sense for each of the implementations metioned above. 12 | * Tested Jacobians against numerical differentiation (with and without SO3 projection). 13 | * Compared Jacobians of different implementations of log map (they turned out to be different). 14 | * Checked the correctess of the Jacobians using chain-rule and a well-known formula of the inverse of right Jacobian, i.e. `J_r^{-1} = dlog(R+x)/dx at x=0`. 15 | * Demonstrated the use case of the derived Jacobians in a Taylor approximation of log map. 16 | * Demonstrated the use case of the derived Jacobians in a pose-graph optimization using ceres solver. 17 | * Compared derived analytical Jacobians against automatically differentiated ones from jax's dual variables. 18 | 19 | ## Conclusions 20 | ### Logarithmic mappings: 21 | * log map in baseline implementation `log_baseline` failed for the rotation matrices corresponding to rotations by angles close to \pi (1e-8 neighborhood -> fails, 1e-7 neighborhood -> 1e-1 max abs difference, 1e-6 -> 1e-4, 1e-5 -> 1e-7) 22 | * log map implementation adjusted for angle \pi `log_pi` gives correct results with precision up to 1e-8 max absolute difference. At angle exactly equal to \pi the log map is defined up to an overall sign. 23 | * log map implementation using intermediate quaternion representation (matrix -> quaternion -> angle/axis) `log_quaternion` gives correct results everywhere. 24 | * Sophus implementation of log map `log_sophus` gives correct results too. Internally it uses the "matrix to quaternion" conversion from Eigen library. 25 | 26 | ### Jacobians comparison 27 | 1. Intermediate angles: 28 | 1. All of the Jacobians give different results. 29 | 2. For all methods analytical Jacobian corresponds to numerical raw (except `Analytical Pi` $\neq$ `Numerical Pi raw`). 30 | 3. `Numerical projected (norm quaternion)` $\approx$ `Analytical quaternion`, and the results are the same for all mappings (except null components of mapping around pi). 31 | 4. `Numerical projected (SVD)` are the same for all mappings (except null components of mapping around pi). 32 | 33 | 2. Small angles (Both `Quaternion` and `Baseline` methods are the best): 34 | 1. `Pi` analytical, numerical diverge starting from angles > 1e-4. (Before 1e-4 we make case differentiation). 35 | 2. `Quaternion` and `Baseline` methods (both analytical and numerical) do not diverge, and are changing continuously with increasing angle. 36 | 3. `Numerical projected (norm quaternion)` prevents `Around Pi` method from diverging. 37 | 4. `Numerical projected (SVD)` does not always prevent `Around Pi` method from diverging. 38 | 39 | 3. Angles close to $\pi$ (`Quaternion` method is the best): 40 | 1. `Baseline` analytical, numerical (raw, projected) - diverge. 41 | 2. `Pi` analytical - do not diverge. But it nullifies the components corresponding to n(i) = 0. 42 | 3. `Pi` numerical raw - diverges. 43 | 4. `Quaternion` analytical - does not diverge, and continuously changes with decreasing angle. 44 | 5. `Quaternion` numerical - diverges at close (~discretization step) proximity of $\pi$ , when the angle crosses $\pi$. 45 | 6. All `numerical projected` help to avoid divergence. But they also diverge at close (~discretization step) proximity of $\pi$ , when the angle crosses $\pi$. 46 | 47 | ### Correctness of Jacobians using chain rule 48 | 1. `Dx_log_x_quaternion` gives correct results everywhere (with high precision, < 1e-9 max abs diff). 49 | 2. `Dx_log_x_pi` gives inaccurate results for angles close to 0. Also it nullifies rows corresponding to exact zero components of vector $n(i)=0$. 50 | 3. `Dx_log_x_baseline` diverges at angle $\pi$, but also gives inaccurate results close to $\pi$ (already in 1e-2 proximity the inaccuracy is up to 1e-6) 51 | 4. Interestingly, the Jacobians of $\log$ map derived from different methods give different results. However, after matrix multiplication with jacobian of boxplus at zero, the results become similar or the same. 52 | 4. Numerical solutions suffer from discretization-dependent inaccuracies (up 1e-5). They also tend to diverge at angles close to $\pi$. 53 | 5. Projections reduce accuracy, but sometimes help to cope with divergence. 54 | 55 | ### Use case: Taylor approximation 56 | 1. The approximation up to precision $||x||^2$ (||x|| < 1e-1) works for all vectors, except the case when $\log(R \boxplus x)$ changes the sign of $\log(R)$ 57 | 58 | ### Use case: ceres approximation 59 | 1. Checked the correctness of derived analytical Jacobians comparing them against autodifferentiated Jacobians of corresponding extended log maps. 60 | 2. Showed the issue with the baseline implementation of log map when optimizing pose-graph with large initial residuals (with angles close to \pi). 61 | 62 | ## Requirements 63 | Libraries used: 64 | - numpy 65 | - Sophus (with python bindings) 66 | - PyCeres 67 | - jax 68 | 69 | ## References: 70 | 71 | * Lie theory, differential calculus on manifolds: 72 | 73 | [[1] A micro Lie theory for state estimation in robotics. Joan Sola, Jeremie Deray, Dinesh Atchuthan, 2020](https://arxiv.org/pdf/1812.01537.pdf) 74 | 75 | [[2] A Primer on the Differential Calculus of 3D Orientations. Michael Bloesch et al., 2016](https://arxiv.org/pdf/1606.05285.pdf) 76 | 77 | [[3] A compact formula for the derivative of a 3-D rotation in exponential coordinates. Guillermo Gallego, Anthony Yezzi, 2014](https://arxiv.org/pdf/1312.0788.pdf) 78 | -------------------------------------------------------------------------------- /so3_log_map_analysis.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Analyse edge cases of $SO(3)$ $\\log$ map, derive Jacobians" 8 | ] 9 | }, 10 | { 11 | "cell_type": "code", 12 | "execution_count": null, 13 | "metadata": {}, 14 | "outputs": [], 15 | "source": [ 16 | "import sophus as sp\n", 17 | "import numpy as np\n", 18 | "import sys" 19 | ] 20 | }, 21 | { 22 | "cell_type": "markdown", 23 | "metadata": {}, 24 | "source": [ 25 | "## 1. Log maps" 26 | ] 27 | }, 28 | { 29 | "cell_type": "markdown", 30 | "metadata": {}, 31 | "source": [ 32 | "$$ \\log(R)^{\\vee} = \\vec{v}$$ \n", 33 | "$R \\in SO(3)$ - rotation matrix, $\\vec{v} \\in \\mathbb{R}^3$ - angle/axis representation of rotation: $\\vec{v} = \\theta \\vec{n}$, $\\theta \\in [0, \\pi]$ - angle of rotation, $\\vec{n}: ||{\\vec{n}}||_2 = 1$ - unit vector corresponding to axis of rotation." 34 | ] 35 | }, 36 | { 37 | "cell_type": "code", 38 | "execution_count": null, 39 | "metadata": {}, 40 | "outputs": [], 41 | "source": [ 42 | "class SO3():\n", 43 | " @staticmethod\n", 44 | " def log_pi(R):\n", 45 | " trR = R[0, 0] + R[1, 1] + R[2, 2]\n", 46 | " cos_theta = max(min(0.5 * (trR - 1), 1), -1)\n", 47 | " sin_theta = 0.5 * np.sqrt(max(0, (3 - trR) * (1 + trR)))\n", 48 | " theta = np.arctan2(sin_theta, cos_theta)\n", 49 | " R_minus_R_T_vee = np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]])\n", 50 | " \n", 51 | " if abs(3 - trR) < 1e-8:\n", 52 | " # return log map at Theta = 0\n", 53 | " c = 0.5 * (1 + theta*theta / 6 + 7 / 360 * (theta**4))\n", 54 | " return c * R_minus_R_T_vee\n", 55 | "\n", 56 | " S = R + R.transpose() + (1 - trR) * np.eye(3)\n", 57 | " rest_tr = 3 - trR\n", 58 | " n = np.ones(3)\n", 59 | " # Fix modules of n_i\n", 60 | " for i in range(3):\n", 61 | " n[i] = np.sqrt(max(0, S[i, i] / rest_tr))\n", 62 | " max_i = np.argmax(n)\n", 63 | " \n", 64 | " # Fix signs according to the sign of max element\n", 65 | " for i in range(3):\n", 66 | " if i != max_i:\n", 67 | " n[i] *= np.sign(S[max_i, i])\n", 68 | "\n", 69 | " # Fix an overall sign\n", 70 | " if any(np.sign(n) * np.sign(R_minus_R_T_vee) < 0):\n", 71 | " n = -n\n", 72 | " return theta * n\n", 73 | " \n", 74 | " @staticmethod\n", 75 | " def log_baseline(R): \n", 76 | " trR = R[0, 0] + R[1, 1] + R[2, 2]\n", 77 | " cos_theta = max(min(0.5 * (trR - 1), 1), -1)\n", 78 | " sin_theta = 0.5 * np.sqrt(max(0, (3 - trR) * (1 + trR)))\n", 79 | " theta = np.arctan2(sin_theta, cos_theta)\n", 80 | " R_minus_R_T_vee = np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]])\n", 81 | "\n", 82 | " if abs(3 - trR) < 1e-8:\n", 83 | " # return log map at Theta = 0\n", 84 | " c = 0.5 * (1 + theta*theta / 6 + 7 / 360 * (theta**4))\n", 85 | " return c * R_minus_R_T_vee\n", 86 | "\n", 87 | " # it diverges around theta=pi\n", 88 | " v = theta / (2 * sin_theta) * R_minus_R_T_vee\n", 89 | " return v\n", 90 | " \n", 91 | " @staticmethod\n", 92 | " def matrix2quaternion(R): \n", 93 | " # quaternion [q_0, q_{1:3}] = [c, vec]\n", 94 | " q = np.zeros(4)\n", 95 | " t = R[0, 0] + R[1, 1] + R[2, 2]\n", 96 | " if t > 0:\n", 97 | " # case 1\n", 98 | " t = np.sqrt(1 + t)\n", 99 | " q[0] = 0.5 * t\n", 100 | " t = 0.5 / t\n", 101 | " q[1] = (R[2, 1] - R[1, 2]) * t\n", 102 | " q[2] = (R[0, 2] - R[2, 0]) * t\n", 103 | " q[3] = (R[1, 0] - R[0, 1]) * t\n", 104 | " else:\n", 105 | " i = 0\n", 106 | " if R[1, 1] > R[0, 0]:\n", 107 | " i = 1\n", 108 | " if R[2, 2] > R[i, i]:\n", 109 | " i = 2\n", 110 | " j = (i + 1) % 3\n", 111 | " k = (j + 1) % 3\n", 112 | " t = np.sqrt(R[i, i] - R[j, j] - R[k, k] + 1)\n", 113 | " q[1+i] = 0.5 * t\n", 114 | " t = 0.5 / t\n", 115 | " q[0] = (R[k, j] - R[j, k]) * t\n", 116 | " q[1+j] = (R[j, i] + R[i, j]) * t\n", 117 | " q[1+k] = (R[k, i] + R[i, k]) * t\n", 118 | " return q\n", 119 | " \n", 120 | " @staticmethod\n", 121 | " def quaternion2vec(q):\n", 122 | " squared_n = q[1]*q[1] + q[2]*q[2] + q[3]*q[3]\n", 123 | " w = q[0]\n", 124 | " theta = 0\n", 125 | " if squared_n < np.finfo(float).eps * np.finfo(float).eps:\n", 126 | " squared_w = w*w\n", 127 | " two_atan_nbyw_by_n = 2 / w - 2.0 / 3 * squared_n / (w * squared_w) \n", 128 | " theta = 2 * squared_n / w\n", 129 | " else:\n", 130 | " n = np.sqrt(squared_n)\n", 131 | " if (abs(w) < np.finfo(float).eps):\n", 132 | " if w >= 0:\n", 133 | " two_atan_nbyw_by_n = np.pi / n\n", 134 | " else:\n", 135 | " two_atan_nbyw_by_n = -np.pi / n\n", 136 | " else:\n", 137 | " two_atan_nbyw_by_n = 2 * np.arctan(n / w) / n;\n", 138 | " theta = two_atan_nbyw_by_n * n\n", 139 | " \n", 140 | " tangent = two_atan_nbyw_by_n * q[1:]\n", 141 | " return tangent\n", 142 | " \n", 143 | " @staticmethod\n", 144 | " def log_quaternion(R): \n", 145 | " q = SO3.matrix2quaternion(R)\n", 146 | " v = SO3.quaternion2vec(q)\n", 147 | " return v\n", 148 | " \n", 149 | " @staticmethod\n", 150 | " def log_sophus(R):\n", 151 | " return sp.SO3(R).log()\n", 152 | " \n", 153 | " @staticmethod\n", 154 | " def exp(v):\n", 155 | " norm_sq = sum(v**2)\n", 156 | " v_hat = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])\n", 157 | " v_hat_sq = v_hat @ v_hat\n", 158 | " if norm_sq < np.finfo(float).eps * np.finfo(float).eps:\n", 159 | " return np.eye(3) + v_hat + v_hat_sq * 0.5\n", 160 | " else:\n", 161 | " norm = np.sqrt(norm_sq)\n", 162 | " return np.eye(3) + np.sin(norm) / norm * v_hat + (1 - np.cos(norm)) / norm_sq * v_hat_sq" 163 | ] 164 | }, 165 | { 166 | "cell_type": "markdown", 167 | "metadata": {}, 168 | "source": [ 169 | "## Test log maps" 170 | ] 171 | }, 172 | { 173 | "cell_type": "code", 174 | "execution_count": null, 175 | "metadata": {}, 176 | "outputs": [], 177 | "source": [ 178 | "np.random.seed(42)\n", 179 | "\n", 180 | "\n", 181 | "class TestSO3LogMap():\n", 182 | " def __init__(self):\n", 183 | " self.v_own = np.array([[0, -np.pi, 0], [0, np.pi - 1e-8, 0], [0, 0, -np.pi + 1e-7],\n", 184 | " [np.pi - 1e-6, 0, 0], [-np.pi + 1e-5, 0, 0],\n", 185 | " [-np.pi/2, 0, 0],\n", 186 | " [0, 0, 0], [0, 0, -1e-8], [1e-5, 0, 0], [1e-4, -1e-4, 0], \n", 187 | " [-1, -1, -1]])\n", 188 | " self.v_generated = self.generate(10000)\n", 189 | " self.v_set = np.vstack((self.v_own, self.v_generated))\n", 190 | " \n", 191 | " def test_log_map(self, log_map):\n", 192 | " correct, total = 0, 0\n", 193 | " for i, v in enumerate(self.v_set):\n", 194 | " sp_obj = sp.SO3.exp(v)\n", 195 | " R = sp_obj.matrix()\n", 196 | " pred = log_map(R)\n", 197 | " if abs(np.linalg.norm(v) - np.pi) < 1e-8:\n", 198 | " if np.any(np.sign(v) * np.sign(pred) < 0):\n", 199 | " pred = -pred\n", 200 | " try:\n", 201 | " np.testing.assert_array_almost_equal(pred, v, 8)\n", 202 | " except AssertionError as aerr:\n", 203 | " print(\"Assertion Error in function: \", log_map.__name__, aerr)\n", 204 | " print(\"Test \", i+1, \", Angle: \", np.linalg.norm(v), \"\\n\")\n", 205 | " total += 1\n", 206 | " else:\n", 207 | " correct += 1\n", 208 | " total += 1\n", 209 | " return correct, total\n", 210 | " \n", 211 | " def run(self):\n", 212 | " correct_log_baseline, total_log_baseline = self.test_log_map(SO3.log_baseline)\n", 213 | " correct_log_pi, total_log_pi = self.test_log_map(SO3.log_pi)\n", 214 | " correct_log_quaternion, total_log_quaternion = self.test_log_map(SO3.log_quaternion)\n", 215 | " correct_sophus, total_sophus = self.test_log_map(SO3.log_sophus)\n", 216 | " \n", 217 | " print(f\"SO3.log_baseline() had: {total_log_baseline - correct_log_baseline} / {total_log_baseline} \"\n", 218 | " f\"failed tests\")\n", 219 | " print(f\"SO3.log_pi() had: {total_log_pi - correct_log_pi} / {total_log_pi} failed tests\")\n", 220 | " print(f\"SO3.log_quaternion() had: {total_log_quaternion - correct_log_quaternion} / \"\n", 221 | " f\"{total_log_quaternion} failed tests\")\n", 222 | " print(f\"sophus.SO3(R).log() had: {total_sophus - correct_sophus} / {total_sophus} failed tests\")\n", 223 | " \n", 224 | " def generate(self, k=1000):\n", 225 | " angles_unit_vecs = np.random.uniform(low=0, high=2 * np.pi, size=(k, 2))\n", 226 | " cos_a = np.cos(angles_unit_vecs)\n", 227 | " sin_a = np.sin(angles_unit_vecs)\n", 228 | " x = cos_a[:, 0] * cos_a[:, 1]\n", 229 | " y = sin_a[:, 0] * cos_a[:, 1]\n", 230 | " z = sin_a[:, 1]\n", 231 | " unit_vecs = np.vstack((x, y, z)).transpose()\n", 232 | " \n", 233 | " thetas = np.random.uniform(low=0, high=np.pi, size=(k-20))\n", 234 | " thetas = np.hstack((thetas, np.array(10*[np.pi])))\n", 235 | " thetas = np.hstack((thetas, np.array(9*[1e-8])))\n", 236 | " thetas = np.hstack((thetas, np.array([0])))\n", 237 | " \n", 238 | " v = thetas[:, None] * unit_vecs\n", 239 | " return v\n", 240 | " " 241 | ] 242 | }, 243 | { 244 | "cell_type": "code", 245 | "execution_count": null, 246 | "metadata": { 247 | "scrolled": true 248 | }, 249 | "outputs": [], 250 | "source": [ 251 | "tests = TestSO3LogMap()\n", 252 | "tests.run()" 253 | ] 254 | }, 255 | { 256 | "cell_type": "markdown", 257 | "metadata": {}, 258 | "source": [ 259 | "### Conclusion:\n", 260 | "1. $\\log$ map in __baseline__ implementation failed for the rotation matrices corresponding to rotations by angles close to $\\pi$ (1e-8 neighborhood -> fails, 1e-7 neighborhood -> 1e-1 max abs difference, 1e-6 -> 1e-4, 1e-5 -> 1e-7 )\n", 261 | "2. $\\log$ map implementation adjusted for **angle $\\pi$** gives correct results with precision up to 1e-8 max absolute difference. At angle exactly equal to $\\pi$ the $\\log$ map is defined up to an overall sign.\n", 262 | "3. $\\log$ map implementation using intermediate **quaternion** representation (matrix -> quaternion -> angle/axis) gives correct results everywhere.\n", 263 | "4. **Sophus** implementation of $\\log$ map gives correct results too. Internally it uses the \"matrix to quaternion\" conversion from Eigen library." 264 | ] 265 | }, 266 | { 267 | "cell_type": "markdown", 268 | "metadata": {}, 269 | "source": [ 270 | "## 2. Deriving the Jacobians of $\\log$ map" 271 | ] 272 | }, 273 | { 274 | "cell_type": "markdown", 275 | "metadata": {}, 276 | "source": [ 277 | "$$ \\frac{\\partial log(R)^{\\vee}}{\\partial R} $$\n", 278 | "It is well-defined only in the manifold of $SO(3)$ matrices. \n", 279 | "However, we are trying to approximate it as if it was defined on the whole space of $M(3, 3)$ matrices.\n", 280 | "\n", 281 | "The resulting Jacobian will have shape $3 \\times 9$, where 9 corresponds to vectorized rotation matrix in row-major way." 282 | ] 283 | }, 284 | { 285 | "cell_type": "code", 286 | "execution_count": null, 287 | "metadata": {}, 288 | "outputs": [], 289 | "source": [ 290 | "def num_diff(N, M, func, a, eps=1e-8, project=None):\n", 291 | " J = np.zeros((N, M))\n", 292 | " h = np.zeros(M)\n", 293 | " shape_a = a.shape\n", 294 | " a_flattened = np.reshape(a, -1)\n", 295 | " precomp_func = func(a).reshape(N)\n", 296 | " \n", 297 | " for i in range(M):\n", 298 | " h[i] = eps\n", 299 | " disturbed = np.reshape(a_flattened + h, shape_a)\n", 300 | " if project is not None:\n", 301 | " disturbed = project(disturbed)\n", 302 | " J[:, i] = (func(disturbed).reshape(N) - precomp_func) / (eps)\n", 303 | " h[i] = 0\n", 304 | " return J\n", 305 | "\n", 306 | "# From sophus (Python) documentation\n", 307 | "# sp.to_orthogonal(R)\n", 308 | "# 3.if R is not a strict rotation matrix, normalize it. Uses Eigen3 \n", 309 | "# Eigen::Quaterniond q(R);\n", 310 | "# q.normalized().toRotationMatrix();\n", 311 | "\n", 312 | "def makeRotationMatrix(R):\n", 313 | " assert(R.shape == (3, 3))\n", 314 | " \n", 315 | " U, s, Vtr = np.linalg.svd(R, full_matrices=True)\n", 316 | " det = np.linalg.det(U @ Vtr)\n", 317 | " Diag = np.eye(3)\n", 318 | " Diag[2, 2] = det\n", 319 | " return U @ Diag @ Vtr" 320 | ] 321 | }, 322 | { 323 | "cell_type": "code", 324 | "execution_count": null, 325 | "metadata": {}, 326 | "outputs": [], 327 | "source": [ 328 | "def Dx_log_x_baseline(R):\n", 329 | " trR = R[0, 0] + R[1, 1] + R[2, 2]\n", 330 | " cos_theta = max(min(0.5 * (trR - 1), 1), -1)\n", 331 | " if cos_theta > 1 - 1e-8:\n", 332 | " return np.array([[0, 0, 0, 0, 0, -0.5, 0, 0.5, 0],\n", 333 | " [0, 0, 0.5, 0, 0, 0, -0.5, 0, 0], \n", 334 | " [0, -0.5, 0, 0.5, 0, 0, 0, 0, 0]])\n", 335 | " \n", 336 | " theta = np.arccos(cos_theta)\n", 337 | " sin_theta = np.sqrt(1 - cos_theta * cos_theta)\n", 338 | " n = np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], \n", 339 | " R[1, 0] - R[0, 1]])\n", 340 | " c = (theta * cos_theta - sin_theta) / (4 * sin_theta**3)\n", 341 | " a = c * n\n", 342 | " b = theta / (2 * sin_theta)\n", 343 | " \n", 344 | " return np.array([[a[0], 0, 0, 0, a[0], -b, 0, b, a[0]],\n", 345 | " [a[1], 0, b, 0, a[1], 0, -b, 0, a[1]],\n", 346 | " [a[2], -b, 0, b, a[2], 0, 0, 0, a[2]]])" 347 | ] 348 | }, 349 | { 350 | "cell_type": "code", 351 | "execution_count": null, 352 | "metadata": {}, 353 | "outputs": [], 354 | "source": [ 355 | "def Dx_log_x_pi(R):\n", 356 | " trR = R[0, 0] + R[1, 1] + R[2, 2]\n", 357 | " cos_theta = max(min(0.5 * (trR - 1), 1), -1)\n", 358 | " sin_theta = np.sqrt(1 - cos_theta * cos_theta)\n", 359 | " theta = np.arctan2(sin_theta, cos_theta)\n", 360 | " R_minus_R_T_vee = np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]])\n", 361 | "\n", 362 | " if abs(3 - trR) < 1e-8:\n", 363 | " # return Jacobian of log map at Theta = 0\n", 364 | " return np.array([[0, 0, 0, 0, 0, -0.5, 0, 0.5, 0],\n", 365 | " [0, 0, 0.5, 0, 0, 0, -0.5, 0, 0], \n", 366 | " [0, -0.5, 0, 0.5, 0, 0, 0, 0, 0]])\n", 367 | " \n", 368 | " S = R + R.transpose() + (1 - trR) * np.eye(3)\n", 369 | " rest_tr = 3 - trR\n", 370 | " n = np.ones(3)\n", 371 | " # Fix modules of n_i\n", 372 | " for i in range(3):\n", 373 | " n[i] = np.sqrt(max(0, S[i, i] / rest_tr))\n", 374 | " max_i = np.argmax(n)\n", 375 | "\n", 376 | " # Fix signs according to the sign of max element\n", 377 | " for i in range(3):\n", 378 | " if i != max_i:\n", 379 | " n[i] *= np.sign(S[max_i, i])\n", 380 | "\n", 381 | " # Fix an overall sign\n", 382 | " if any(np.sign(n) * np.sign(R_minus_R_T_vee) < 0):\n", 383 | " n = -n\n", 384 | " \n", 385 | " dn_dR_11 = np.zeros(3)\n", 386 | " dn_dR_22 = np.zeros(3)\n", 387 | " dn_dR_33 = np.zeros(3)\n", 388 | " if abs(n[0]) > np.finfo(float).eps:\n", 389 | " dn_dR_11[0] = (S[0, 0] + rest_tr) / (2 * n[0] * rest_tr * rest_tr)\n", 390 | " dn_dR_22[0] = (S[0, 0] - rest_tr) / (2 * n[0] * rest_tr * rest_tr)\n", 391 | " dn_dR_33[0] = dn_dR_22[0]\n", 392 | " if abs(n[1]) > np.finfo(float).eps:\n", 393 | " dn_dR_11[1] = (S[1, 1] - rest_tr) / (2 * n[1] * rest_tr * rest_tr)\n", 394 | " dn_dR_22[1] = (S[1, 1] + rest_tr) / (2 * n[1] * rest_tr * rest_tr)\n", 395 | " dn_dR_33[1] = dn_dR_11[1]\n", 396 | " if abs(n[2]) > np.finfo(float).eps:\n", 397 | " dn_dR_11[2] = (S[2, 2] - rest_tr) / (2 * n[2] * rest_tr * rest_tr)\n", 398 | " dn_dR_22[2] = dn_dR_11[2]\n", 399 | " dn_dR_33[2] = (S[2, 2] + rest_tr) / (2 * n[2] * rest_tr * rest_tr)\n", 400 | " \n", 401 | " dtheta_d_R_11 = -sin_theta * 0.5 + cos_theta * 0.5 * (dn_dR_11.dot(R_minus_R_T_vee))\n", 402 | " dtheta_d_R_22 = -sin_theta * 0.5 + cos_theta * 0.5 * (dn_dR_22.dot(R_minus_R_T_vee))\n", 403 | " dtheta_d_R_33 = -sin_theta * 0.5 + cos_theta * 0.5 * (dn_dR_33.dot(R_minus_R_T_vee)) \n", 404 | " \n", 405 | " dtheta_d_R_12 = cos_theta * 0.5 * (-n[2])\n", 406 | " dtheta_d_R_13 = cos_theta * 0.5 * (n[1])\n", 407 | " dtheta_d_R_23 = cos_theta * 0.5 * (-n[0])\n", 408 | " dtheta_d_R_21 = -dtheta_d_R_12\n", 409 | " dtheta_d_R_31 = -dtheta_d_R_13\n", 410 | " dtheta_d_R_32 = -dtheta_d_R_23\n", 411 | " \n", 412 | " J = np.zeros((3, 3, 3))\n", 413 | " \n", 414 | " J[0, 0, 0] = dtheta_d_R_11 * n[0] + theta * dn_dR_11[0]\n", 415 | " J[0, 1, 1] = dtheta_d_R_22 * n[0] + theta * dn_dR_22[0]\n", 416 | " J[0, 2, 2] = dtheta_d_R_33 * n[0] + theta * dn_dR_33[0]\n", 417 | " J[0, 0, 1] = dtheta_d_R_12 * n[0]\n", 418 | " J[0, 0, 2] = dtheta_d_R_13 * n[0]\n", 419 | " J[0, 1, 2] = dtheta_d_R_23 * n[0]\n", 420 | " J[0, 1, 0] = -J[0, 0, 1]\n", 421 | " J[0, 2, 0] = -J[0, 0, 2]\n", 422 | " J[0, 2, 1] = -J[0, 1, 2]\n", 423 | " \n", 424 | " J[1, 0, 0] = dtheta_d_R_11 * n[1] + theta * dn_dR_11[1]\n", 425 | " J[1, 1, 1] = dtheta_d_R_22 * n[1] + theta * dn_dR_22[1]\n", 426 | " J[1, 2, 2] = dtheta_d_R_33 * n[1] + theta * dn_dR_33[1]\n", 427 | " J[1, 0, 1] = dtheta_d_R_12 * n[1]\n", 428 | " J[1, 0, 2] = dtheta_d_R_13 * n[1]\n", 429 | " J[1, 1, 2] = dtheta_d_R_23 * n[1]\n", 430 | " J[1, 1, 0] = -J[1, 0, 1]\n", 431 | " J[1, 2, 0] = -J[1, 0, 2]\n", 432 | " J[1, 2, 1] = -J[1, 1, 2]\n", 433 | " \n", 434 | " J[2, 0, 0] = dtheta_d_R_11 * n[2] + theta * dn_dR_11[2]\n", 435 | " J[2, 1, 1] = dtheta_d_R_22 * n[2] + theta * dn_dR_22[2]\n", 436 | " J[2, 2, 2] = dtheta_d_R_33 * n[2] + theta * dn_dR_33[2]\n", 437 | " J[2, 0, 1] = dtheta_d_R_12 * n[2]\n", 438 | " J[2, 0, 2] = dtheta_d_R_13 * n[2]\n", 439 | " J[2, 1, 2] = dtheta_d_R_23 * n[2]\n", 440 | " J[2, 1, 0] = -J[2, 0, 1]\n", 441 | " J[2, 2, 0] = -J[2, 0, 2]\n", 442 | " J[2, 2, 1] = -J[2, 1, 2]\n", 443 | " \n", 444 | " return J.reshape(3, 9)" 445 | ] 446 | }, 447 | { 448 | "cell_type": "code", 449 | "execution_count": null, 450 | "metadata": {}, 451 | "outputs": [], 452 | "source": [ 453 | "def Dquaternion_DR(R):\n", 454 | " \"\"\"Computes d quaternion(R) / d R , 4 x 9 Jacobian.\"\"\"\n", 455 | " J_quat = np.zeros((4, 3, 3))\n", 456 | " t = R[0, 0] + R[1, 1] + R[2, 2]\n", 457 | " if t > 0:\n", 458 | " # case 1\n", 459 | " isqrt_t = 1 / np.sqrt(1 + t)\n", 460 | " J_quat[0, :, :] = 0.25 * isqrt_t * np.eye(3)\n", 461 | " J_quat[1, :, :] = -0.25 * isqrt_t / (1 + t) * (R[2, 1] - R[1, 2]) * np.eye(3)\n", 462 | " J_quat[1, 2, 1] = 0.5 * isqrt_t\n", 463 | " J_quat[1, 1, 2] = -0.5 * isqrt_t\n", 464 | " J_quat[2, :, :] = -0.25 * isqrt_t / (1 + t) * (R[0, 2] - R[2, 0]) * np.eye(3)\n", 465 | " J_quat[2, 0, 2] = 0.5 * isqrt_t\n", 466 | " J_quat[2, 2, 0] = -0.5 * isqrt_t\n", 467 | " J_quat[3, :, :] = -0.25 * isqrt_t / (1 + t) * (R[1, 0] - R[0, 1]) * np.eye(3)\n", 468 | " J_quat[3, 1, 0] = 0.5 * isqrt_t\n", 469 | " J_quat[3, 0, 1] = -0.5 * isqrt_t\n", 470 | " else:\n", 471 | " i = 0\n", 472 | " if R[1, 1] > R[0, 0]:\n", 473 | " i = 1\n", 474 | " if R[2, 2] > R[i, i]:\n", 475 | " i = 2\n", 476 | " j = (i + 1) % 3\n", 477 | " k = (j + 1) % 3\n", 478 | " r = np.sqrt(R[i, i] - R[j, j] - R[k, k] + 1)\n", 479 | " i_r = 1 / r\n", 480 | " i_r_cube = 1 / ((R[i, i] - R[j, j] - R[k, k] + 1) * r)\n", 481 | " r_eye = np.eye(3)\n", 482 | " r_eye[j, j] = -1\n", 483 | " r_eye[k, k] = -1\n", 484 | " J_quat[1 + i, :, :] = 0.25 * i_r * r_eye\n", 485 | " \n", 486 | " J_quat[0, :, :] = -0.25 * (R[k, j] - R[j, k]) * i_r_cube * r_eye\n", 487 | " J_quat[0, k, j] = 0.5 * i_r\n", 488 | " J_quat[0, j, k] = -0.5 * i_r\n", 489 | " J_quat[1+j, :, :] = -0.25 * (R[j, i] + R[i, j]) * i_r_cube * r_eye\n", 490 | " J_quat[1+j, j, i] = 0.5 * i_r\n", 491 | " J_quat[1+j, i, j] = 0.5 * i_r\n", 492 | " J_quat[1+k, :, :] = -0.25 * (R[k, i] + R[i, k]) * i_r_cube * r_eye\n", 493 | " J_quat[1+k, k, i] = 0.5 * i_r\n", 494 | " J_quat[1+k, i, k] = 0.5 * i_r\n", 495 | " return J_quat.reshape((4, 9))\n", 496 | "\n", 497 | "\n", 498 | "def Dlog_Dquaternion(q):\n", 499 | " \"\"\"Computes d log(q) / d q , 3 x 4 Jacobian.\"\"\"\n", 500 | " J_vec = np.zeros((3, 4))\n", 501 | " squared_n = q[1]*q[1] + q[2]*q[2] + q[3]*q[3]\n", 502 | " n = np.sqrt(squared_n)\n", 503 | " w = q[0]\n", 504 | " squared_w = w*w\n", 505 | " if squared_n < np.finfo(float).eps * np.finfo(float).eps:\n", 506 | " # Theta close to 0\n", 507 | " two_atan_nbyw_by_n = 2 / w - 2.0 / 3 * squared_n / (w * squared_w)\n", 508 | " d_q0 = -2 / squared_w + 2 * squared_n / (squared_w * squared_w)\n", 509 | " d_q1 = -4.0 / 3 * q[1] / (w * squared_w)\n", 510 | " d_q2 = -4.0 / 3 * q[2] / (w * squared_w)\n", 511 | " d_q3 = -4.0 / 3 * q[3] / (w * squared_w)\n", 512 | " else:\n", 513 | " if (abs(w) < np.finfo(float).eps):\n", 514 | " # Theta close to pi\n", 515 | " d_q0 = -2 / (squared_w + squared_n)\n", 516 | " if w >= 0:\n", 517 | " # From left\n", 518 | " # 2 * arccos(w), w -> 0\n", 519 | " # arcos'(w) = -1 / (sqrt(1 - w*w)) = |w=0| = -1\n", 520 | " \n", 521 | " two_atan_nbyw_by_n = np.pi / n\n", 522 | " d_q1 = -q[1] * np.pi / (squared_n * n)\n", 523 | " d_q2 = -q[2] * np.pi / (squared_n * n)\n", 524 | " d_q3 = -q[3] * np.pi / (squared_n * n)\n", 525 | " else:\n", 526 | " # From right\n", 527 | " two_atan_nbyw_by_n = -np.pi / n\n", 528 | " d_q1 = q[1] * np.pi / (squared_n * n)\n", 529 | " d_q2 = q[2] * np.pi / (squared_n * n)\n", 530 | " d_q3 = q[3] * np.pi / (squared_n * n)\n", 531 | " else:\n", 532 | " # Regular case\n", 533 | " two_atan_nbyw_by_n = 2 * np.arctan(n / w) / n\n", 534 | " d_q0 = -2 / (squared_w + squared_n)\n", 535 | " c0 = (2 / (squared_n * (w + squared_n/w)) - 2 * np.arctan(n / w) / (squared_n * n))\n", 536 | " d_q1 = c0 * q[1]\n", 537 | " d_q2 = c0 * q[2]\n", 538 | " d_q3 = c0 * q[3]\n", 539 | " \n", 540 | " J_vec[:, 0] = d_q0 * q[1:]\n", 541 | " J_vec[:, 1] = d_q1 * q[1:]\n", 542 | " J_vec[0, 1] += two_atan_nbyw_by_n\n", 543 | " J_vec[:, 2] = d_q2 * q[1:]\n", 544 | " J_vec[1, 2] += two_atan_nbyw_by_n\n", 545 | " J_vec[:, 3] = d_q3 * q[1:]\n", 546 | " J_vec[2, 3] += two_atan_nbyw_by_n\n", 547 | " return J_vec\n", 548 | "\n", 549 | "\n", 550 | "def Dlog_Dquaternion2(q):\n", 551 | " \"\"\"Computes d log(q) / d q , 3 x 4 Jacobian.\"\"\"\n", 552 | " J_vec = np.zeros((3, 4))\n", 553 | " squared_n = q[1]*q[1] + q[2]*q[2] + q[3]*q[3]\n", 554 | " n = np.sqrt(squared_n)\n", 555 | " w = q[0]\n", 556 | " squared_w = w*w\n", 557 | " sign = 1\n", 558 | " if w < 0:\n", 559 | " sign = -1\n", 560 | " w = -w\n", 561 | " if squared_n < np.finfo(float).eps * np.finfo(float).eps:\n", 562 | " # n (~Theta) close to 0\n", 563 | " two_atan_nbyw_by_n = 2 / w - 2.0 / 3 * squared_n / (w * squared_w)\n", 564 | " d_q0 = -2 / squared_w + 2 * squared_n / (squared_w * squared_w)\n", 565 | " d_q1 = -4.0 / 3 * q[1] / (w * squared_w)\n", 566 | " d_q2 = -4.0 / 3 * q[2] / (w * squared_w)\n", 567 | " d_q3 = -4.0 / 3 * q[3] / (w * squared_w)\n", 568 | " else:\n", 569 | " # Regular case\n", 570 | " two_atan_nbyw_by_n = 4 * np.arctan(n / (w + np.sqrt(squared_w + squared_n))) / n\n", 571 | " d_q0 = -2 / (squared_w + squared_n)\n", 572 | " c0 = (2 * w - two_atan_nbyw_by_n) / squared_n\n", 573 | " d_q1 = c0 * q[1]\n", 574 | " d_q2 = c0 * q[2]\n", 575 | " d_q3 = c0 * q[3]\n", 576 | " \n", 577 | " J_vec[:, 0] = sign * d_q0 * q[1:]\n", 578 | " J_vec[:, 1] = d_q1 * q[1:]\n", 579 | " J_vec[0, 1] += two_atan_nbyw_by_n\n", 580 | " J_vec[:, 2] = d_q2 * q[1:]\n", 581 | " J_vec[1, 2] += two_atan_nbyw_by_n\n", 582 | " J_vec[:, 3] = d_q3 * q[1:]\n", 583 | " J_vec[2, 3] += two_atan_nbyw_by_n\n", 584 | " J_vec = J_vec * sign\n", 585 | " return J_vec\n", 586 | "\n", 587 | "\n", 588 | "def Dx_log_x_quaternion(R):\n", 589 | " \"\"\"Computes d log(R) / d R , 3 x 9 Jacobian.\"\"\"\n", 590 | " J_quat = Dquaternion_DR(R)\n", 591 | " q = SO3.matrix2quaternion(R)\n", 592 | " J_vec = Dlog_Dquaternion2(q)\n", 593 | " return J_vec @ J_quat" 594 | ] 595 | }, 596 | { 597 | "cell_type": "markdown", 598 | "metadata": {}, 599 | "source": [ 600 | "Check `Dquaternion_DR` and `Dlog_Dquaternion` against numerical differentiation." 601 | ] 602 | }, 603 | { 604 | "cell_type": "code", 605 | "execution_count": null, 606 | "metadata": {}, 607 | "outputs": [], 608 | "source": [ 609 | "# 1. Check Dquaternion_DR\n", 610 | "errors = 0\n", 611 | "total = 1000\n", 612 | "min_angle = np.pi+1\n", 613 | "for i in range(total):\n", 614 | " v = np.random.rand(3) * 2 - 1\n", 615 | " norm = np.random.rand(1) * (np.pi + 1e-8)\n", 616 | " v = v / np.linalg.norm(v) * norm\n", 617 | " sp_obj = sp.SO3.exp(v)\n", 618 | " R = sp_obj.matrix()\n", 619 | " \n", 620 | " numerical = num_diff(4, 9, func=SO3.matrix2quaternion, a=R, eps=1e-8, project=None)\n", 621 | " analytical = Dquaternion_DR(R)\n", 622 | " \n", 623 | " precision = 1e-7\n", 624 | " if np.max(np.linalg.norm(numerical - analytical)) > precision:\n", 625 | " errors += 1\n", 626 | " min_angle = min(min_angle, np.linalg.norm(v))\n", 627 | " print(\"iter = \", i+1)\n", 628 | " print(v, np.linalg.norm(v))\n", 629 | " print(\"R:\\n\", R)\n", 630 | " print(\"num:\\n\", np.round(numerical, 4))\n", 631 | " print(\"analytical:\\n\", np.round(analytical, 4))\n", 632 | " print(\"diff:\\n\", np.linalg.norm(numerical - analytical))\n", 633 | "print(\"\\n ___________________________\\n num of errors: \", errors, \"/\", total)\n", 634 | "# print(\"min_angle = \", min_angle)" 635 | ] 636 | }, 637 | { 638 | "cell_type": "markdown", 639 | "metadata": {}, 640 | "source": [ 641 | "Conclusion:\n", 642 | "\n", 643 | "1. `Dquaternion_DR` gives the same results as the numerical differentiation (without projection) up to precision `1e-7` for `eps=1e-8` in numerical discretization. For different discretization steps the precision changes (non-monotonously).\n", 644 | "2. Both numerical differentiation and analytical Jacobian are continuously changing at 0 and at $\\pi$.\n", 645 | "3. Numerical differentiation with both matrix `projections` change the result.\n", 646 | "4. Without projection the Jaobians are sparse with bunch of zeros corresponding to vector part of quaternion." 647 | ] 648 | }, 649 | { 650 | "cell_type": "code", 651 | "execution_count": null, 652 | "metadata": {}, 653 | "outputs": [], 654 | "source": [ 655 | "# 2. Check Dlog_Dquaternion\n", 656 | "\n", 657 | "def normalize(q):\n", 658 | " norm = np.linalg.norm(q)\n", 659 | " return q / norm\n", 660 | "\n", 661 | "errors = 0\n", 662 | "total = 1000\n", 663 | "min_angle = np.pi+1\n", 664 | "max_angle = -1\n", 665 | "\n", 666 | "for i in range(total):\n", 667 | " v = np.random.rand(3) * 2 - 1\n", 668 | " norm = np.random.rand(1) * (np.pi + 1e-8)\n", 669 | " v = v / np.linalg.norm(v) * norm\n", 670 | " \n", 671 | " sp_obj = sp.SO3.exp(v)\n", 672 | " R = sp_obj.matrix()\n", 673 | " q = SO3.matrix2quaternion(R)\n", 674 | " \n", 675 | " num = num_diff(3, 4, func=SO3.quaternion2vec, a=q, eps=1e-8, project=normalize)\n", 676 | " J_vec = Dlog_Dquaternion2(q)\n", 677 | " precision = 1e-6\n", 678 | " if np.max(np.linalg.norm(J_vec - num)) > precision:\n", 679 | " errors += 1\n", 680 | " min_angle = min(min_angle, np.linalg.norm(v))\n", 681 | " max_angle = max(max_angle, np.linalg.norm(v))\n", 682 | " print(\"iter = \", i+1)\n", 683 | " print(v, np.linalg.norm(v))\n", 684 | " print(\"R:\\n\", R)\n", 685 | "\n", 686 | " print(\"numerical:\\n\", num)\n", 687 | " print(\"analytical:\\n\", J_vec)\n", 688 | " print(\"diff:\\n\", np.linalg.norm(J_vec - num))\n", 689 | "\n", 690 | "print(\"\\n ___________________________\\n num of errors: \", errors, \"/\", total)\n", 691 | "print(\"min_angle = \", min_angle)\n", 692 | "print(\"max_angle = \", max_angle)" 693 | ] 694 | }, 695 | { 696 | "cell_type": "markdown", 697 | "metadata": {}, 698 | "source": [ 699 | "Conclusion:\n", 700 | "\n", 701 | "1. `Dlog_Dquaternion` gives the same results as the numerical differentiation (with and without projection=`normalize`) up to max total difference = 1e-6 for discretization step = 1e-8. The precision depends on discretization step non-monotonously.\n", 702 | "2. However, numerical differentiation (with and without projection) diverges for the angles in proximity of Pi (proximity distance $\\approx$ step in numerical discretization). Divergence is due to the change of the `sign` of $q_0 = \\cos\\big(\\frac{\\theta}{2}\\big)$ in numerical step, and only $\\frac{\\partial v}{\\partial q_0}$ diverges. \n", 703 | "3. The Jacobian `Dlog_Dquaternion` is not continuous around $\\pi$, particularly, $\\frac{\\partial \\vec{v}}{\\partial q_{1:3}}$ changes its sign whenever the angle crosses $\\pi$, i.e. $J(\\pi - \\epsilon) \\neq J(\\pi+\\epsilon)$." 704 | ] 705 | }, 706 | { 707 | "cell_type": "code", 708 | "execution_count": null, 709 | "metadata": {}, 710 | "outputs": [], 711 | "source": [ 712 | "def Dx_log_x_numerical(R, func=SO3.log_quaternion, project=sp.to_orthogonal, eps=1e-8):\n", 713 | " return num_diff(N=3, M=9, func=func, a=R, eps=eps, project=project)" 714 | ] 715 | }, 716 | { 717 | "cell_type": "code", 718 | "execution_count": null, 719 | "metadata": {}, 720 | "outputs": [], 721 | "source": [ 722 | "SO3.Dx_log_x_baseline = Dx_log_x_baseline\n", 723 | "SO3.Dx_log_x_pi = Dx_log_x_pi\n", 724 | "SO3.Dx_log_x_quaternion = Dx_log_x_quaternion\n", 725 | "SO3.Dx_log_x_numerical = Dx_log_x_numerical" 726 | ] 727 | }, 728 | { 729 | "cell_type": "markdown", 730 | "metadata": {}, 731 | "source": [ 732 | "## Looking at the Jacobians of $\\log$ map\n", 733 | "$$ \\frac{\\partial log(R)}{\\partial R} $$\n", 734 | "1. Compare different analytical implementations of Jacobians with numerical differentiation (both projected to SO(3) and not)" 735 | ] 736 | }, 737 | { 738 | "cell_type": "code", 739 | "execution_count": null, 740 | "metadata": {}, 741 | "outputs": [], 742 | "source": [ 743 | "v_set = [np.array([0, 0, 0]), np.array([1, 1, 1]), np.array([0, 1, -0.7]), \n", 744 | " np.array([0, 1, -0.7])*1e-7, np.array([0, 1, -0.7])*1e-6, np.array([0, 1, -0.7])*1e-5, \n", 745 | " np.array([0, 1, -0.7])*1e-4, np.array([0, 1, -0.7])*1e-3, np.array([0, 1, -0.7])*1e-2,\n", 746 | " np.array([0, 1, -0.7])*1e-1,\n", 747 | " np.array([np.pi, 0, 0]), np.array([0, np.pi-1e-8, 0]), np.array([0, 0, -np.pi+1e-7]),]\n", 748 | "\n", 749 | "round_out_dec = 4\n", 750 | "\n", 751 | "for i, v in enumerate(v_set):\n", 752 | " print(\"_________________________________________\")\n", 753 | " print(\"Test \", i+1)\n", 754 | " sp_obj = sp.SO3.exp(v)\n", 755 | " R = sp_obj.matrix()\n", 756 | " print(\"v = \", v)\n", 757 | " print(\"Angle = \", np.linalg.norm(v))\n", 758 | " print(\"R:\\n\", R)\n", 759 | " \n", 760 | " print(\"\\nAnalytical baseline:\")\n", 761 | " print(np.round(SO3.Dx_log_x_baseline(R), round_out_dec))\n", 762 | " print(\"Numerical baseline raw:\")\n", 763 | " print(np.round(SO3.Dx_log_x_numerical(R, SO3.log_baseline, None), round_out_dec))\n", 764 | " print(\"Numerical baseline projected (norm quaternion):\")\n", 765 | " print(np.round(SO3.Dx_log_x_numerical(R, SO3.log_baseline), round_out_dec))\n", 766 | " print(\"Numerical baseline projected (SVD):\")\n", 767 | " print(np.round(SO3.Dx_log_x_numerical(R, SO3.log_baseline, makeRotationMatrix), round_out_dec))\n", 768 | " \n", 769 | " print(\"\\nAnalytical Pi:\")\n", 770 | " print(np.round(SO3.Dx_log_x_pi(R), round_out_dec))\n", 771 | " print(\"Numerical Pi raw:\")\n", 772 | " print(np.round(SO3.Dx_log_x_numerical(R, SO3.log_pi, None), round_out_dec))\n", 773 | " print(\"Numerical Pi projected (norm quaternion):\")\n", 774 | " print(np.round(SO3.Dx_log_x_numerical(R, SO3.log_pi), round_out_dec))\n", 775 | " print(\"Numerical Pi projected (SVD):\")\n", 776 | " print(np.round(SO3.Dx_log_x_numerical(R, SO3.log_pi, makeRotationMatrix), round_out_dec))\n", 777 | " \n", 778 | " print(\"\\nAnalytical quaternion:\")\n", 779 | " print(np.round(SO3.Dx_log_x_quaternion(R), round_out_dec))\n", 780 | " print(\"Numerical quaternion raw:\")\n", 781 | " print(np.round(SO3.Dx_log_x_numerical(R, SO3.log_quaternion, None), round_out_dec))\n", 782 | " print(\"Numerical quaternion projected (norm quaternion):\")\n", 783 | " print(np.round(SO3.Dx_log_x_numerical(R, SO3.log_quaternion), round_out_dec))\n", 784 | " print(\"Numerical quaternion projected (SVD):\")\n", 785 | " print(np.round(SO3.Dx_log_x_numerical(R, SO3.log_quaternion, makeRotationMatrix), round_out_dec))\n", 786 | " \n", 787 | " print(\"Numerical Sophus projected (norm quaternion):\")\n", 788 | " print(np.round(SO3.Dx_log_x_numerical(R, SO3.log_sophus), round_out_dec))\n", 789 | " print(\"Numerical Sophus projected (SVD):\")\n", 790 | " print(np.round(SO3.Dx_log_x_numerical(R, SO3.log_sophus, makeRotationMatrix), round_out_dec))\n", 791 | " " 792 | ] 793 | }, 794 | { 795 | "cell_type": "markdown", 796 | "metadata": {}, 797 | "source": [ 798 | "### Conclusions:\n", 799 | "1. Intermediate angles:\n", 800 | " 1. All of the Jacobians give different results.\n", 801 | " 2. For all methods analytical Jacobian corresponds to numerical raw (except `Analytical Pi` $\\neq$ `Numerical Pi raw`).\n", 802 | " 3. `Numerical projected (norm quaternion)` $\\approx$ `Analytical quaternion`, and the results are the same for all mappings (except null components of mapping around pi). \n", 803 | " 4. `Numerical projected (SVD)` are the same for all mappings (except null components of mapping around pi).\n", 804 | " \n", 805 | "2. Small angles (Both `Quaternion` and `Baseline` methods are the best):\n", 806 | " 1. `Pi` analytical, numerical diverge starting from angles > 1e-4. (Before 1e-4 we make case differentiation).\n", 807 | " 2. `Quaternion` and `Baseline` methods (both analytical and numerical) do not diverge, and are changing continuously with increasing angle.\n", 808 | " 3. `Numerical projected (norm quaternion)` prevents `Around Pi` method from diverging.\n", 809 | " 4. `Numerical projected (SVD)` does not always prevent `Around Pi` method from diverging.\n", 810 | " \n", 811 | "3. Angles close to $\\pi$ (`Quaternion` method is best):\n", 812 | " 1. `Baseline` analytical, numerical (raw, projected) - diverge.\n", 813 | " 2. `Pi` analytical - do not diverge. But it nullifies the components corresponding to n(i) = 0.\n", 814 | " 3. `Pi` numerical raw - diverges.\n", 815 | " 4. `Quaternion` analytical - does not diverge, and continuously changes with decreasing angle. \n", 816 | " 5. `Quaternion` numerical - diverges at close (~discretization step) proximity of $\\pi$ , when the angle crosses $\\pi$. \n", 817 | " 6. All `numerical projected` help to avoid divergence. But they also diverge at close (~discretization step) proximity of $\\pi$ , when the angle crosses $\\pi$." 818 | ] 819 | }, 820 | { 821 | "cell_type": "markdown", 822 | "metadata": {}, 823 | "source": [ 824 | "## 3. Checking the correctness of the above Jacobians." 825 | ] 826 | }, 827 | { 828 | "cell_type": "markdown", 829 | "metadata": {}, 830 | "source": [ 831 | "We already know the expression for $\\frac{\\partial \\log (R \\boxplus x)}{\\partial x} \\big|_{x=0} = J_r^{-1}$, and for $\\frac{\\partial (R \\boxplus x)}{\\partial x} \\big|_{x=0}$.\n", 832 | "\n", 833 | "So we can check the following chain rule equation:\n", 834 | "\n", 835 | "$$ \\frac{\\partial \\log (R \\boxplus x)}{\\partial x} \\big|_{x=0} = \n", 836 | "\\frac{\\partial log(R)}{\\partial R} \n", 837 | "\\frac{\\partial (R \\boxplus x)}{\\partial x} \\big|_{x=0}$$" 838 | ] 839 | }, 840 | { 841 | "cell_type": "code", 842 | "execution_count": null, 843 | "metadata": {}, 844 | "outputs": [], 845 | "source": [ 846 | "def log_R_boxplus_x(R, x):\n", 847 | " return SO3.log_sophus(R @ sp.SO3.exp(x).matrix())\n", 848 | "\n", 849 | "def box_plus(R, x):\n", 850 | " return R @ sp.SO3.exp(x).matrix()\n", 851 | "\n", 852 | "def num_diff_2_args(N, M, func, R, eps=1e-8):\n", 853 | " J = np.zeros((N, M))\n", 854 | " h = np.zeros(M)\n", 855 | " precomp_func = func(R, h).reshape(N)\n", 856 | " for i in range(M):\n", 857 | " h[i] = eps\n", 858 | " J[:, i] = (func(R, h).reshape(N) - precomp_func) / (eps)\n", 859 | " h[i] = 0\n", 860 | " return J" 861 | ] 862 | }, 863 | { 864 | "cell_type": "code", 865 | "execution_count": null, 866 | "metadata": {}, 867 | "outputs": [], 868 | "source": [ 869 | "def dx_log_R_mul_exp_x_at_0(R):\n", 870 | " \"\"\"Computes d log(R exp(x)) / dx at x=0, 3x3 Inverse right Jacobian $J_r^{-1}$\"\"\"\n", 871 | " phi = SO3.log_sophus(R)\n", 872 | " phi_norm2 = sum(phi**2)\n", 873 | " phi_hat = sp.SO3.hat(phi)\n", 874 | " phi_hat2 = phi_hat @ phi_hat\n", 875 | " \n", 876 | " J = np.eye(3)\n", 877 | " J += phi_hat / 2;\n", 878 | " if phi_norm2 > np.finfo(float).eps:\n", 879 | " phi_norm = np.sqrt(phi_norm2)\n", 880 | " # assert(phi_norm <= np.pi + np.finfo(float).eps)\n", 881 | " if phi_norm < np.pi - np.finfo(float).eps:\n", 882 | " J += phi_hat2 * (1 / phi_norm2 - (1 + np.cos(phi_norm)) / (2 * phi_norm * np.sin(phi_norm)))\n", 883 | " else:\n", 884 | " # Approximate around pi\n", 885 | " J += phi_hat2 / (np.pi * np.pi)\n", 886 | " else:\n", 887 | " # Around 0\n", 888 | " J += phi_hat2 / 12\n", 889 | " return J" 890 | ] 891 | }, 892 | { 893 | "cell_type": "code", 894 | "execution_count": null, 895 | "metadata": {}, 896 | "outputs": [], 897 | "source": [ 898 | "# 1. Check `dx_log_R_mul_exp_x_at_0` numerically:\n", 899 | "errors = 0\n", 900 | "total = 1000\n", 901 | "min_angle = np.pi+1\n", 902 | "max_angle = -1\n", 903 | "np.random.seed(42)\n", 904 | "\n", 905 | "for i in range(total):\n", 906 | " v = np.random.rand(3) * 2 - 1\n", 907 | " if i == 0:\n", 908 | " norm = np.pi - 1e-9\n", 909 | " else:\n", 910 | " norm = np.random.rand(1) * (np.pi + 1e-8)\n", 911 | " v = v / np.linalg.norm(v) * norm\n", 912 | " \n", 913 | " sp_obj = sp.SO3.exp(v)\n", 914 | " R = sp_obj.matrix()\n", 915 | " numerical = num_diff_2_args(3, 3, log_R_boxplus_x, R, eps=1e-8)\n", 916 | " analytical = dx_log_R_mul_exp_x_at_0(R)\n", 917 | " \n", 918 | " precision = 1e-6\n", 919 | " if np.max(np.linalg.norm(numerical - analytical)) > precision:\n", 920 | " errors += 1\n", 921 | " min_angle = min(min_angle, np.linalg.norm(v))\n", 922 | " max_angle = max(max_angle, np.linalg.norm(v))\n", 923 | " print(\"iter = \", i+1)\n", 924 | " print(v, np.linalg.norm(v))\n", 925 | " print(\"R:\\n\", R)\n", 926 | " print(\"num:\\n\", numerical)\n", 927 | " print(\"analytical:\\n\", analytical)\n", 928 | " print(\"diff:\\n\", np.linalg.norm(numerical - analytical))\n", 929 | "print(\"\\n ___________________________\\n num of errors: \", errors, \"/\", total)\n", 930 | "print(\"min_angle = \", min_angle)\n", 931 | "print(\"max_angle = \", max_angle)\n" 932 | ] 933 | }, 934 | { 935 | "cell_type": "markdown", 936 | "metadata": {}, 937 | "source": [ 938 | "Conclusion:\n", 939 | "Comparing analytical and numerical Jacobians at point $x=0$:\n", 940 | "$$ \\frac{\\partial \\log (R \\boxplus x)}{\\partial x} \\big|_{x=0} $$\n", 941 | "1. Analytical derrivative continuously changes up to $\\pi$.\n", 942 | "2. However, Analytical Jacobian transposes once the angle crosses $\\pi$, i.e. $J(\\pi) \\approx J(\\pi - \\epsilon) = J^T(\\pi+\\epsilon)$.\n", 943 | "2. Numerical differentiation diverges close to $\\pi$ (log sophus). For close proximity of $\\pi$ ~ discretization step.\n", 944 | "3. Everywhere else the two methods give the same results up to precision 1e-6." 945 | ] 946 | }, 947 | { 948 | "cell_type": "code", 949 | "execution_count": null, 950 | "metadata": {}, 951 | "outputs": [], 952 | "source": [ 953 | "def dx_R_mul_exp_x_at_0(R):\n", 954 | " \"\"\"Computes d (R exp(x)) / dx at x=0, 9x3 Jacobian.\"\"\"\n", 955 | " G_1 = (R @ sp.SO3.hat([1, 0, 0])).reshape(9)\n", 956 | " G_2 = (R @ sp.SO3.hat([0, 1, 0])).reshape(9)\n", 957 | " G_3 = (R @ sp.SO3.hat([0, 0, 1])).reshape(9)\n", 958 | " J = np.zeros((9, 3))\n", 959 | " J[:, 0] = G_1\n", 960 | " J[:, 1] = G_2\n", 961 | " J[:, 2] = G_3\n", 962 | " return J" 963 | ] 964 | }, 965 | { 966 | "cell_type": "code", 967 | "execution_count": null, 968 | "metadata": {}, 969 | "outputs": [], 970 | "source": [ 971 | "def dx_exp_x(x):\n", 972 | " \"\"\"Computes d (exp(x)) / dx, 9x3 Jacobian.\"\"\"\n", 973 | " norm_x = np.linalg.norm(x)\n", 974 | " norm_sq = norm_x * norm_x\n", 975 | " e_1 = [1, 0, 0]\n", 976 | " e_2 = [0, 1, 0]\n", 977 | " e_3 = [0, 0, 1]\n", 978 | " if norm_x < 1e-8:\n", 979 | " G_1 = (sp.SO3.hat(e_1)).reshape(9)\n", 980 | " G_2 = (sp.SO3.hat(e_2)).reshape(9)\n", 981 | " G_3 = (sp.SO3.hat(e_3)).reshape(9)\n", 982 | " else:\n", 983 | " exp_x = sp.SO3.exp(x).matrix()\n", 984 | " x_hat = sp.SO3.hat(x)\n", 985 | " id_minus_exp = np.eye(3) - exp_x\n", 986 | " a_1 = sp.SO3.hat(np.cross(x, id_minus_exp @ e_1))\n", 987 | " a_2 = sp.SO3.hat(np.cross(x, id_minus_exp @ e_2))\n", 988 | " a_3 = sp.SO3.hat(np.cross(x, id_minus_exp @ e_3))\n", 989 | " G_1 = ((x[0]*x_hat + a_1) / norm_sq @ exp_x).reshape(9)\n", 990 | " G_2 = ((x[1]*x_hat + a_2) / norm_sq @ exp_x).reshape(9)\n", 991 | " G_3 = ((x[2]*x_hat + a_3) / norm_sq @ exp_x).reshape(9)\n", 992 | " J = np.zeros((9, 3))\n", 993 | " J[:, 0] = G_1\n", 994 | " J[:, 1] = G_2\n", 995 | " J[:, 2] = G_3\n", 996 | " return J" 997 | ] 998 | }, 999 | { 1000 | "cell_type": "code", 1001 | "execution_count": null, 1002 | "metadata": {}, 1003 | "outputs": [], 1004 | "source": [ 1005 | "def dx_R_mul_exp_x(R, x):\n", 1006 | " J = dx_exp_x(x)\n", 1007 | " for i in range(3):\n", 1008 | " J[:, i] = (R @ (J[:, i]).reshape(3, 3)).reshape(9)\n", 1009 | " return J" 1010 | ] 1011 | }, 1012 | { 1013 | "cell_type": "code", 1014 | "execution_count": null, 1015 | "metadata": {}, 1016 | "outputs": [], 1017 | "source": [ 1018 | "# 2. Check `dx_exp_x` numerically:\n", 1019 | "\n", 1020 | "errors = 0\n", 1021 | "total = 1000\n", 1022 | "min_angle = np.pi+1\n", 1023 | "max_angle = -1\n", 1024 | "np.random.seed(42)\n", 1025 | "for i in range(total):\n", 1026 | " v = np.random.rand(3) * 2 - 1\n", 1027 | " if i == 0:\n", 1028 | " norm = np.pi\n", 1029 | " else:\n", 1030 | " norm = np.random.rand(1) * (np.pi + 1e-8)\n", 1031 | " v = v / np.linalg.norm(v) * norm\n", 1032 | " numerical = num_diff(9, 3, lambda w: sp.SO3.exp(w).matrix(), v)\n", 1033 | " analytical = dx_exp_x(v)\n", 1034 | " if np.max(np.linalg.norm(numerical - analytical)) > 1e-6:\n", 1035 | " errors += 1\n", 1036 | " min_angle = min(min_angle, np.linalg.norm(v))\n", 1037 | " max_angle = max(max_angle, np.linalg.norm(v))\n", 1038 | " print(\"iter = \", i+1)\n", 1039 | " print(v, np.linalg.norm(v))\n", 1040 | " print(\"R:\\n\", R)\n", 1041 | " print(\"num:\\n\", numerical)\n", 1042 | " print(\"analytical:\\n\", analytical)\n", 1043 | " print(\"diff:\\n\", np.linalg.norm(numerical - analytical))\n", 1044 | "print(\"\\n ___________________________\\n num of errors: \", errors, \"/\", total)\n", 1045 | "print(\"min_angle = \", min_angle)\n", 1046 | "print(\"max_angle = \", max_angle)" 1047 | ] 1048 | }, 1049 | { 1050 | "cell_type": "code", 1051 | "execution_count": null, 1052 | "metadata": {}, 1053 | "outputs": [], 1054 | "source": [ 1055 | "# 3. Check `dx_R_mul_exp_x_at_0` numerically:\n", 1056 | "\n", 1057 | "errors = 0\n", 1058 | "total = 1000\n", 1059 | "min_angle = np.pi+1\n", 1060 | "max_angle = -1\n", 1061 | "np.random.seed(42)\n", 1062 | "for i in range(total):\n", 1063 | " v = np.random.rand(3) * 2 - 1\n", 1064 | " if i == 0:\n", 1065 | " norm = np.pi\n", 1066 | " else:\n", 1067 | " norm = np.random.rand(1) * (np.pi + 1e-8)\n", 1068 | " v = v / np.linalg.norm(v) * norm\n", 1069 | " \n", 1070 | " sp_obj = sp.SO3.exp(v)\n", 1071 | " R = sp_obj.matrix()\n", 1072 | " \n", 1073 | " numerical = num_diff_2_args(9, 3, box_plus, R)\n", 1074 | " analytical = dx_R_mul_exp_x_at_0(R)\n", 1075 | " if np.max(np.linalg.norm(numerical - analytical)) > 1e-7:\n", 1076 | " errors += 1\n", 1077 | " min_angle = min(min_angle, np.linalg.norm(v))\n", 1078 | " max_angle = max(max_angle, np.linalg.norm(v))\n", 1079 | " print(\"iter = \", i+1)\n", 1080 | " print(v, np.linalg.norm(v))\n", 1081 | " print(\"R:\\n\", R)\n", 1082 | " print(\"num:\\n\", numerical)\n", 1083 | " print(\"analytical:\\n\", analytical)\n", 1084 | " print(\"diff:\\n\", np.linalg.norm(numerical - analytical))\n", 1085 | "print(\"\\n ___________________________\\n num of errors: \", errors, \"/\", total)\n", 1086 | "print(\"min_angle = \", min_angle)\n", 1087 | "print(\"max_angle = \", max_angle)" 1088 | ] 1089 | }, 1090 | { 1091 | "cell_type": "markdown", 1092 | "metadata": {}, 1093 | "source": [ 1094 | "Conclusion: analytical and numerical jacobians for boxplus operation at $x=0$:\n", 1095 | "$$\\frac{\\partial (R \\boxplus x)}{\\partial x} \\big|_{x=0}$$\n", 1096 | "The two methods give the same results up to precision 1e-7." 1097 | ] 1098 | }, 1099 | { 1100 | "cell_type": "markdown", 1101 | "metadata": {}, 1102 | "source": [ 1103 | "### Checking the Jacobians of log map using chain rule:\n", 1104 | "$$ \\frac{\\partial \\log (R \\boxplus x)}{\\partial x} \\big|_{x=0} = \n", 1105 | "\\frac{\\partial log(y)}{\\partial y} \\big|_{y=R} \n", 1106 | "\\frac{\\partial (R \\boxplus x)}{\\partial x} \\big|_{x=0}$$" 1107 | ] 1108 | }, 1109 | { 1110 | "cell_type": "code", 1111 | "execution_count": null, 1112 | "metadata": {}, 1113 | "outputs": [], 1114 | "source": [ 1115 | "def test_jacobian_log_map(jacobian_log_map, v, precision=8):\n", 1116 | " R = sp.SO3.exp(v).matrix()\n", 1117 | " J_boxplus_x_at_0 = dx_R_mul_exp_x_at_0(R)\n", 1118 | " J_log_boxplus_x_at_0 = dx_log_R_mul_exp_x_at_0(R)\n", 1119 | " J_log_at_R = jacobian_log_map(R)\n", 1120 | " predicted = J_log_at_R @ J_boxplus_x_at_0\n", 1121 | " try:\n", 1122 | " np.testing.assert_array_almost_equal(J_log_boxplus_x_at_0, predicted, precision)\n", 1123 | " except AssertionError as aerr:\n", 1124 | "# print(\"Assertion Error in function: \", jacobian_log_map.__name__, aerr)\n", 1125 | "# print(\"v = \", v, \", Angle: \", np.linalg.norm(v), \"\\n\")\n", 1126 | " return 1\n", 1127 | " return 0" 1128 | ] 1129 | }, 1130 | { 1131 | "cell_type": "code", 1132 | "execution_count": null, 1133 | "metadata": { 1134 | "scrolled": true 1135 | }, 1136 | "outputs": [], 1137 | "source": [ 1138 | "v_set = [np.array([np.pi, 0, 0]), np.array([0, np.pi-1e-8, 0]), np.array([0, 0, np.pi+1e-7]), \n", 1139 | " np.array([0, 0, 0]), np.array([1, 1, 1]), np.array([0, 1, -0.7])]\n", 1140 | "\n", 1141 | "errors_baseline, errors_pi, errors_quaternion = 0, 0, 0\n", 1142 | "errors_num_baseline, errors_num_SVD_baseline, errors_num_sp_baseline = 0, 0, 0\n", 1143 | "errors_num_pi, errors_num_SVD_pi, errors_num_sp_pi = 0, 0, 0\n", 1144 | "errors_num_quaternion, errors_num_SVD_quaternion, errors_num_sp_quaternion = 0, 0 ,0\n", 1145 | "\n", 1146 | "total = 1000\n", 1147 | "min_angle = np.pi+1\n", 1148 | "max_angle = -1\n", 1149 | "np.random.seed(42)\n", 1150 | "\n", 1151 | "for i in range(total):\n", 1152 | " if i < len(v_set):\n", 1153 | " v = v_set[i]\n", 1154 | " else:\n", 1155 | " v = np.random.rand(3) * 2 - 1\n", 1156 | " norm = np.random.rand(1) * (np.pi + 1e-8)\n", 1157 | " v = v / np.linalg.norm(v) * norm\n", 1158 | " errors_baseline += test_jacobian_log_map(SO3.Dx_log_x_baseline, v)\n", 1159 | " errors_pi += test_jacobian_log_map(SO3.Dx_log_x_pi, v)\n", 1160 | " errors_quaternion += test_jacobian_log_map(SO3.Dx_log_x_quaternion, v)\n", 1161 | " \n", 1162 | " errors_num_baseline += test_jacobian_log_map(\n", 1163 | " lambda R: SO3.Dx_log_x_numerical(R, func=SO3.log_baseline, project=None), v)\n", 1164 | " errors_num_SVD_baseline += test_jacobian_log_map(\n", 1165 | " lambda R: SO3.Dx_log_x_numerical(R, func=SO3.log_baseline, project=makeRotationMatrix), v)\n", 1166 | " errors_num_sp_baseline += test_jacobian_log_map(\n", 1167 | " lambda R: SO3.Dx_log_x_numerical(R, func=SO3.log_baseline, project=sp.to_orthogonal), v)\n", 1168 | " errors_num_pi += test_jacobian_log_map(\n", 1169 | " lambda R: SO3.Dx_log_x_numerical(R, func=SO3.log_pi, project=None), v)\n", 1170 | " errors_num_SVD_pi += test_jacobian_log_map(\n", 1171 | " lambda R: SO3.Dx_log_x_numerical(R, func=SO3.log_pi, project=makeRotationMatrix), v)\n", 1172 | " errors_num_sp_pi += test_jacobian_log_map(\n", 1173 | " lambda R: SO3.Dx_log_x_numerical(R, func=SO3.log_pi, project=sp.to_orthogonal), v)\n", 1174 | " errors_num_quaternion += test_jacobian_log_map(\n", 1175 | " lambda R: SO3.Dx_log_x_numerical(R, func=SO3.log_quaternion, project=None), v)\n", 1176 | " errors_num_SVD_quaternion += test_jacobian_log_map(\n", 1177 | " lambda R: SO3.Dx_log_x_numerical(R, func=SO3.log_quaternion, project=makeRotationMatrix), v)\n", 1178 | " errors_num_sp_quaternion += test_jacobian_log_map(\n", 1179 | " lambda R: SO3.Dx_log_x_numerical(R, func=SO3.log_quaternion, project=sp.to_orthogonal), v)\n", 1180 | "\n", 1181 | "\n", 1182 | "print(\"___________________________\\n errors baseline: \", errors_baseline, \"/\", total)\n", 1183 | "print(\"errors pi: \", errors_pi, \"/\", total)\n", 1184 | "print(\"errors quaternion: \", errors_quaternion, \"/\", total)\n", 1185 | "print(\"errors num raw baseline: \", errors_num_baseline, \"/\", total)\n", 1186 | "print(\"errors num SVD baseline: \", errors_num_SVD_baseline, \"/\", total)\n", 1187 | "print(\"errors num sp baseline: \", errors_num_sp_baseline, \"/\", total)\n", 1188 | "print(\"errors num raw pi: \", errors_num_pi, \"/\", total)\n", 1189 | "print(\"errors num SVD pi: \", errors_num_SVD_pi, \"/\", total)\n", 1190 | "print(\"errors num sp pi: \", errors_num_sp_pi, \"/\", total)\n", 1191 | "print(\"errors num raw quaternion: \", errors_num_quaternion, \"/\", total)\n", 1192 | "print(\"errors num SVD quaternion: \", errors_num_SVD_quaternion, \"/\", total)\n", 1193 | "print(\"errors num sp quaternion: \", errors_num_sp_quaternion, \"/\", total)\n", 1194 | " " 1195 | ] 1196 | }, 1197 | { 1198 | "cell_type": "markdown", 1199 | "metadata": {}, 1200 | "source": [ 1201 | "### Conclusion:\n", 1202 | "\n", 1203 | "1. `Dx_log_x_quaternion` gives correct results everywhere (with high precision, < 1e-9 max abs diff).\n", 1204 | "2. `Dx_log_x_pi` gives inaccurate results for angles close to 0. Also it nullifies rows corresponding to exact zero components of vector $n(i)=0$.\n", 1205 | "3. `Dx_log_x_baseline` diverges at angle $\\pi$, but also gives inaccurate results close to $\\pi$ (already in 1e-2 proximity the inaccuracy is up to 1e-6)\n", 1206 | "4. Interestingly, the Jacobians of $\\log$ map derived from different methods give different results. However, after matrix multiplication with jacobian of boxplus at zero, the results become similar or the same.\n", 1207 | "4. Numerical solutions suffer from discretization-dependent inaccuracies (up 1e-5). They also tend to diverge at angles close to $\\pi$.\n", 1208 | "5. Projections reduce accuracy, but sometimes help to cope with divergence. \n", 1209 | "\n" 1210 | ] 1211 | }, 1212 | { 1213 | "cell_type": "markdown", 1214 | "metadata": {}, 1215 | "source": [ 1216 | "## 4. Use of Jacobians in Taylor expansion\n", 1217 | "\n", 1218 | "By Taylor expansion, for $x \\in \\mathbb{R}^3$, $||x|| \\rightarrow 0$:\n", 1219 | "\n", 1220 | "$$ \\log (R \\boxplus x) = \\log(R) + \\frac{d log(R)}{dR} \\frac{d(R \\boxplus x)}{dx} \\big|_{x=0} \\cdot {x} + O(||x||^2)$$" 1221 | ] 1222 | }, 1223 | { 1224 | "cell_type": "code", 1225 | "execution_count": null, 1226 | "metadata": {}, 1227 | "outputs": [], 1228 | "source": [ 1229 | "def test_taylor_expansion(jacobian_log_map, R, x, precision=6):\n", 1230 | " J_boxplus_x_at_0 = dx_R_mul_exp_x_at_0(R)\n", 1231 | " J_log_boxplus_x_at_0 = dx_log_R_mul_exp_x_at_0(R)\n", 1232 | " J_log_at_R = jacobian_log_map(R)\n", 1233 | " logR = SO3.log_quaternion(R)\n", 1234 | " logR_plus_x = log_R_boxplus_x(R, x)\n", 1235 | " predicted = logR + J_log_at_R @ J_boxplus_x_at_0 @ x\n", 1236 | " err = np.linalg.norm(predicted - logR_plus_x)\n", 1237 | " try:\n", 1238 | " np.testing.assert_array_almost_equal(logR_plus_x, predicted, precision)\n", 1239 | " except AssertionError as aerr:\n", 1240 | "# print(\"Assertion Error in function: \", jacobian_log_map.__name__, aerr)\n", 1241 | "# print(\"v = \", v, \", Angle: \", np.linalg.norm(v), \"\\n\")\n", 1242 | " return 1\n", 1243 | " return 0" 1244 | ] 1245 | }, 1246 | { 1247 | "cell_type": "code", 1248 | "execution_count": null, 1249 | "metadata": {}, 1250 | "outputs": [], 1251 | "source": [ 1252 | "v_set = [np.array([0.5, 1, -0.7]), np.array([np.pi, 0, 0]), np.array([0, np.pi-1e-8, 0]), \n", 1253 | " np.array([0, 0, np.pi+1e-7]), \n", 1254 | " np.array([0, 0, 0]), np.array([1, 1, 1]), \n", 1255 | " np.array([0.5, 1, -0.7]) * 1e-8, np.array([0.5, 1, -0.7]) * 1e-7,\n", 1256 | " np.array([1, 1, 1]) * 1e-6, np.array([1, 1, 1])*1e-5, np.array([1, 1, 1])*1e-4,\n", 1257 | " np.array([1, 1, 1])*1e-3,np.array([1, 1, 1])*1e-2, np.array([1, 1, 1])*1e-1]\n", 1258 | "\n", 1259 | "errors_baseline, errors_pi, errors_quaternion = 0, 0, 0\n", 1260 | "errors_num_baseline, errors_num_SVD_baseline, errors_num_sp_baseline = 0, 0, 0\n", 1261 | "errors_num_pi, errors_num_SVD_pi, errors_num_sp_pi = 0, 0, 0\n", 1262 | "errors_num_quaternion, errors_num_SVD_quaternion, errors_num_sp_quaternion = 0, 0 ,0\n", 1263 | "\n", 1264 | "total = 1000\n", 1265 | "min_angle = np.pi+1\n", 1266 | "max_angle = -1\n", 1267 | "np.random.seed(42)\n", 1268 | "precision = 8\n", 1269 | "\n", 1270 | "norm_x = 1e-3\n", 1271 | "\n", 1272 | "for i in range(total):\n", 1273 | " if i < len(v_set):\n", 1274 | " v = v_set[i]\n", 1275 | " else:\n", 1276 | " v = np.random.rand(3) * 2 - 1\n", 1277 | " norm = np.random.rand(1) * (np.pi + 1e-8)\n", 1278 | " v = v / np.linalg.norm(v) * norm\n", 1279 | " R = sp.SO3.exp(v).matrix()\n", 1280 | " x = np.random.rand(3) * 2 - 1\n", 1281 | " x = x / np.linalg.norm(x) * norm_x\n", 1282 | " \n", 1283 | " errors_baseline += test_taylor_expansion(SO3.Dx_log_x_baseline, R, x)\n", 1284 | " errors_pi += test_taylor_expansion(SO3.Dx_log_x_pi, R, x)\n", 1285 | " errors_quaternion += test_taylor_expansion(SO3.Dx_log_x_quaternion, R, x)\n", 1286 | " \n", 1287 | " errors_num_baseline += test_taylor_expansion(\n", 1288 | " lambda R: SO3.Dx_log_x_numerical(R, func=SO3.log_baseline, project=None), R, x)\n", 1289 | " errors_num_SVD_baseline += test_taylor_expansion(\n", 1290 | " lambda R: SO3.Dx_log_x_numerical(R, func=SO3.log_baseline, project=makeRotationMatrix), R, x)\n", 1291 | " errors_num_sp_baseline += test_taylor_expansion(\n", 1292 | " lambda R: SO3.Dx_log_x_numerical(R, func=SO3.log_baseline, project=sp.to_orthogonal), R, x)\n", 1293 | " errors_num_pi += test_taylor_expansion(\n", 1294 | " lambda R: SO3.Dx_log_x_numerical(R, func=SO3.log_pi, project=None), R, x)\n", 1295 | " errors_num_SVD_pi += test_taylor_expansion(\n", 1296 | " lambda R: SO3.Dx_log_x_numerical(R, func=SO3.log_pi, project=makeRotationMatrix), R, x)\n", 1297 | " errors_num_sp_pi += test_taylor_expansion(\n", 1298 | " lambda R: SO3.Dx_log_x_numerical(R, func=SO3.log_pi, project=sp.to_orthogonal), R, x)\n", 1299 | " errors_num_quaternion += test_taylor_expansion(\n", 1300 | " lambda R: SO3.Dx_log_x_numerical(R, func=SO3.log_quaternion, project=None), R, x)\n", 1301 | " errors_num_SVD_quaternion += test_taylor_expansion(\n", 1302 | " lambda R: SO3.Dx_log_x_numerical(R, func=SO3.log_quaternion, project=makeRotationMatrix), R, x)\n", 1303 | " errors_num_sp_quaternion += test_taylor_expansion(\n", 1304 | " lambda R: SO3.Dx_log_x_numerical(R, func=SO3.log_quaternion, project=sp.to_orthogonal), R, x)\n", 1305 | "\n", 1306 | "\n", 1307 | "print(\"___________________________\\n errors baseline: \", errors_baseline, \"/\", total)\n", 1308 | "print(\"errors pi: \", errors_pi, \"/\", total)\n", 1309 | "print(\"errors quaternion: \", errors_quaternion, \"/\", total)\n", 1310 | "print(\"errors num raw baseline: \", errors_num_baseline, \"/\", total)\n", 1311 | "print(\"errors num SVD baseline: \", errors_num_SVD_baseline, \"/\", total)\n", 1312 | "print(\"errors num sp baseline: \", errors_num_sp_baseline, \"/\", total)\n", 1313 | "print(\"errors num raw pi: \", errors_num_pi, \"/\", total)\n", 1314 | "print(\"errors num SVD pi: \", errors_num_SVD_pi, \"/\", total)\n", 1315 | "print(\"errors num sp pi: \", errors_num_sp_pi, \"/\", total)\n", 1316 | "print(\"errors num raw quaternion: \", errors_num_quaternion, \"/\", total)\n", 1317 | "print(\"errors num SVD quaternion: \", errors_num_SVD_quaternion, \"/\", total)\n", 1318 | "print(\"errors num sp quaternion: \", errors_num_sp_quaternion, \"/\", total)" 1319 | ] 1320 | }, 1321 | { 1322 | "cell_type": "markdown", 1323 | "metadata": {}, 1324 | "source": [ 1325 | "Conclusion:\n", 1326 | "\n", 1327 | "1. The approximation up to precision $||x||^2$ (||x|| < 1e-1) works for all vectors, except the case when $\\log(R \\boxplus x)$ changes the sign of $\\log(R)$" 1328 | ] 1329 | }, 1330 | { 1331 | "cell_type": "markdown", 1332 | "metadata": {}, 1333 | "source": [ 1334 | "References:\n", 1335 | "\n", 1336 | "* Lie theory, differential calculus on manifolds:\n", 1337 | "\n", 1338 | " [[1] A micro Lie theory for state estimation in robotics. Joan Sola, Jeremie Deray, Dinesh Atchuthan, 2020](https://arxiv.org/pdf/1812.01537.pdf)\n", 1339 | "\n", 1340 | " [[2] A Primer on the Differential Calculus of 3D Orientations. Michael Bloesch et al., 2016](https://arxiv.org/pdf/1606.05285.pdf)\n", 1341 | " \n", 1342 | " [[3] A compact formula for the derivative of a 3-D rotation in exponential coordinates. Guillermo Gallego, Anthony Yezzi, 2014](https://arxiv.org/pdf/1312.0788.pdf)" 1343 | ] 1344 | }, 1345 | { 1346 | "cell_type": "markdown", 1347 | "metadata": {}, 1348 | "source": [ 1349 | "## Use case: Ceres optimization" 1350 | ] 1351 | }, 1352 | { 1353 | "cell_type": "code", 1354 | "execution_count": null, 1355 | "metadata": {}, 1356 | "outputs": [], 1357 | "source": [ 1358 | "pyceres_location=\"/home/nurlanov/tum/lib/ceres-bin/lib/\"\n", 1359 | "\n", 1360 | "import sys\n", 1361 | "sys.path.insert(0, pyceres_location)" 1362 | ] 1363 | }, 1364 | { 1365 | "cell_type": "code", 1366 | "execution_count": null, 1367 | "metadata": {}, 1368 | "outputs": [], 1369 | "source": [ 1370 | "import PyCeres" 1371 | ] 1372 | }, 1373 | { 1374 | "cell_type": "code", 1375 | "execution_count": null, 1376 | "metadata": {}, 1377 | "outputs": [], 1378 | "source": [ 1379 | "from jax import grad, jacfwd\n", 1380 | "import jax.numpy as jnp\n", 1381 | "import math" 1382 | ] 1383 | }, 1384 | { 1385 | "cell_type": "code", 1386 | "execution_count": null, 1387 | "metadata": {}, 1388 | "outputs": [], 1389 | "source": [ 1390 | "def jax_matrix2quaternion(R): \n", 1391 | " # quaternion [q_0, q_{1:3}] = [c, vec]\n", 1392 | " q = jnp.zeros(4)\n", 1393 | " t = R[0, 0] + R[1, 1] + R[2, 2]\n", 1394 | " if t > 0:\n", 1395 | " # case 1\n", 1396 | " t = jnp.sqrt(1 + t)\n", 1397 | " q[0] = 0.5 * t\n", 1398 | " t = 0.5 / t\n", 1399 | " q[1] = (R[2, 1] - R[1, 2]) * t\n", 1400 | " q[2] = (R[0, 2] - R[2, 0]) * t\n", 1401 | " q[3] = (R[1, 0] - R[0, 1]) * t\n", 1402 | " else:\n", 1403 | " i = 0\n", 1404 | " if R[1, 1] > R[0, 0]:\n", 1405 | " i = 1\n", 1406 | " if R[2, 2] > R[i, i]:\n", 1407 | " i = 2\n", 1408 | " j = (i + 1) % 3\n", 1409 | " k = (j + 1) % 3\n", 1410 | " t = jnp.sqrt(R[i, i] - R[j, j] - R[k, k] + 1)\n", 1411 | " q[1+i] = 0.5 * t\n", 1412 | " t = 0.5 / t\n", 1413 | " q[0] = (R[k, j] - R[j, k]) * t\n", 1414 | " q[1+j] = (R[j, i] + R[i, j]) * t\n", 1415 | " q[1+k] = (R[k, i] + R[i, k]) * t\n", 1416 | " return q\n", 1417 | " \n", 1418 | "def jax_quaternion2vec(q):\n", 1419 | " squared_n = q[1]*q[1] + q[2]*q[2] + q[3]*q[3]\n", 1420 | " w = q[0]\n", 1421 | " theta = 0\n", 1422 | " if squared_n < np.finfo(float).eps * np.finfo(float).eps:\n", 1423 | " squared_w = w*w\n", 1424 | " two_atan_nbyw_by_n = 2 / w - 2.0 / 3 * squared_n / (w * squared_w) \n", 1425 | " theta = 2 * squared_n / w\n", 1426 | " else:\n", 1427 | " n = jnp.sqrt(squared_n)\n", 1428 | " if (abs(w) < np.finfo(float).eps):\n", 1429 | " if w >= 0:\n", 1430 | " two_atan_nbyw_by_n = np.pi / n\n", 1431 | " else:\n", 1432 | " two_atan_nbyw_by_n = -np.pi / n\n", 1433 | " else:\n", 1434 | " two_atan_nbyw_by_n = 2 * jnp.arctan(n / w) / n;\n", 1435 | " theta = two_atan_nbyw_by_n * n\n", 1436 | "\n", 1437 | " tangent = two_atan_nbyw_by_n * q[1:]\n", 1438 | " return tangent\n", 1439 | " \n", 1440 | "def jax_log_quaternion(R): \n", 1441 | " q = jax_matrix2quaternion(R)\n", 1442 | " v = jax_quaternion2vec(q)\n", 1443 | " return v\n", 1444 | " \n", 1445 | "def jax_exp(v):\n", 1446 | " norm_sq = sum(v**2)\n", 1447 | " v_hat = jnp.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])\n", 1448 | " v_hat_sq = v_hat @ v_hat\n", 1449 | " if norm_sq < np.finfo(float).eps * np.finfo(float).eps:\n", 1450 | " return np.eye(3) + v_hat + v_hat_sq * 0.5\n", 1451 | " else:\n", 1452 | " norm = jnp.sqrt(norm_sq)\n", 1453 | " return np.eye(3) + jnp.sin(norm) / norm * v_hat + (1 - jnp.cos(norm)) / norm_sq * v_hat_sq\n", 1454 | " \n", 1455 | "def jax_log_baseline(R): \n", 1456 | " trR = R[0, 0] + R[1, 1] + R[2, 2]\n", 1457 | " cos_theta = max(min(0.5 * (trR - 1), 1), -1)\n", 1458 | " sin_theta = 0.5 * jnp.sqrt(max(0, (3 - trR) * (1 + trR)))\n", 1459 | " theta = jnp.arctan2(sin_theta, cos_theta)\n", 1460 | " R_minus_R_T_vee = jnp.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]])\n", 1461 | "\n", 1462 | " if abs(3 - trR) < 1e-8:\n", 1463 | " # return log map at Theta = 0\n", 1464 | " c = 0.5 * (1 + theta*theta / 6 + 7 / 360 * (theta**4))\n", 1465 | " return c * R_minus_R_T_vee\n", 1466 | "\n", 1467 | " # it diverges around theta=pi\n", 1468 | " v = theta / (2 * sin_theta) * R_minus_R_T_vee\n", 1469 | " return v" 1470 | ] 1471 | }, 1472 | { 1473 | "cell_type": "code", 1474 | "execution_count": null, 1475 | "metadata": {}, 1476 | "outputs": [], 1477 | "source": [ 1478 | "def jax_residual_calc(R_inv_mat, x):\n", 1479 | " x_mat = jax_exp(x)\n", 1480 | " product = R_inv_mat @ x_mat\n", 1481 | " residual = jax_log_baseline(product)\n", 1482 | " \n", 1483 | "# x_mat = sp.SO3.exp(x).matrix()\n", 1484 | "# product = R_inv_mat @ x_mat\n", 1485 | "# residual = sp.SO3.log(product)\n", 1486 | " return residual\n", 1487 | "\n", 1488 | "def residual_calc(R_inv_mat, x):\n", 1489 | " x_mat = SO3.exp(x)\n", 1490 | " product = R_inv_mat @ x_mat\n", 1491 | " residual = SO3.log_quaternion(product)\n", 1492 | " return residual\n", 1493 | "\n", 1494 | "def jacobian_calc(R_inv_mat, x):\n", 1495 | " x_mat = SO3.exp(x)\n", 1496 | " product = R_inv_mat @ x_mat\n", 1497 | " jacobian = SO3.Dx_log_x_quaternion(product) @ dx_R_mul_exp_x(R_inv_mat, x)\n", 1498 | " return jacobian\n", 1499 | "\n", 1500 | "\n", 1501 | "class BoxMinusAutoDiff(PyCeres.CostFunction):\n", 1502 | " def __init__(self, R_inv):\n", 1503 | " super().__init__()\n", 1504 | " self.set_num_residuals(3)\n", 1505 | " self.set_parameter_block_sizes([3])\n", 1506 | " self.R_inv = R_inv\n", 1507 | "\n", 1508 | " def Evaluate(self, parameters, residuals, jacobians):\n", 1509 | " x = parameters[0]\n", 1510 | " t = residual_calc(self.R_inv, x)\n", 1511 | " for i in range(len(residuals)):\n", 1512 | " residuals[i] = t[i]\n", 1513 | "\n", 1514 | " if (jacobians!=None):\n", 1515 | " J = jacfwd(lambda x: jax_residual_calc(self.R_inv, x))(x)\n", 1516 | " print(\"J auto:\\n\", J)\n", 1517 | " J_a = jacobian_calc(self.R_inv, x)\n", 1518 | " print(\"J_analytical:\\n\", J_a)\n", 1519 | " tt = J_a.reshape(-1, order=\"C\")\n", 1520 | "# tt = J.reshape(-1, order=\"C\")\n", 1521 | " for i in range(len(jacobians[0])):\n", 1522 | " jacobians[0][i] = tt[i]\n", 1523 | "# jacobians[0] = grad(lambda x: residual_calc(self.R_inv, x))(x)\n", 1524 | "\n", 1525 | " return True" 1526 | ] 1527 | }, 1528 | { 1529 | "cell_type": "code", 1530 | "execution_count": null, 1531 | "metadata": {}, 1532 | "outputs": [], 1533 | "source": [ 1534 | "# The variable to solve for with its initial value.\n", 1535 | "initial_x = [0.1, 0.05, -0.7]\n", 1536 | "x = np.array(initial_x) # Requires the variable to be in a numpy array\n", 1537 | "v = np.random.rand(3) * 2 - 1\n", 1538 | "v = v / np.linalg.norm(v) * (np.pi)\n", 1539 | "R = sp.SO3.exp(x) * sp.SO3.exp(v)\n", 1540 | "R_inv_mat = R.inverse().matrix()\n", 1541 | "R_mat = R.matrix()\n", 1542 | "\n", 1543 | "# Here we create the problem as in normal Ceres\n", 1544 | "problem=PyCeres.Problem()\n", 1545 | "\n", 1546 | "cost_function=BoxMinusAutoDiff(R_inv_mat)\n", 1547 | "\n", 1548 | "# Add the costfunction and the parameter numpy array to the problem\n", 1549 | "problem.AddResidualBlock(cost_function,None,x) \n", 1550 | "\n", 1551 | "# Setup the solver options as in normal ceres\n", 1552 | "options=PyCeres.SolverOptions()\n", 1553 | "\n", 1554 | "# Ceres enums live in PyCeres and require the enum Type\n", 1555 | "options.linear_solver_type=PyCeres.LinearSolverType.DENSE_QR\n", 1556 | "options.minimizer_progress_to_stdout=True\n", 1557 | "summary=PyCeres.Summary()\n", 1558 | "\n", 1559 | "# Solve as you would normally\n", 1560 | "PyCeres.Solve(options,problem,summary)\n", 1561 | "print(summary.BriefReport() + \" \\n\")\n", 1562 | "print( \"x : \", initial_x, \" -> \", x, \"\\n\")\n", 1563 | "\n", 1564 | "print(\"Target: \", SO3.log_quaternion(R.matrix()))\n", 1565 | "print(\"Optimized: \", x)\n", 1566 | "norm_x = np.linalg.norm(x)\n", 1567 | "print(\"norm optimized = \", norm_x)\n", 1568 | "proj_x = SO3.log_quaternion(SO3.exp(x))\n", 1569 | "print(\"Optimized projected back: \", proj_x)\n", 1570 | "\n", 1571 | "err = np.linalg.norm(proj_x - SO3.log_quaternion(R.matrix()))\n", 1572 | "new_x = x / norm_x * (norm_x - 2*np.pi)\n", 1573 | "\n", 1574 | "print(\"Optimized energy: \", SO3.log_quaternion(R_inv_mat @ SO3.exp(x)))\n", 1575 | "print(\"Difference: \", err)" 1576 | ] 1577 | }, 1578 | { 1579 | "cell_type": "code", 1580 | "execution_count": null, 1581 | "metadata": {}, 1582 | "outputs": [], 1583 | "source": [] 1584 | } 1585 | ], 1586 | "metadata": { 1587 | "kernelspec": { 1588 | "display_name": "Python 3", 1589 | "language": "python", 1590 | "name": "python3" 1591 | }, 1592 | "language_info": { 1593 | "codemirror_mode": { 1594 | "name": "ipython", 1595 | "version": 3 1596 | }, 1597 | "file_extension": ".py", 1598 | "mimetype": "text/x-python", 1599 | "name": "python", 1600 | "nbconvert_exporter": "python", 1601 | "pygments_lexer": "ipython3", 1602 | "version": "3.6.9" 1603 | } 1604 | }, 1605 | "nbformat": 4, 1606 | "nbformat_minor": 4 1607 | } 1608 | --------------------------------------------------------------------------------