From d9a00ded231ea074cd64bdf5cc87690ea5dc0f0a Mon Sep 17 00:00:00 2001 From: Calvin Date: Tue, 25 Jan 2022 16:39:05 -0600 Subject: [PATCH 01/46] Modified the scaling values for plotting uncertainty ellipses to properly display 3 standard deviations for both 2D and 3D cases. --- python/gtsam/utils/plot.py | 42 ++++++++++++++++++++++++-------------- 1 file changed, 27 insertions(+), 15 deletions(-) diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index a632b852a..8060de2fb 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -75,8 +75,9 @@ def plot_covariance_ellipse_3d(axes, Plots a Gaussian as an uncertainty ellipse Based on Maybeck Vol 1, page 366 - k=2.296 corresponds to 1 std, 68.26% of all probability - k=11.82 corresponds to 3 std, 99.74% of all probability + For the 3D case: + k = 3.527 corresponds to 1 std, 68.26% of all probability + k = 14.157 corresponds to 3 std, 99.74% of all probability Args: axes (matplotlib.axes.Axes): Matplotlib axes. @@ -87,7 +88,7 @@ def plot_covariance_ellipse_3d(axes, n: Defines the granularity of the ellipse. Higher values indicate finer ellipses. alpha: Transparency value for the plotted surface in the range [0, 1]. """ - k = 11.82 + k = np.sqrt(14.157) U, S, _ = np.linalg.svd(P) radii = k * np.sqrt(S) @@ -113,7 +114,14 @@ def plot_point2_on_axes(axes, linespec: str, P: Optional[np.ndarray] = None) -> None: """ - Plot a 2D point on given axis `axes` with given `linespec`. + Plot a 2D point and its corresponding uncertainty ellipse on given axis + `axes` with given `linespec`. + + Based on Stochastic Models, Estimation, and Control Vol 1 by Maybeck, + page 366 + For the 2D case: + k = 2.296 corresponds to 1 std, 68.26% of all probability + k = 11.820 corresponds to 3 std, 99.74% of all probability Args: axes (matplotlib.axes.Axes): Matplotlib axes. @@ -125,16 +133,15 @@ def plot_point2_on_axes(axes, if P is not None: w, v = np.linalg.eig(P) - # "Sigma" value for drawing the uncertainty ellipse. 5 sigma corresponds - # to a 99.9999% confidence, i.e. assuming the estimation has been - # computed properly, there is a 99.999% chance that the true position - # of the point will lie within the uncertainty ellipse. - k = 5.0 + # Scaling value for the uncertainty ellipse, we multiply by 2 because + # matplotlib takes the diameter and not the radius of the major and + # minor axes of the ellipse. + k = 2*np.sqrt(11.820) angle = np.arctan2(v[1, 0], v[0, 0]) e1 = patches.Ellipse(point, - np.sqrt(w[0] * k), - np.sqrt(w[1] * k), + np.sqrt(w[0]) * k, + np.sqrt(w[1]) * k, np.rad2deg(angle), fill=False) axes.add_patch(e1) @@ -178,6 +185,12 @@ def plot_pose2_on_axes(axes, """ Plot a 2D pose on given axis `axes` with given `axis_length`. + Based on Stochastic Models, Estimation, and Control Vol 1 by Maybeck, + page 366 + For the 2D case: + k = 2.296 corresponds to 1 std, 68.26% of all probability + k = 11.820 corresponds to 3 std, 99.74% of all probability + Args: axes (matplotlib.axes.Axes): Matplotlib axes. pose: The pose to be plotted. @@ -205,13 +218,12 @@ def plot_pose2_on_axes(axes, w, v = np.linalg.eig(gPp) - # k = 2.296 - k = 5.0 + k = 2*np.sqrt(11.820) angle = np.arctan2(v[1, 0], v[0, 0]) e1 = patches.Ellipse(origin, - np.sqrt(w[0] * k), - np.sqrt(w[1] * k), + np.sqrt(w[0]) * k, + np.sqrt(w[1]) * k, np.rad2deg(angle), fill=False) axes.add_patch(e1) From 1b817760dd10d2d08579f4021968eeda262f34c1 Mon Sep 17 00:00:00 2001 From: Calvin Date: Fri, 28 Jan 2022 13:31:11 -0600 Subject: [PATCH 02/46] Changed all instances of the Sigma value, k, to 5 for plotting the covariance ellipse. --- python/gtsam/utils/plot.py | 30 ++++++++++++++++++++---------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index 8060de2fb..820cefb7c 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -79,6 +79,8 @@ def plot_covariance_ellipse_3d(axes, k = 3.527 corresponds to 1 std, 68.26% of all probability k = 14.157 corresponds to 3 std, 99.74% of all probability + We choose k = 5 which corresponds to 99.99846% of all probability in 3D + Args: axes (matplotlib.axes.Axes): Matplotlib axes. origin: The origin in the world frame. @@ -88,7 +90,8 @@ def plot_covariance_ellipse_3d(axes, n: Defines the granularity of the ellipse. Higher values indicate finer ellipses. alpha: Transparency value for the plotted surface in the range [0, 1]. """ - k = np.sqrt(14.157) + # Sigma value corresponding to the covariance ellipse + k = 5 U, S, _ = np.linalg.svd(P) radii = k * np.sqrt(S) @@ -123,6 +126,8 @@ def plot_point2_on_axes(axes, k = 2.296 corresponds to 1 std, 68.26% of all probability k = 11.820 corresponds to 3 std, 99.74% of all probability + We choose k = 5 which corresponds to 99.99963% of all probability for 2D. + Args: axes (matplotlib.axes.Axes): Matplotlib axes. point: The point to be plotted. @@ -133,15 +138,15 @@ def plot_point2_on_axes(axes, if P is not None: w, v = np.linalg.eig(P) - # Scaling value for the uncertainty ellipse, we multiply by 2 because - # matplotlib takes the diameter and not the radius of the major and - # minor axes of the ellipse. - k = 2*np.sqrt(11.820) + # Sigma value corresponding to the covariance ellipse + k = 5 angle = np.arctan2(v[1, 0], v[0, 0]) + # We multiply k by 2 since k corresponds to the radius but Ellipse uses + # the diameter. e1 = patches.Ellipse(point, - np.sqrt(w[0]) * k, - np.sqrt(w[1]) * k, + np.sqrt(w[0]) * 2 * k, + np.sqrt(w[1]) * 2 * k, np.rad2deg(angle), fill=False) axes.add_patch(e1) @@ -191,6 +196,8 @@ def plot_pose2_on_axes(axes, k = 2.296 corresponds to 1 std, 68.26% of all probability k = 11.820 corresponds to 3 std, 99.74% of all probability + We choose k = 5 which corresponds to 99.99963% of all probability for 2D. + Args: axes (matplotlib.axes.Axes): Matplotlib axes. pose: The pose to be plotted. @@ -218,12 +225,15 @@ def plot_pose2_on_axes(axes, w, v = np.linalg.eig(gPp) - k = 2*np.sqrt(11.820) + # Sigma value corresponding to the covariance ellipse + k = 5 angle = np.arctan2(v[1, 0], v[0, 0]) + # We multiply k by 2 since k corresponds to the radius but Ellipse uses + # the diameter. e1 = patches.Ellipse(origin, - np.sqrt(w[0]) * k, - np.sqrt(w[1]) * k, + np.sqrt(w[0]) * 2 * k, + np.sqrt(w[1]) * 2 * k, np.rad2deg(angle), fill=False) axes.add_patch(e1) From e524e2806b6a341053a7ab4a425a5fe0a4afe72e Mon Sep 17 00:00:00 2001 From: Calvin Date: Fri, 28 Jan 2022 14:16:30 -0600 Subject: [PATCH 03/46] Updated comments to reflect standard deviations instead of variances in the discussions regarding the values for k. --- python/gtsam/utils/plot.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index 820cefb7c..f409968a8 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -76,8 +76,8 @@ def plot_covariance_ellipse_3d(axes, Based on Maybeck Vol 1, page 366 For the 3D case: - k = 3.527 corresponds to 1 std, 68.26% of all probability - k = 14.157 corresponds to 3 std, 99.74% of all probability + k = 1.878 corresponds to 1 std, 68.26% of all probability + k = 3.763 corresponds to 3 std, 99.74% of all probability We choose k = 5 which corresponds to 99.99846% of all probability in 3D @@ -123,8 +123,8 @@ def plot_point2_on_axes(axes, Based on Stochastic Models, Estimation, and Control Vol 1 by Maybeck, page 366 For the 2D case: - k = 2.296 corresponds to 1 std, 68.26% of all probability - k = 11.820 corresponds to 3 std, 99.74% of all probability + k = 1.515 corresponds to 1 std, 68.26% of all probability + k = 3.438 corresponds to 3 std, 99.74% of all probability We choose k = 5 which corresponds to 99.99963% of all probability for 2D. @@ -193,8 +193,8 @@ def plot_pose2_on_axes(axes, Based on Stochastic Models, Estimation, and Control Vol 1 by Maybeck, page 366 For the 2D case: - k = 2.296 corresponds to 1 std, 68.26% of all probability - k = 11.820 corresponds to 3 std, 99.74% of all probability + k = 1.515 corresponds to 1 std, 68.26% of all probability + k = 3.438 corresponds to 3 std, 99.74% of all probability We choose k = 5 which corresponds to 99.99963% of all probability for 2D. From bf8137b0c404997ba6d1678e0b733e3347b0f007 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 8 Mar 2022 09:48:05 -0500 Subject: [PATCH 04/46] refactor python NonlinearOptimizerTest --- python/gtsam/tests/test_NonlinearOptimizer.py | 78 +++++++++---------- 1 file changed, 38 insertions(+), 40 deletions(-) diff --git a/python/gtsam/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py index e2561ae52..2158e10bb 100644 --- a/python/gtsam/tests/test_NonlinearOptimizer.py +++ b/python/gtsam/tests/test_NonlinearOptimizer.py @@ -15,12 +15,10 @@ from __future__ import print_function import unittest import gtsam -from gtsam import (DoglegOptimizer, DoglegParams, - DummyPreconditionerParameters, GaussNewtonOptimizer, - GaussNewtonParams, GncLMParams, GncLMOptimizer, - LevenbergMarquardtOptimizer, LevenbergMarquardtParams, - NonlinearFactorGraph, Ordering, - PCGSolverParameters, Point2, PriorFactorPoint2, Values) +from gtsam import (DoglegOptimizer, DoglegParams, DummyPreconditionerParameters, + GaussNewtonOptimizer, GaussNewtonParams, GncLMParams, GncLMOptimizer, + LevenbergMarquardtOptimizer, LevenbergMarquardtParams, NonlinearFactorGraph, + Ordering, PCGSolverParameters, Point2, PriorFactorPoint2, Values) from gtsam.utils.test_case import GtsamTestCase KEY1 = 1 @@ -28,62 +26,62 @@ KEY2 = 2 class TestScenario(GtsamTestCase): - def test_optimize(self): - """Do trivial test with three optimizer variants.""" - fg = NonlinearFactorGraph() + """Do trivial test with three optimizer variants.""" + + def setUp(self): + """Set up the optimization problem and ordering""" + # create graph + self.fg = NonlinearFactorGraph() model = gtsam.noiseModel.Unit.Create(2) - fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model)) + self.fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model)) # test error at minimum xstar = Point2(0, 0) - optimal_values = Values() - optimal_values.insert(KEY1, xstar) - self.assertEqual(0.0, fg.error(optimal_values), 0.0) + self.optimal_values = Values() + self.optimal_values.insert(KEY1, xstar) + self.assertEqual(0.0, self.fg.error(self.optimal_values), 0.0) # test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = x0 = Point2(3, 3) - initial_values = Values() - initial_values.insert(KEY1, x0) - self.assertEqual(9.0, fg.error(initial_values), 1e-3) + self.initial_values = Values() + self.initial_values.insert(KEY1, x0) + self.assertEqual(9.0, self.fg.error(self.initial_values), 1e-3) # optimize parameters - ordering = Ordering() - ordering.push_back(KEY1) + self.ordering = Ordering() + self.ordering.push_back(KEY1) - # Gauss-Newton + def test_gauss_newton(self): gnParams = GaussNewtonParams() - gnParams.setOrdering(ordering) - actual1 = GaussNewtonOptimizer(fg, initial_values, gnParams).optimize() - self.assertAlmostEqual(0, fg.error(actual1)) + gnParams.setOrdering(self.ordering) + actual = GaussNewtonOptimizer(self.fg, self.initial_values, gnParams).optimize() + self.assertAlmostEqual(0, self.fg.error(actual)) - # Levenberg-Marquardt + def test_levenberg_marquardt(self): lmParams = LevenbergMarquardtParams.CeresDefaults() - lmParams.setOrdering(ordering) - actual2 = LevenbergMarquardtOptimizer( - fg, initial_values, lmParams).optimize() - self.assertAlmostEqual(0, fg.error(actual2)) + lmParams.setOrdering(self.ordering) + actual = LevenbergMarquardtOptimizer(self.fg, self.initial_values, lmParams).optimize() + self.assertAlmostEqual(0, self.fg.error(actual)) - # Levenberg-Marquardt + def test_levenberg_marquardt_pcg(self): lmParams = LevenbergMarquardtParams.CeresDefaults() lmParams.setLinearSolverType("ITERATIVE") cgParams = PCGSolverParameters() cgParams.setPreconditionerParams(DummyPreconditionerParameters()) lmParams.setIterativeParams(cgParams) - actual2 = LevenbergMarquardtOptimizer( - fg, initial_values, lmParams).optimize() - self.assertAlmostEqual(0, fg.error(actual2)) + actual = LevenbergMarquardtOptimizer(self.fg, self.initial_values, lmParams).optimize() + self.assertAlmostEqual(0, self.fg.error(actual)) - # Dogleg + def test_dogleg(self): dlParams = DoglegParams() - dlParams.setOrdering(ordering) - actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize() - self.assertAlmostEqual(0, fg.error(actual3)) - - # Graduated Non-Convexity (GNC) + dlParams.setOrdering(self.ordering) + actual = DoglegOptimizer(self.fg, self.initial_values, dlParams).optimize() + self.assertAlmostEqual(0, self.fg.error(actual)) + + def test_graduated_non_convexity(self): gncParams = GncLMParams() - actual4 = GncLMOptimizer(fg, initial_values, gncParams).optimize() - self.assertAlmostEqual(0, fg.error(actual4)) - + actual = GncLMOptimizer(self.fg, self.initial_values, gncParams).optimize() + self.assertAlmostEqual(0, self.fg.error(actual)) if __name__ == "__main__": From 5f601f76d5eb22434f8681155f5d492fba840401 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 8 Mar 2022 09:57:11 -0500 Subject: [PATCH 05/46] add iteration hook to NonlinearOptimizerParams --- gtsam/nonlinear/nonlinear.i | 2 ++ python/gtsam/tests/test_NonlinearOptimizer.py | 20 +++++++++++++++++++ 2 files changed, 22 insertions(+) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index da5e8b29c..7d2911374 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -403,6 +403,8 @@ virtual class NonlinearOptimizerParams { bool isSequential() const; bool isCholmod() const; bool isIterative() const; + + gtsam::NonlinearOptimizerParams::IterationHook iterationHook; }; bool checkConvergence(double relativeErrorTreshold, diff --git a/python/gtsam/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py index 2158e10bb..37afd9e1c 100644 --- a/python/gtsam/tests/test_NonlinearOptimizer.py +++ b/python/gtsam/tests/test_NonlinearOptimizer.py @@ -83,6 +83,26 @@ class TestScenario(GtsamTestCase): actual = GncLMOptimizer(self.fg, self.initial_values, gncParams).optimize() self.assertAlmostEqual(0, self.fg.error(actual)) + def test_iteration_hook(self): + # set up iteration hook to track some testable values + iteration_count = 0 + final_error = 0 + final_values = None + def iteration_hook(iter, error_before, error_after): + nonlocal iteration_count, final_error, final_values + iteration_count = iter + final_error = error_after + final_values = optimizer.values() + # optimize + params = LevenbergMarquardtParams.CeresDefaults() + params.setOrdering(self.ordering) + params.iterationHook = iteration_hook + optimizer = LevenbergMarquardtOptimizer(self.fg, self.initial_values, params) + actual = optimizer.optimize() + self.assertAlmostEqual(0, self.fg.error(actual)) + self.gtsamAssertEquals(final_values, actual) + self.assertEqual(self.fg.error(actual), final_error) + self.assertEqual(optimizer.iterations(), iteration_count) if __name__ == "__main__": unittest.main() From ddca736c7bd3f2cf2f0eaae9925fbb4a3d6a7cc7 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 8 Mar 2022 10:13:48 -0500 Subject: [PATCH 06/46] add comment about matlab not having iteration hook --- gtsam/nonlinear/nonlinear.i | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 7d2911374..e227de37c 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -404,6 +404,7 @@ virtual class NonlinearOptimizerParams { bool isCholmod() const; bool isIterative() const; + // This only applies to python since matlab does not have lambda machinery. gtsam::NonlinearOptimizerParams::IterationHook iterationHook; }; From d7fbaaab639416fc08d881c58751dd8022f02c62 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Mar 2022 11:24:45 -0400 Subject: [PATCH 07/46] Bump version --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b86598663..cfb251663 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ endif() set (GTSAM_VERSION_MAJOR 4) set (GTSAM_VERSION_MINOR 2) set (GTSAM_VERSION_PATCH 0) -set (GTSAM_PRERELEASE_VERSION "a5") +set (GTSAM_PRERELEASE_VERSION "a6") math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") if (${GTSAM_VERSION_PATCH} EQUAL 0) From 938d4093956335351a5f841bb14d70557876dd25 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 13 Apr 2022 21:34:54 -0400 Subject: [PATCH 08/46] Notebook --- .gitignore | 1 + python/gtsam/notebooks/ellipses.ipynb | 136 ++++++++++++++++++++++++++ 2 files changed, 137 insertions(+) create mode 100644 python/gtsam/notebooks/ellipses.ipynb diff --git a/.gitignore b/.gitignore index 0e34eed34..e3f7613fe 100644 --- a/.gitignore +++ b/.gitignore @@ -18,3 +18,4 @@ CMakeLists.txt.user* xcode/ /Dockerfile +/python/gtsam/notebooks/.ipynb_checkpoints/ellipses-checkpoint.ipynb diff --git a/python/gtsam/notebooks/ellipses.ipynb b/python/gtsam/notebooks/ellipses.ipynb new file mode 100644 index 000000000..d1ce9a015 --- /dev/null +++ b/python/gtsam/notebooks/ellipses.ipynb @@ -0,0 +1,136 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Ellipse Scaling\n", + "\n", + "The code to calculate the percentages included in ellipses with various values of \"k\" in `plot.py`.\n", + "\n", + "Thanks to @senselessDev, January 26, for providing the code in [PR #1067](https://github.com/borglab/gtsam/pull/1067)." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "import scipy\n", + "import scipy.stats" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [], + "source": [ + "def pct_to_sigma(pct, dof):\n", + " return np.sqrt(scipy.stats.chi2.ppf(pct / 100., df=dof))\n", + "\n", + "def sigma_to_pct(sigma, dof):\n", + " return scipy.stats.chi2.cdf(sigma**2, df=dof) * 100." + ] + }, + { + "cell_type": "code", + "execution_count": 23, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "0D\t 1 \t 2 \t 3 \t 4 \t 5 \n", + "1D\t68.26895%\t95.44997%\t99.73002%\t99.99367%\t99.99994%\n", + "2D\t39.34693%\t86.46647%\t98.88910%\t99.96645%\t99.99963%\n", + "3D\t19.87480%\t73.85359%\t97.07091%\t99.88660%\t99.99846%\n" + ] + } + ], + "source": [ + "for dim in range(0, 4):\n", + " print(\"{}D\".format(dim), end=\"\")\n", + " for sigma in range(1, 6):\n", + " if dim == 0: print(\"\\t {} \".format(sigma), end=\"\")\n", + " else: print(\"\\t{:.5f}%\".format(sigma_to_pct(sigma, dim)), end=\"\")\n", + " print()" + ] + }, + { + "cell_type": "code", + "execution_count": 29, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "1D\n", + "\n", + "sigma=1.0 -> 68.26895%\n", + "sigma=2.0 -> 95.44997%\n", + "sigma=2.5 -> 98.75807%\n", + "sigma=5.0 -> 99.99994%\n", + "\n", + "2D\n", + "\n", + "sigma=1.0 -> 39.34693%\n", + "sigma=2.0 -> 86.46647%\n", + "sigma=2.5 -> 95.60631%\n", + "sigma=5.0 -> 99.99963%\n", + "\n", + "3D\n", + "\n", + "sigma=1.0 -> 19.87480%\n", + "sigma=2.0 -> 73.85359%\n", + "sigma=2.5 -> 89.99392%\n", + "sigma=5.0 -> 99.99846%\n", + "\n" + ] + } + ], + "source": [ + "for dim in range(1, 4):\n", + " print(\"{}D\\n\".format(dim))\n", + " for sigma in [1, 2, 2.5, 5]:\n", + " print(\"sigma={:.1f} -> {:.5f}%\".format(sigma, sigma_to_pct(sigma, dim)), end=\"\")\n", + " print()\n", + " print()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "interpreter": { + "hash": "341996cd3f3db7b5e0d1eaea072c5502d80452314e72e6b77c40445f6e9ba101" + }, + "kernelspec": { + "display_name": "Python 3 (ipykernel)", + "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.10.4" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} From 71aa20ff33ed08c9fbd708d771005f810b370257 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 19 Apr 2022 16:03:38 -0400 Subject: [PATCH 09/46] Update `logging_optimizer.gtsam_optimize` to use NonlinearOptimizerParams::iterationHook --- python/gtsam/tests/test_logging_optimizer.py | 6 +++++- python/gtsam/utils/logging_optimizer.py | 15 ++++++--------- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/python/gtsam/tests/test_logging_optimizer.py b/python/gtsam/tests/test_logging_optimizer.py index 47eb32e7b..4ec782635 100644 --- a/python/gtsam/tests/test_logging_optimizer.py +++ b/python/gtsam/tests/test_logging_optimizer.py @@ -63,12 +63,14 @@ class TestOptimizeComet(GtsamTestCase): def hook(_, error): print(error) - # Only thing we require from optimizer is an iterate method + # Wrapper function sets the hook and calls optimizer.optimize() for us. gtsam_optimize(self.optimizer, self.params, hook) # Check that optimizing yields the identity. actual = self.optimizer.values() self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6) + self.assertEqual(self.capturedOutput.getvalue(), + "0.020000000000000004\n0.010000000000000005\n0.010000000000000004\n") def test_lm_simple_printing(self): """Make sure we are properly terminating LM""" @@ -79,6 +81,8 @@ class TestOptimizeComet(GtsamTestCase): actual = self.lmoptimizer.values() self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6) + self.assertEqual(self.capturedOutput.getvalue(), + "0.020000000000000004\n0.010000000000249996\n0.009999999999999998\n") @unittest.skip("Not a test we want run every time, as needs comet.ml account") def test_comet(self): diff --git a/python/gtsam/utils/logging_optimizer.py b/python/gtsam/utils/logging_optimizer.py index 3d9175951..1e55ce990 100644 --- a/python/gtsam/utils/logging_optimizer.py +++ b/python/gtsam/utils/logging_optimizer.py @@ -21,7 +21,8 @@ def optimize(optimizer, check_convergence, hook): current_error = optimizer.error() hook(optimizer, current_error) - # Iterative loop + # Iterative loop. Cannot use `params.iterationHook` because we don't have access to params + # (backwards compatibility issue). while True: # Do next iteration optimizer.iterate() @@ -35,7 +36,7 @@ def optimize(optimizer, check_convergence, hook): def gtsam_optimize(optimizer, params, hook): - """ Given an optimizer and params, iterate until convergence. + """ Given an optimizer and its params, iterate until convergence. After each iteration, hook(optimizer) is called. After the function, use values and errors to get the result. Arguments: @@ -43,10 +44,6 @@ def gtsam_optimize(optimizer, params {NonlinearOptimizarParams} -- Nonlinear optimizer parameters hook -- hook function to record the error """ - def check_convergence(optimizer, current_error, new_error): - return (optimizer.iterations() >= params.getMaxIterations()) or ( - gtsam.checkConvergence(params.getRelativeErrorTol(), params.getAbsoluteErrorTol(), params.getErrorTol(), - current_error, new_error)) or ( - isinstance(optimizer, gtsam.LevenbergMarquardtOptimizer) and optimizer.lambda_() > params.getlambdaUpperBound()) - optimize(optimizer, check_convergence, hook) - return optimizer.values() + hook(optimizer, optimizer.error()) # call once at start (backwards compatibility) + params.iterationHook = lambda iteration, error_before, error_after: hook(optimizer, error_after) + return optimizer.optimize() From 5cfc87010d8496f39c194647632e4c27e28e4f89 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 19 Apr 2022 16:10:36 -0400 Subject: [PATCH 10/46] Add comment to docstring of `gtsam_optimize` --- python/gtsam/utils/logging_optimizer.py | 1 + 1 file changed, 1 insertion(+) diff --git a/python/gtsam/utils/logging_optimizer.py b/python/gtsam/utils/logging_optimizer.py index 1e55ce990..d2cd9b9f6 100644 --- a/python/gtsam/utils/logging_optimizer.py +++ b/python/gtsam/utils/logging_optimizer.py @@ -39,6 +39,7 @@ def gtsam_optimize(optimizer, """ Given an optimizer and its params, iterate until convergence. After each iteration, hook(optimizer) is called. After the function, use values and errors to get the result. + Optimizer must have been created with the same params as the one passed into this function! Arguments: optimizer {NonlinearOptimizer} -- Nonlinear optimizer params {NonlinearOptimizarParams} -- Nonlinear optimizer parameters From 2a17bb1715b0d74fe6cbc6f87b807cf4af6cb881 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 20 Apr 2022 15:40:09 -0400 Subject: [PATCH 11/46] Revert "Add comment to docstring of `gtsam_optimize`" This reverts commit 5cfc87010d8496f39c194647632e4c27e28e4f89. --- python/gtsam/utils/logging_optimizer.py | 1 - 1 file changed, 1 deletion(-) diff --git a/python/gtsam/utils/logging_optimizer.py b/python/gtsam/utils/logging_optimizer.py index d2cd9b9f6..1e55ce990 100644 --- a/python/gtsam/utils/logging_optimizer.py +++ b/python/gtsam/utils/logging_optimizer.py @@ -39,7 +39,6 @@ def gtsam_optimize(optimizer, """ Given an optimizer and its params, iterate until convergence. After each iteration, hook(optimizer) is called. After the function, use values and errors to get the result. - Optimizer must have been created with the same params as the one passed into this function! Arguments: optimizer {NonlinearOptimizer} -- Nonlinear optimizer params {NonlinearOptimizarParams} -- Nonlinear optimizer parameters From 61eef0639abcb89b632412bda1a1e1906fecf474 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 20 Apr 2022 15:44:17 -0400 Subject: [PATCH 12/46] Partially Revert "Update `logging_optimizer.gtsam_optimize` to use NonlinearOptimizerParams::iterationHook" This reverts commit 71aa20ff33ed08c9fbd708d771005f810b370257. --- python/gtsam/utils/logging_optimizer.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/python/gtsam/utils/logging_optimizer.py b/python/gtsam/utils/logging_optimizer.py index 1e55ce990..bf727f997 100644 --- a/python/gtsam/utils/logging_optimizer.py +++ b/python/gtsam/utils/logging_optimizer.py @@ -36,7 +36,7 @@ def optimize(optimizer, check_convergence, hook): def gtsam_optimize(optimizer, params, hook): - """ Given an optimizer and its params, iterate until convergence. + """ Given an optimizer and params, iterate until convergence. After each iteration, hook(optimizer) is called. After the function, use values and errors to get the result. Arguments: @@ -44,6 +44,10 @@ def gtsam_optimize(optimizer, params {NonlinearOptimizarParams} -- Nonlinear optimizer parameters hook -- hook function to record the error """ - hook(optimizer, optimizer.error()) # call once at start (backwards compatibility) - params.iterationHook = lambda iteration, error_before, error_after: hook(optimizer, error_after) - return optimizer.optimize() + def check_convergence(optimizer, current_error, new_error): + return (optimizer.iterations() >= params.getMaxIterations()) or ( + gtsam.checkConvergence(params.getRelativeErrorTol(), params.getAbsoluteErrorTol(), params.getErrorTol(), + current_error, new_error)) or ( + isinstance(optimizer, gtsam.LevenbergMarquardtOptimizer) and optimizer.lambda_() > params.getlambdaUpperBound()) + optimize(optimizer, check_convergence, hook) + return optimizer.values() From 5796fe348820c5d6a2e35110b2223e88eb77488c Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 20 Apr 2022 16:21:59 -0400 Subject: [PATCH 13/46] Create convenience wrapper function in logging_optimizer --- python/gtsam/tests/test_logging_optimizer.py | 54 ++++++++++---------- python/gtsam/utils/logging_optimizer.py | 45 ++++++++++++++++ 2 files changed, 73 insertions(+), 26 deletions(-) diff --git a/python/gtsam/tests/test_logging_optimizer.py b/python/gtsam/tests/test_logging_optimizer.py index 4ec782635..b4f32b14f 100644 --- a/python/gtsam/tests/test_logging_optimizer.py +++ b/python/gtsam/tests/test_logging_optimizer.py @@ -18,7 +18,7 @@ import numpy as np from gtsam import Rot3 from gtsam.utils.test_case import GtsamTestCase -from gtsam.utils.logging_optimizer import gtsam_optimize +from gtsam.utils.logging_optimizer import gtsam_optimize, optimize_using KEY = 0 MODEL = gtsam.noiseModel.Unit.Create(3) @@ -34,19 +34,18 @@ class TestOptimizeComet(GtsamTestCase): rotations = {R, R.inverse()} # mean is the identity self.expected = Rot3() - graph = gtsam.NonlinearFactorGraph() - for R in rotations: - graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL)) - initial = gtsam.Values() - initial.insert(KEY, R) - self.params = gtsam.GaussNewtonParams() - self.optimizer = gtsam.GaussNewtonOptimizer( - graph, initial, self.params) + def check(actual): + # Check that optimizing yields the identity + self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6) + # Check that logging output prints out 3 lines (exact intermediate values differ by OS) + self.assertEqual(self.capturedOutput.getvalue().count('\n'), 3) + self.check = check - self.lmparams = gtsam.LevenbergMarquardtParams() - self.lmoptimizer = gtsam.LevenbergMarquardtOptimizer( - graph, initial, self.lmparams - ) + self.graph = gtsam.NonlinearFactorGraph() + for R in rotations: + self.graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL)) + self.initial = gtsam.Values() + self.initial.insert(KEY, R) # setup output capture self.capturedOutput = StringIO() @@ -64,25 +63,28 @@ class TestOptimizeComet(GtsamTestCase): print(error) # Wrapper function sets the hook and calls optimizer.optimize() for us. - gtsam_optimize(self.optimizer, self.params, hook) - - # Check that optimizing yields the identity. - actual = self.optimizer.values() - self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6) - self.assertEqual(self.capturedOutput.getvalue(), - "0.020000000000000004\n0.010000000000000005\n0.010000000000000004\n") + params = gtsam.GaussNewtonParams() + actual = optimize_using(gtsam.GaussNewtonOptimizer, hook)(self.graph, self.initial) + self.check(actual) + actual = optimize_using(gtsam.GaussNewtonOptimizer, hook)(self.graph, self.initial, params) + self.check(actual) + actual = gtsam_optimize(gtsam.GaussNewtonOptimizer(self.graph, self.initial, params), + params, hook) + self.check(actual) def test_lm_simple_printing(self): """Make sure we are properly terminating LM""" def hook(_, error): print(error) - gtsam_optimize(self.lmoptimizer, self.lmparams, hook) - - actual = self.lmoptimizer.values() - self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6) - self.assertEqual(self.capturedOutput.getvalue(), - "0.020000000000000004\n0.010000000000249996\n0.009999999999999998\n") + params = gtsam.LevenbergMarquardtParams() + actual = optimize_using(gtsam.LevenbergMarquardtOptimizer, hook)(self.graph, self.initial) + self.check(actual) + actual = optimize_using(gtsam.LevenbergMarquardtOptimizer, hook)(self.graph, self.initial, + params) + self.check(actual) + actual = gtsam_optimize(gtsam.LevenbergMarquardtOptimizer(self.graph, self.initial, params), + params, hook) @unittest.skip("Not a test we want run every time, as needs comet.ml account") def test_comet(self): diff --git a/python/gtsam/utils/logging_optimizer.py b/python/gtsam/utils/logging_optimizer.py index bf727f997..f89208bc5 100644 --- a/python/gtsam/utils/logging_optimizer.py +++ b/python/gtsam/utils/logging_optimizer.py @@ -6,6 +6,50 @@ Author: Jing Wu and Frank Dellaert from gtsam import NonlinearOptimizer, NonlinearOptimizerParams import gtsam +from typing import Any, Callable + +OPTIMIZER_PARAMS_MAP = { + gtsam.GaussNewtonOptimizer: gtsam.GaussNewtonParams, + gtsam.LevenbergMarquardtOptimizer: gtsam.LevenbergMarquardtParams, + gtsam.DoglegOptimizer: gtsam.DoglegParams, + gtsam.GncGaussNewtonOptimizer: gtsam.GaussNewtonParams, + gtsam.GncLMOptimizer: gtsam.LevenbergMarquardtParams +} + + +def optimize_using(OptimizerClass, hook) -> Callable[[Any], gtsam.Values]: + """ Wraps the constructor and "optimize()" call for an Optimizer together and adds an iteration + hook. + Example usage: + solution = optimize_using(gtsam.GaussNewtonOptimizer, hook)(graph, init, params) + + Args: + OptimizerClass (T): A NonlinearOptimizer class (e.g. GaussNewtonOptimizer, + LevenbergMarquadrtOptimizer) + hook ([T, double] -> None): Function to callback after each iteration. Args are (optimizer, + error) and return should be None. + Returns: + (Callable[*, gtsam.Values]): Call the returned function with the usual NonlinearOptimizer + arguments (will be forwarded to constructor) and it will return a Values object + representing the solution. See example usage above. + """ + + def wrapped_optimize(*args): + for arg in args: + if isinstance(arg, gtsam.NonlinearOptimizerParams): + arg.iterationHook = lambda iteration, error_before, error_after: hook( + optimizer, error_after) + break + else: + params = OPTIMIZER_PARAMS_MAP[OptimizerClass]() + params.iterationHook = lambda iteration, error_before, error_after: hook( + optimizer, error_after) + args = (*args, params) + optimizer = OptimizerClass(*args) + hook(optimizer, optimizer.error()) + return optimizer.optimize() + + return wrapped_optimize def optimize(optimizer, check_convergence, hook): @@ -37,6 +81,7 @@ def gtsam_optimize(optimizer, params, hook): """ Given an optimizer and params, iterate until convergence. + Recommend using optimize_using instead. After each iteration, hook(optimizer) is called. After the function, use values and errors to get the result. Arguments: From 1e03c8b1955b25fcf95ba37f401bdf3c2e0b50be Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 20 Apr 2022 16:55:51 -0400 Subject: [PATCH 14/46] reset captured stdout buffer for each test --- python/gtsam/tests/test_logging_optimizer.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/python/gtsam/tests/test_logging_optimizer.py b/python/gtsam/tests/test_logging_optimizer.py index b4f32b14f..c58e4f121 100644 --- a/python/gtsam/tests/test_logging_optimizer.py +++ b/python/gtsam/tests/test_logging_optimizer.py @@ -39,6 +39,8 @@ class TestOptimizeComet(GtsamTestCase): self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6) # Check that logging output prints out 3 lines (exact intermediate values differ by OS) self.assertEqual(self.capturedOutput.getvalue().count('\n'), 3) + # reset stdout catcher + self.capturedOutput.truncate(0) self.check = check self.graph = gtsam.NonlinearFactorGraph() From 7fe577b553f92232bce23e049bc089fdb765e702 Mon Sep 17 00:00:00 2001 From: senselessDev Date: Fri, 22 Apr 2022 18:39:51 +0200 Subject: [PATCH 15/46] update explanation of uncertainty ellipses, try to fix #1067 --- python/gtsam/utils/plot.py | 155 +++++++++++++++++++++++-------------- 1 file changed, 96 insertions(+), 59 deletions(-) diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index abadf62aa..db78135b7 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -12,13 +12,42 @@ from mpl_toolkits.mplot3d import Axes3D # pylint: disable=unused-import import gtsam from gtsam import Marginals, Point2, Point3, Pose2, Pose3, Values -# For future reference: following -# https://www.xarg.org/2018/04/how-to-plot-a-covariance-error-ellipse/ -# we have, in 2D: -# def kk(p): return math.sqrt(-2*math.log(1-p)) # k to get p probability mass -# def pp(k): return 1-math.exp(-float(k**2)/2.0) # p as a function of k -# Some values: -# k = 5 => p = 99.9996 % + +# For translation between a scaling of the uncertainty ellipse and the percentage of +# inliers see discussion in https://github.com/borglab/gtsam/pull/1067 +# +# Both directions can be calculated by the following functions: +# def pct_to_sigma(pct, dof): +# return np.sqrt(scipy.stats.chi2.ppf(pct / 100., df=dof)) +# +# def sigma_to_pct(sigma, dof): +# return scipy.stats.chi2.cdf(sigma**2, df=dof) * 100. +# +# In the following, the default scaling is chosen for 95% inliers, which +# translates to the following sigma values: +# 2D: pct_to_sigma(95, 2) -> 2.447746830680816 +# 3D: pct_to_sigma(95, 3) -> 2.7954834829151074 +# +# Further references are Stochastic Models, Estimation, and Control Vol 1 by Maybeck, +# page 366 and https://www.xarg.org/2018/04/how-to-plot-a-covariance-error-ellipse/ +# +# For reference, here are the inlier percentages for some sigma values: +# 1 2 3 4 5 +# 1D 68.26895 95.44997 99.73002 99.99367 99.99994 +# 2D 39.34693 86.46647 98.88910 99.96645 99.99963 +# 3D 19.87480 73.85359 97.07091 99.88660 99.99846 +# +# This can be generated by: +# for dim in range(0, 4): +# print("{}D".format(dim), end="") +# for n_sigma in range(1, 6): +# if dim == 0: print("\t {} ".format(n_sigma), end="") +# else: print("\t{:.5f}".format(sigma_to_pct(n_sigma, dim)), end="") +# print() +# +# The translation routines are not included in GTSAM, because it would introduce +# scipy as a new dependency. + def set_axes_equal(fignum: int) -> None: """ @@ -81,12 +110,8 @@ def plot_covariance_ellipse_3d(axes, """ Plots a Gaussian as an uncertainty ellipse - Based on Maybeck Vol 1, page 366 - For the 3D case: - k = 1.878 corresponds to 1 std, 68.26% of all probability - k = 3.763 corresponds to 3 std, 99.74% of all probability - - We choose k = 5 which corresponds to 99.99846% of all probability in 3D + The ellipse is scaled in such a way that 95% of drawn samples are inliers. + Derivation of the scaling factor is explained at the beginning of this file. Args: axes (matplotlib.axes.Axes): Matplotlib axes. @@ -97,8 +122,8 @@ def plot_covariance_ellipse_3d(axes, n: Defines the granularity of the ellipse. Higher values indicate finer ellipses. alpha: Transparency value for the plotted surface in the range [0, 1]. """ - # Sigma value corresponding to the covariance ellipse - k = 5 + # this corresponds to 95%, see note above + k = 2.7954834829151074 U, S, _ = np.linalg.svd(P) radii = k * np.sqrt(S) @@ -119,6 +144,38 @@ def plot_covariance_ellipse_3d(axes, axes.plot_surface(x, y, z, alpha=alpha, cmap='hot') +def plot_covariance_ellipse_2d(axes, + origin: Point2, + covariance: np.ndarray) -> None: + """ + Plots a Gaussian as an uncertainty ellipse + + The ellipse is scaled in such a way that 95% of drawn samples are inliers. + Derivation of the scaling factor is explained at the beginning of this file. + + Args: + axes (matplotlib.axes.Axes): Matplotlib axes. + origin: The origin in the world frame. + covariance: The marginal covariance matrix of the 2D point + which will be represented as an ellipse. + """ + + w, v = np.linalg.eig(covariance) + + # this corresponds to 95%, see note above + k = 2.447746830680816 + + angle = np.arctan2(v[1, 0], v[0, 0]) + # We multiply k by 2 since k corresponds to the radius but Ellipse uses + # the diameter. + e1 = patches.Ellipse(origin, + np.sqrt(w[0]) * 2 * k, + np.sqrt(w[1]) * 2 * k, + np.rad2deg(angle), + fill=False) + axes.add_patch(e1) + + def plot_point2_on_axes(axes, point: Point2, linespec: str, @@ -127,13 +184,8 @@ def plot_point2_on_axes(axes, Plot a 2D point and its corresponding uncertainty ellipse on given axis `axes` with given `linespec`. - Based on Stochastic Models, Estimation, and Control Vol 1 by Maybeck, - page 366 - For the 2D case: - k = 1.515 corresponds to 1 std, 68.26% of all probability - k = 3.438 corresponds to 3 std, 99.74% of all probability - - We choose k = 5 which corresponds to 99.99963% of all probability for 2D. + The uncertainty ellipse (if covariance is given) is scaled in such a way + that 95% of drawn samples are inliers, see `plot_covariance_ellipse_2d`. Args: axes (matplotlib.axes.Axes): Matplotlib axes. @@ -143,21 +195,7 @@ def plot_point2_on_axes(axes, """ axes.plot([point[0]], [point[1]], linespec, marker='.', markersize=10) if P is not None: - w, v = np.linalg.eig(P) - - # 5 sigma corresponds to 99.9996%, see note above - k = 5.0 - - angle = np.arctan2(v[1, 0], v[0, 0]) - # We multiply k by 2 since k corresponds to the radius but Ellipse uses - # the diameter. - e1 = patches.Ellipse(point, - np.sqrt(w[0]) * 2 * k, - np.sqrt(w[1]) * 2 * k, - np.rad2deg(angle), - fill=False) - axes.add_patch(e1) - + plot_covariance_ellipse_2d(axes, point, P) def plot_point2( fignum: int, @@ -169,6 +207,9 @@ def plot_point2( """ Plot a 2D point on given figure with given `linespec`. + The uncertainty ellipse (if covariance is given) is scaled in such a way + that 95% of drawn samples are inliers, see `plot_covariance_ellipse_2d`. + Args: fignum: Integer representing the figure number to use for plotting. point: The point to be plotted. @@ -197,13 +238,8 @@ def plot_pose2_on_axes(axes, """ Plot a 2D pose on given axis `axes` with given `axis_length`. - Based on Stochastic Models, Estimation, and Control Vol 1 by Maybeck, - page 366 - For the 2D case: - k = 1.515 corresponds to 1 std, 68.26% of all probability - k = 3.438 corresponds to 3 std, 99.74% of all probability - - We choose k = 5 which corresponds to 99.99963% of all probability for 2D. + The ellipse is scaled in such a way that 95% of drawn samples are inliers, + see `plot_covariance_ellipse_2d`. Args: axes (matplotlib.axes.Axes): Matplotlib axes. @@ -229,21 +265,7 @@ def plot_pose2_on_axes(axes, if covariance is not None: pPp = covariance[0:2, 0:2] gPp = np.matmul(np.matmul(gRp, pPp), gRp.T) - - w, v = np.linalg.eig(gPp) - - # 5 sigma corresponds to 99.9996%, see note above - k = 5.0 - - angle = np.arctan2(v[1, 0], v[0, 0]) - # We multiply k by 2 since k corresponds to the radius but Ellipse uses - # the diameter. - e1 = patches.Ellipse(origin, - np.sqrt(w[0]) * 2 * k, - np.sqrt(w[1]) * 2 * k, - np.rad2deg(angle), - fill=False) - axes.add_patch(e1) + plot_covariance_ellipse_2d(axes, origin, gPp) def plot_pose2( @@ -256,6 +278,9 @@ def plot_pose2( """ Plot a 2D pose on given figure with given `axis_length`. + The uncertainty ellipse (if covariance is given) is scaled in such a way + that 95% of drawn samples are inliers, see `plot_covariance_ellipse_2d`. + Args: fignum: Integer representing the figure number to use for plotting. pose: The pose to be plotted. @@ -285,6 +310,9 @@ def plot_point3_on_axes(axes, """ Plot a 3D point on given axis `axes` with given `linespec`. + The uncertainty ellipse (if covariance is given) is scaled in such a way + that 95% of drawn samples are inliers, see `plot_covariance_ellipse_3d`. + Args: axes (matplotlib.axes.Axes): Matplotlib axes. point: The point to be plotted. @@ -306,6 +334,9 @@ def plot_point3( """ Plot a 3D point on given figure with given `linespec`. + The uncertainty ellipse (if covariance is given) is scaled in such a way + that 95% of drawn samples are inliers, see `plot_covariance_ellipse_3d`. + Args: fignum: Integer representing the figure number to use for plotting. point: The point to be plotted. @@ -380,6 +411,9 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1): """ Plot a 3D pose on given axis `axes` with given `axis_length`. + The uncertainty ellipse (if covariance is given) is scaled in such a way + that 95% of drawn samples are inliers, see `plot_covariance_ellipse_3d`. + Args: axes (matplotlib.axes.Axes): Matplotlib axes. point (gtsam.Point3): The point to be plotted. @@ -422,6 +456,9 @@ def plot_pose3( """ Plot a 3D pose on given figure with given `axis_length`. + The uncertainty ellipse (if covariance is given) is scaled in such a way + that 95% of drawn samples are inliers, see `plot_covariance_ellipse_3d`. + Args: fignum: Integer representing the figure number to use for plotting. pose (gtsam.Pose3): 3D pose to be plotted. From 3ea19f4bd0df147558c46a2c82ee8c2928a580f0 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 23 Apr 2022 19:08:16 +0530 Subject: [PATCH 16/46] add additional methods to TA + wrapper --- gtsam/sfm/TranslationRecovery.cpp | 48 ++++++++++++++++++++++++++----- gtsam/sfm/TranslationRecovery.h | 17 ++++++++++- gtsam/sfm/sfm.i | 16 +++++++++-- tests/testTranslationRecovery.cpp | 39 ++++++++++++++++++++++++- 4 files changed, 109 insertions(+), 11 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 2e81c2d56..7938d6b93 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -28,6 +29,7 @@ #include #include #include +#include #include #include @@ -81,7 +83,7 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const { } void TranslationRecovery::addPrior( - const double scale, NonlinearFactorGraph *graph, + const double scale, const boost::shared_ptr graph, const SharedNoiseModel &priorNoiseModel) const { auto edge = relativeTranslations_.begin(); if (edge == relativeTranslations_.end()) return; @@ -91,6 +93,32 @@ void TranslationRecovery::addPrior( edge->key2(), scale * edge->measured().point3(), edge->noiseModel()); } +void TranslationRecovery::addPrior( + Key i, const Point3 &prior, + const boost::shared_ptr graph, + const SharedNoiseModel &priorNoiseModel) const { + graph->addPrior(getUniqueKey(i), prior, priorNoiseModel); +} + +Key TranslationRecovery::getUniqueKey(const Key i) const { + for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) { + Key optimizedKey = optimizedAndDuplicateKeys.first; + std::set duplicateKeys = optimizedAndDuplicateKeys.second; + if (i == optimizedKey || duplicateKeys.count(i)) return optimizedKey; + } + // Unlikely case, when i is not in the graph. + return i; +} + +void TranslationRecovery::addRelativeHardConstraint( + Key i, Key j, const Point3 &w_itj, + const boost::shared_ptr graph) const { + Point3_ wti_(getUniqueKey(i)), wtj_(getUniqueKey(j)); + Expression w_itj_ = wtj_ - wti_; + graph->addExpressionFactor(noiseModel::Constrained::All(3, 1e9), w_itj, + w_itj_); +} + Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const { uniform_real_distribution randomVal(-1, 1); // Create a lambda expression that checks whether value exists and randomly @@ -125,12 +153,17 @@ Values TranslationRecovery::initializeRandomly() const { } Values TranslationRecovery::run(const double scale) const { - auto graph = buildGraph(); - addPrior(scale, &graph); + boost::shared_ptr graph_ptr = + boost::make_shared(buildGraph()); + addPrior(scale, graph_ptr); const Values initial = initializeRandomly(); - LevenbergMarquardtOptimizer lm(graph, initial, params_); + LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params_); Values result = lm.optimize(); + return addDuplicateNodes(result); +} +Values TranslationRecovery::addDuplicateNodes(const Values &result) const { + Values final_result = result; // Nodes that were not optimized are stored in sameTranslationNodes_ as a map // from a key that was optimized to keys that were not optimized. Iterate over // map and add results for keys not optimized. @@ -139,11 +172,12 @@ Values TranslationRecovery::run(const double scale) const { std::set duplicateKeys = optimizedAndDuplicateKeys.second; // Add the result for the duplicate key if it does not already exist. for (const Key duplicateKey : duplicateKeys) { - if (result.exists(duplicateKey)) continue; - result.insert(duplicateKey, result.at(optimizedKey)); + if (final_result.exists(duplicateKey)) continue; + final_result.insert(duplicateKey, + final_result.at(optimizedKey)); } } - return result; + return final_result; } TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements( diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 30c9a14e3..a0bcb5dd4 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -93,10 +93,20 @@ class TranslationRecovery { * @param graph factor graph to which prior is added. * @param priorNoiseModel the noise model to use with the prior. */ - void addPrior(const double scale, NonlinearFactorGraph *graph, + void addPrior(const double scale, + const boost::shared_ptr graph, const SharedNoiseModel &priorNoiseModel = noiseModel::Isotropic::Sigma(3, 0.01)) const; + void addPrior(Key i, const Point3 &prior, + const boost::shared_ptr graph, + const SharedNoiseModel &priorNoiseModel = + noiseModel::Isotropic::Sigma(3, 0.01)) const; + + void addRelativeHardConstraint( + Key i, Key j, const Point3 &w_itj, + const boost::shared_ptr graph) const; + /** * @brief Create random initial translations. * @@ -120,6 +130,8 @@ class TranslationRecovery { */ Values run(const double scale = 1.0) const; + Values addDuplicateNodes(const Values &result) const; + /** * @brief Simulate translation direction measurements * @@ -131,5 +143,8 @@ class TranslationRecovery { */ static TranslationEdges SimulateMeasurements( const Values &poses, const std::vector &edges); + + private: + Key getUniqueKey(const Key i) const; }; } // namespace gtsam diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index bf9a73ac5..36a73b792 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -4,6 +4,8 @@ namespace gtsam { +#include +#include #include class SfmTrack { SfmTrack(); @@ -142,8 +144,8 @@ class ShonanAveraging2 { ShonanAveraging2(string g2oFile); ShonanAveraging2(string g2oFile, const gtsam::ShonanAveragingParameters2& parameters); - ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors, - const gtsam::ShonanAveragingParameters2 ¶meters); + ShonanAveraging2(const gtsam::BetweenFactorPose2s& factors, + const gtsam::ShonanAveragingParameters2& parameters); // Query properties size_t nrUnknowns() const; @@ -259,6 +261,16 @@ class TranslationRecovery { TranslationRecovery( const gtsam::BinaryMeasurementsUnit3& relativeTranslations); // default LevenbergMarquardtParams + gtsam::NonlinearFactorGraph buildGraph() const; + gtsam::Values initializeRandomly() const; + void addPrior(gtsam::Key i, const gtsam::Point3& prior, + gtsam::NonlinearFactorGraph* graph, + const gtsam::SharedNoiseModel& model = + gtsam::noiseModel::Isotropic::Sigma(3, 0.01)) const; + void addRelativeHardConstraint(gtsam::Key i, gtsam::Key j, + const gtsam::Point3& w_itj, + gtsam::NonlinearFactorGraph* graph) const; + gtsam::Values addDuplicateNodes(const gtsam::Values& result) const; gtsam::Values run(const double scale) const; gtsam::Values run() const; // default scale = 1.0 }; diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 833f11355..006750252 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -17,8 +17,8 @@ */ #include -#include #include +#include #include using namespace std; @@ -265,6 +265,43 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) { EXPECT(assert_equal(Point3(0, 0, 0), result.at(2), 1e-8)); } +TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) { + // Create a dataset with 3 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // \ __ / + // \\// + // 3 + // + // 0 and 1 face in the same direction but have a translation offset. 3 is in + // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset. + + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); + + auto relativeTranslations = + TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {3, 0}}); + + TranslationRecovery algorithm(relativeTranslations); + boost::shared_ptr graph_ptr = + boost::make_shared(algorithm.buildGraph()); + algorithm.addPrior(0, Point3(), graph_ptr); + algorithm.addRelativeHardConstraint(0, 1, Point3(2, 0, 0), graph_ptr); + const Values initial = algorithm.initializeRandomly(); + LevenbergMarquardtParams params; + LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params); + auto result = algorithm.addDuplicateNodes(lm.optimize()); + EXPECT_LONGS_EQUAL(4, graph_ptr->size()); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); + EXPECT(assert_equal(Point3(2, 0, 0), result.at(1), 1e-4)); + EXPECT(assert_equal(Point3(1, -1, 0), result.at(3), 1e-4)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 4d275a45e64dc3c39b975e582bcdf05b8c248aa3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 5 May 2022 11:23:51 -0400 Subject: [PATCH 17/46] remove key formatting from keys when defining vertices and edges to avoid syntax errors --- gtsam/inference/DotWriter.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/inference/DotWriter.cpp b/gtsam/inference/DotWriter.cpp index ad5330575..eac0c90f9 100644 --- a/gtsam/inference/DotWriter.cpp +++ b/gtsam/inference/DotWriter.cpp @@ -43,7 +43,7 @@ void DotWriter::drawVariable(Key key, const KeyFormatter& keyFormatter, const boost::optional& position, ostream* os) const { // Label the node with the label from the KeyFormatter - *os << " var" << keyFormatter(key) << "[label=\"" << keyFormatter(key) + *os << " var" << key << "[label=\"" << keyFormatter(key) << "\""; if (position) { *os << ", pos=\"" << position->x() << "," << position->y() << "!\""; @@ -65,13 +65,13 @@ void DotWriter::DrawFactor(size_t i, const boost::optional& position, static void ConnectVariables(Key key1, Key key2, const KeyFormatter& keyFormatter, ostream* os) { - *os << " var" << keyFormatter(key1) << "--" - << "var" << keyFormatter(key2) << ";\n"; + *os << " var" << key1 << "--" + << "var" << key2 << ";\n"; } static void ConnectVariableFactor(Key key, const KeyFormatter& keyFormatter, size_t i, ostream* os) { - *os << " var" << keyFormatter(key) << "--" + *os << " var" << key << "--" << "factor" << i << ";\n"; } From 51d1c27f2d8c829dfb4711739f2a0fd4d1cd3311 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 5 May 2022 11:24:26 -0400 Subject: [PATCH 18/46] add prior factor tests and remove TODO --- gtsam/nonlinear/Marginals.h | 2 +- gtsam/nonlinear/PriorFactor.h | 1 - gtsam/slam/tests/testPriorFactor.cpp | 40 +++++++++++++++++++++++++--- 3 files changed, 37 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 028545d01..3c5aa9cab 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -121,7 +121,7 @@ public: /** Optimize the bayes tree */ VectorValues optimize() const; - + protected: /** Compute the Bayes Tree as a helper function to the constructor */ diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index c745f7bd9..a490162ac 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -94,7 +94,6 @@ namespace gtsam { Vector evaluateError(const T& x, boost::optional H = boost::none) const override { if (H) (*H) = Matrix::Identity(traits::GetDimension(x),traits::GetDimension(x)); // manifold equivalent of z-x -> Local(x,z) - // TODO(ASL) Add Jacobians. return -traits::Local(x, prior_); } diff --git a/gtsam/slam/tests/testPriorFactor.cpp b/gtsam/slam/tests/testPriorFactor.cpp index 2dc083cb2..d1a60e346 100644 --- a/gtsam/slam/tests/testPriorFactor.cpp +++ b/gtsam/slam/tests/testPriorFactor.cpp @@ -5,12 +5,16 @@ * @date Nov 4, 2014 */ -#include -#include #include +#include +#include +#include +#include using namespace std; +using namespace std::placeholders; using namespace gtsam; +using namespace imuBias; /* ************************************************************************* */ @@ -23,16 +27,44 @@ TEST(PriorFactor, ConstructorScalar) { // Constructor vector3 TEST(PriorFactor, ConstructorVector3) { SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); - PriorFactor factor(1, Vector3(1,2,3), model); + PriorFactor factor(1, Vector3(1, 2, 3), model); } // Constructor dynamic sized vector TEST(PriorFactor, ConstructorDynamicSizeVector) { - Vector v(5); v << 1, 2, 3, 4, 5; + Vector v(5); + v << 1, 2, 3, 4, 5; SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0); PriorFactor factor(1, v, model); } +Vector callEvaluateError(const PriorFactor& factor, + const ConstantBias& bias) { + return factor.evaluateError(bias); +} + +// Test for imuBias::ConstantBias +TEST(PriorFactor, ConstantBias) { + Vector3 biasAcc(1, 2, 3); + Vector3 biasGyro(0.1, 0.2, 0.3); + ConstantBias bias(biasAcc, biasGyro); + + PriorFactor factor(1, bias, + noiseModel::Isotropic::Sigma(6, 0.1)); + Values values; + values.insert(1, bias); + + EXPECT_DOUBLES_EQUAL(0.0, factor.error(values), 1e-8); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); + + ConstantBias incorrectBias( + (Vector6() << 1.1, 2.1, 3.1, 0.2, 0.3, 0.4).finished()); + values.clear(); + values.insert(1, incorrectBias); + EXPECT_DOUBLES_EQUAL(3.0, factor.error(values), 1e-8); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + /* ************************************************************************* */ int main() { TestResult tr; From 0cfeb8621c5605b0d32fb469f4aa8d1600c4c8fd Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 5 May 2022 14:35:10 -0400 Subject: [PATCH 19/46] fix tests and remove additional keyformatter calls --- .../tests/testDiscreteFactorGraph.cpp | 14 +++---- gtsam/inference/BayesNet-inst.h | 5 ++- gtsam/symbolic/tests/testSymbolicBayesNet.cpp | 18 ++++----- tests/testNonlinearFactorGraph.cpp | 40 +++++++++---------- 4 files changed, 39 insertions(+), 38 deletions(-) diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp index 0a7d869ec..3d9621aff 100644 --- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp +++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp @@ -415,16 +415,16 @@ TEST(DiscreteFactorGraph, DotWithNames) { "graph {\n" " size=\"5,5\";\n" "\n" - " varC[label=\"C\"];\n" - " varA[label=\"A\"];\n" - " varB[label=\"B\"];\n" + " var0[label=\"C\"];\n" + " var1[label=\"A\"];\n" + " var2[label=\"B\"];\n" "\n" " factor0[label=\"\", shape=point];\n" - " varC--factor0;\n" - " varA--factor0;\n" + " var0--factor0;\n" + " var1--factor0;\n" " factor1[label=\"\", shape=point];\n" - " varC--factor1;\n" - " varB--factor1;\n" + " var0--factor1;\n" + " var2--factor1;\n" "}\n"; EXPECT(actual == expected); } diff --git a/gtsam/inference/BayesNet-inst.h b/gtsam/inference/BayesNet-inst.h index afde5498d..e792b5c03 100644 --- a/gtsam/inference/BayesNet-inst.h +++ b/gtsam/inference/BayesNet-inst.h @@ -53,8 +53,9 @@ void BayesNet::dot(std::ostream& os, auto frontals = conditional->frontals(); const Key me = frontals.front(); auto parents = conditional->parents(); - for (const Key& p : parents) - os << " var" << keyFormatter(p) << "->var" << keyFormatter(me) << "\n"; + for (const Key& p : parents) { + os << " var" << p << "->var" << me << "\n"; + } } os << "}"; diff --git a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp index 2e13be10e..7795d5b89 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp @@ -104,16 +104,16 @@ TEST(SymbolicBayesNet, Dot) { "digraph {\n" " size=\"5,5\";\n" "\n" - " vara1[label=\"a1\", pos=\"1,2!\", shape=box];\n" - " vara2[label=\"a2\", pos=\"2,2!\", shape=box];\n" - " varx1[label=\"x1\", pos=\"1,1!\"];\n" - " varx2[label=\"x2\", pos=\"2,1!\"];\n" - " varx3[label=\"x3\", pos=\"3,1!\"];\n" + " var6989586621679009793[label=\"a1\", pos=\"1,2!\", shape=box];\n" + " var6989586621679009794[label=\"a2\", pos=\"2,2!\", shape=box];\n" + " var8646911284551352321[label=\"x1\", pos=\"1,1!\"];\n" + " var8646911284551352322[label=\"x2\", pos=\"2,1!\"];\n" + " var8646911284551352323[label=\"x3\", pos=\"3,1!\"];\n" "\n" - " varx1->varx2\n" - " vara1->varx2\n" - " varx2->varx3\n" - " vara2->varx3\n" + " var8646911284551352321->var8646911284551352322\n" + " var6989586621679009793->var8646911284551352322\n" + " var8646911284551352322->var8646911284551352323\n" + " var6989586621679009794->var8646911284551352323\n" "}"); } diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 05a6e7f45..e1a88d616 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -335,21 +335,21 @@ TEST(NonlinearFactorGraph, dot) { "graph {\n" " size=\"5,5\";\n" "\n" - " varl1[label=\"l1\"];\n" - " varx1[label=\"x1\"];\n" - " varx2[label=\"x2\"];\n" + " var7782220156096217089[label=\"l1\"];\n" + " var8646911284551352321[label=\"x1\"];\n" + " var8646911284551352322[label=\"x2\"];\n" "\n" " factor0[label=\"\", shape=point];\n" - " varx1--factor0;\n" + " var8646911284551352321--factor0;\n" " factor1[label=\"\", shape=point];\n" - " varx1--factor1;\n" - " varx2--factor1;\n" + " var8646911284551352321--factor1;\n" + " var8646911284551352322--factor1;\n" " factor2[label=\"\", shape=point];\n" - " varx1--factor2;\n" - " varl1--factor2;\n" + " var8646911284551352321--factor2;\n" + " var7782220156096217089--factor2;\n" " factor3[label=\"\", shape=point];\n" - " varx2--factor3;\n" - " varl1--factor3;\n" + " var8646911284551352322--factor3;\n" + " var7782220156096217089--factor3;\n" "}\n"; const NonlinearFactorGraph fg = createNonlinearFactorGraph(); @@ -363,21 +363,21 @@ TEST(NonlinearFactorGraph, dot_extra) { "graph {\n" " size=\"5,5\";\n" "\n" - " varl1[label=\"l1\", pos=\"0,0!\"];\n" - " varx1[label=\"x1\", pos=\"1,0!\"];\n" - " varx2[label=\"x2\", pos=\"1,1.5!\"];\n" + " var7782220156096217089[label=\"l1\", pos=\"0,0!\"];\n" + " var8646911284551352321[label=\"x1\", pos=\"1,0!\"];\n" + " var8646911284551352322[label=\"x2\", pos=\"1,1.5!\"];\n" "\n" " factor0[label=\"\", shape=point];\n" - " varx1--factor0;\n" + " var8646911284551352321--factor0;\n" " factor1[label=\"\", shape=point];\n" - " varx1--factor1;\n" - " varx2--factor1;\n" + " var8646911284551352321--factor1;\n" + " var8646911284551352322--factor1;\n" " factor2[label=\"\", shape=point];\n" - " varx1--factor2;\n" - " varl1--factor2;\n" + " var8646911284551352321--factor2;\n" + " var7782220156096217089--factor2;\n" " factor3[label=\"\", shape=point];\n" - " varx2--factor3;\n" - " varl1--factor3;\n" + " var8646911284551352322--factor3;\n" + " var7782220156096217089--factor3;\n" "}\n"; const NonlinearFactorGraph fg = createNonlinearFactorGraph(); From 1d77ba55e400560e59dce39961d56ea59ccd240b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 5 May 2022 16:49:03 -0400 Subject: [PATCH 20/46] fix python test --- python/gtsam/tests/test_DiscreteBayesNet.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/python/gtsam/tests/test_DiscreteBayesNet.py b/python/gtsam/tests/test_DiscreteBayesNet.py index 74191dcc7..10c5db612 100644 --- a/python/gtsam/tests/test_DiscreteBayesNet.py +++ b/python/gtsam/tests/test_DiscreteBayesNet.py @@ -11,12 +11,12 @@ Author: Frank Dellaert # pylint: disable=no-name-in-module, invalid-name -import unittest import textwrap +import unittest import gtsam -from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteFactorGraph, - DiscreteKeys, DiscreteDistribution, DiscreteValues, Ordering) +from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteDistribution, + DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering) from gtsam.utils.test_case import GtsamTestCase # Some keys: @@ -152,10 +152,10 @@ class TestDiscreteBayesNet(GtsamTestCase): var4[label="4"]; var5[label="5"]; var6[label="6"]; - vara0[label="a0", pos="0,2!"]; + var6989586621679009792[label="a0", pos="0,2!"]; var4->var6 - vara0->var3 + var6989586621679009792->var3 var3->var5 var6->var5 }""" From 1d6fd5409af322749b84b07c90986d99e6a88ee8 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Thu, 5 May 2022 18:40:58 -0700 Subject: [PATCH 21/46] change to input params, add docstring --- gtsam/sfm/TranslationRecovery.cpp | 57 ++++++++++++------------ gtsam/sfm/TranslationRecovery.h | 73 ++++++++++++++++++++++++------- tests/testTranslationRecovery.cpp | 55 ++++++++++++++++++----- 3 files changed, 129 insertions(+), 56 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 7938d6b93..da258bbc0 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -42,8 +43,8 @@ static std::mt19937 kRandomNumberGenerator(42); TranslationRecovery::TranslationRecovery( const TranslationRecovery::TranslationEdges &relativeTranslations, - const LevenbergMarquardtParams &lmParams) - : params_(lmParams) { + const TranslationRecoveryParams ¶ms) + : params_(params) { // Some relative translations may be zero. We treat nodes that have a zero // relativeTranslation as a single node. @@ -73,12 +74,21 @@ TranslationRecovery::TranslationRecovery( NonlinearFactorGraph TranslationRecovery::buildGraph() const { NonlinearFactorGraph graph; - // Add all relative translation edges + // Add translation factors for input translation directions. for (auto edge : relativeTranslations_) { graph.emplace_shared(edge.key1(), edge.key2(), edge.measured(), edge.noiseModel()); } + // Add between factors for optional relative translations. + for (auto edge : params_.getBetweenTranslations()) { + Key k1 = getUniqueKey(edge.key1()), k2 = getUniqueKey(edge.key2()); + if (k1 != k2) { + graph.emplace_shared>(k1, k2, edge.measured(), + edge.noiseModel()); + } + } + return graph; } @@ -87,17 +97,13 @@ void TranslationRecovery::addPrior( const SharedNoiseModel &priorNoiseModel) const { auto edge = relativeTranslations_.begin(); if (edge == relativeTranslations_.end()) return; - graph->emplace_shared >(edge->key1(), Point3(0, 0, 0), - priorNoiseModel); - graph->emplace_shared >( - edge->key2(), scale * edge->measured().point3(), edge->noiseModel()); -} - -void TranslationRecovery::addPrior( - Key i, const Point3 &prior, - const boost::shared_ptr graph, - const SharedNoiseModel &priorNoiseModel) const { - graph->addPrior(getUniqueKey(i), prior, priorNoiseModel); + graph->emplace_shared>(edge->key1(), Point3(0, 0, 0), + priorNoiseModel); + // Add a scale prior only if no other between factors were added. + if (params_.getBetweenTranslations().empty()) { + graph->emplace_shared>( + edge->key2(), scale * edge->measured().point3(), edge->noiseModel()); + } } Key TranslationRecovery::getUniqueKey(const Key i) const { @@ -110,25 +116,21 @@ Key TranslationRecovery::getUniqueKey(const Key i) const { return i; } -void TranslationRecovery::addRelativeHardConstraint( - Key i, Key j, const Point3 &w_itj, - const boost::shared_ptr graph) const { - Point3_ wti_(getUniqueKey(i)), wtj_(getUniqueKey(j)); - Expression w_itj_ = wtj_ - wti_; - graph->addExpressionFactor(noiseModel::Constrained::All(3, 1e9), w_itj, - w_itj_); -} - Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const { uniform_real_distribution randomVal(-1, 1); // Create a lambda expression that checks whether value exists and randomly // initializes if not. Values initial; + const Values inputInitial = params_.getInitialValues(); auto insert = [&](Key j) { - if (!initial.exists(j)) { + if (initial.exists(j)) return; + if (inputInitial.exists(j)) { + initial.insert(j, inputInitial.at(j)); + } else { initial.insert( j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng))); } + // Assumes all nodes connected by zero-edges have the same initialization. }; // Loop over measurements and add a random translation @@ -157,12 +159,13 @@ Values TranslationRecovery::run(const double scale) const { boost::make_shared(buildGraph()); addPrior(scale, graph_ptr); const Values initial = initializeRandomly(); - LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params_); + LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params_.getLMParams()); Values result = lm.optimize(); - return addDuplicateNodes(result); + return addSameTranslationNodes(result); } -Values TranslationRecovery::addDuplicateNodes(const Values &result) const { +Values TranslationRecovery::addSameTranslationNodes( + const Values &result) const { Values final_result = result; // Nodes that were not optimized are stored in sameTranslationNodes_ as a map // from a key that was optimized to keys that were not optimized. Iterate over diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index a0bcb5dd4..e555941e0 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -28,6 +28,40 @@ namespace gtsam { +// Parameters for the Translation Recovery problem. +class TranslationRecoveryParams { + public: + std::vector> getBetweenTranslations() const { + return betweenTranslations_; + } + + void setBetweenTranslations( + const std::vector> &betweenTranslations) { + betweenTranslations_ = betweenTranslations; + } + + LevenbergMarquardtParams getLMParams() const { return lmParams_; } + + Values getInitialValues() const { return initial_; } + + void setInitialValues(const Values &values) { initial_ = values; } + + void setLMParams(const LevenbergMarquardtParams &lmParams) { + lmParams_ = lmParams; + } + + private: + // Relative translations with a known scale used as between factors in the + // problem if provided. + std::vector> betweenTranslations_; + + // LevenbergMarquardtParams for optimization. + LevenbergMarquardtParams lmParams_; + + // Initial values, random intialization will be used if not provided. + Values initial_; +}; + // Set up an optimization problem for the unknown translations Ti in the world // coordinate frame, given the known camera attitudes wRi with respect to the // world frame, and a set of (noisy) translation directions of type Unit3, @@ -57,8 +91,8 @@ class TranslationRecovery { // Translation directions between camera pairs. TranslationEdges relativeTranslations_; - // Parameters used by the LM Optimizer. - LevenbergMarquardtParams params_; + // Parameters. + TranslationRecoveryParams params_; // Map from a key in the graph to a set of keys that share the same // translation. @@ -71,13 +105,11 @@ class TranslationRecovery { * @param relativeTranslations the relative translations, in world coordinate * frames, vector of BinaryMeasurements of Unit3, where each key of a * measurement is a point in 3D. - * @param lmParams (optional) gtsam::LavenbergMarquardtParams that can be - * used to modify the parameters for the LM optimizer. By default, uses the - * default LM parameters. + * @param params (optional) parameters for the recovery problem. */ TranslationRecovery( const TranslationEdges &relativeTranslations, - const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams()); + const TranslationRecoveryParams ¶ms = TranslationRecoveryParams()); /** * @brief Build the factor graph to do the optimization. @@ -98,15 +130,6 @@ class TranslationRecovery { const SharedNoiseModel &priorNoiseModel = noiseModel::Isotropic::Sigma(3, 0.01)) const; - void addPrior(Key i, const Point3 &prior, - const boost::shared_ptr graph, - const SharedNoiseModel &priorNoiseModel = - noiseModel::Isotropic::Sigma(3, 0.01)) const; - - void addRelativeHardConstraint( - Key i, Key j, const Point3 &w_itj, - const boost::shared_ptr graph) const; - /** * @brief Create random initial translations. * @@ -126,12 +149,11 @@ class TranslationRecovery { * @brief Build and optimize factor graph. * * @param scale scale for first relative translation which fixes gauge. + * The scale is only used if relativeTranslations in the params is empty. * @return Values */ Values run(const double scale = 1.0) const; - Values addDuplicateNodes(const Values &result) const; - /** * @brief Simulate translation direction measurements * @@ -145,6 +167,23 @@ class TranslationRecovery { const Values &poses, const std::vector &edges); private: + /** + * @brief Gets the key of the variable being optimized among multiple input + * variables that have the same translation. + * + * @param i key of input variable. + * @return Key of optimized variable - same as input if it does not have any + * zero-translation edges. + */ Key getUniqueKey(const Key i) const; + + /** + * @brief Adds nodes that were not optimized for because they were connected + * to another node with a zero-translation edge in the input. + * + * @param result optimization problem result + * @return translation estimates for all variables in the input. + */ + Values addSameTranslationNodes(const Values &result) const; }; } // namespace gtsam diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 006750252..b419d8c58 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -265,6 +265,41 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) { EXPECT(assert_equal(Point3(0, 0, 0), result.at(2), 1e-8)); } +TEST(TranslationRecovery, ThreePosesWithOneSoftConstraint) { + // Create a dataset with 3 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // \ __ / + // \\// + // 3 + // + // 0 and 1 face in the same direction but have a translation offset. 3 is in + // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset. + + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); + + auto relativeTranslations = + TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {0, 3}, {1, 3}}); + + TranslationRecoveryParams params; + std::vector> betweenTranslations; + betweenTranslations.emplace_back(0, 3, Point3(1, -1, 0), noiseModel::Isotropic::Sigma(3, 1e-2)); + params.setBetweenTranslations(betweenTranslations); + + TranslationRecovery algorithm(relativeTranslations, params); + auto result = algorithm.run(); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); + EXPECT(assert_equal(Point3(2, 0, 0), result.at(1), 1e-4)); + EXPECT(assert_equal(Point3(1, -1, 0), result.at(3), 1e-4)); +} + + TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) { // Create a dataset with 3 poses. // __ __ @@ -283,25 +318,21 @@ TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) { poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); auto relativeTranslations = - TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {3, 0}}); + TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {0, 3}, {1, 3}}); - TranslationRecovery algorithm(relativeTranslations); - boost::shared_ptr graph_ptr = - boost::make_shared(algorithm.buildGraph()); - algorithm.addPrior(0, Point3(), graph_ptr); - algorithm.addRelativeHardConstraint(0, 1, Point3(2, 0, 0), graph_ptr); - const Values initial = algorithm.initializeRandomly(); - LevenbergMarquardtParams params; - LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params); - auto result = algorithm.addDuplicateNodes(lm.optimize()); - EXPECT_LONGS_EQUAL(4, graph_ptr->size()); + TranslationRecoveryParams params; + std::vector> betweenTranslations; + betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0), noiseModel::Constrained::All(3, 1e2)); + params.setBetweenTranslations(betweenTranslations); + + TranslationRecovery algorithm(relativeTranslations, params); + auto result = algorithm.run(); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); EXPECT(assert_equal(Point3(2, 0, 0), result.at(1), 1e-4)); EXPECT(assert_equal(Point3(1, -1, 0), result.at(3), 1e-4)); } - /* ************************************************************************* */ int main() { TestResult tr; From 279b9bedf9aed461e19422d2f0dac8381af4143c Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Thu, 5 May 2022 18:52:01 -0700 Subject: [PATCH 22/46] wrapper changes --- gtsam/sfm/sfm.i | 36 ++++++++++++++++++------------ gtsam/slam/dataset.h | 1 + python/CMakeLists.txt | 2 ++ python/gtsam/specializations/sfm.h | 2 ++ 4 files changed, 27 insertions(+), 14 deletions(-) diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 36a73b792..2b6f37a45 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -85,6 +85,7 @@ class BinaryMeasurement { typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; +typedef gtsam::BinaryMeasurement BinaryMeasurementPoint3; class BinaryMeasurementsUnit3 { BinaryMeasurementsUnit3(); @@ -93,6 +94,13 @@ class BinaryMeasurementsUnit3 { void push_back(const gtsam::BinaryMeasurement& measurement); }; +class BinaryMeasurementsPoint3 { + BinaryMeasurementsPoint3(); + size_t size() const; + gtsam::BinaryMeasurement at(size_t idx) const; + void push_back(const gtsam::BinaryMeasurement& measurement); +}; + #include // TODO(frank): copy/pasta below until we have integer template paremeters in @@ -254,23 +262,23 @@ class MFAS { }; #include +class TranslationRecoveryParams { + gtsam::BinaryMeasurementsPoint3 getBetweenTranslations() const; + gtsam::Values getInitialValues() const; + gtsam::LevenbergMarquardtParams getLMParams() const; + + void setBetweenTranslations( + const gtsam::BinaryMeasurementsPoint3& betweenTranslations); + void setInitialValues(const gtsam::Values& values); + void setLMParams(const gtsam::LevenbergMarquardtParams& lmParams); +}; + class TranslationRecovery { TranslationRecovery( const gtsam::BinaryMeasurementsUnit3& relativeTranslations, - const gtsam::LevenbergMarquardtParams& lmParams); - TranslationRecovery( - const gtsam::BinaryMeasurementsUnit3& - relativeTranslations); // default LevenbergMarquardtParams - gtsam::NonlinearFactorGraph buildGraph() const; - gtsam::Values initializeRandomly() const; - void addPrior(gtsam::Key i, const gtsam::Point3& prior, - gtsam::NonlinearFactorGraph* graph, - const gtsam::SharedNoiseModel& model = - gtsam::noiseModel::Isotropic::Sigma(3, 0.01)) const; - void addRelativeHardConstraint(gtsam::Key i, gtsam::Key j, - const gtsam::Point3& w_itj, - gtsam::NonlinearFactorGraph* graph) const; - gtsam::Values addDuplicateNodes(const gtsam::Values& result) const; + const gtsam::TranslationRecoveryParams& lmParams); + TranslationRecovery(const gtsam::BinaryMeasurementsUnit3& + relativeTranslations); // default params gtsam::Values run(const double scale) const; gtsam::Values run() const; // default scale = 1.0 }; diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index dc450a9f7..247be5bae 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -223,6 +223,7 @@ parse3DFactors(const std::string &filename, size_t maxIndex = 0); using BinaryMeasurementsUnit3 = std::vector>; +using BinaryMeasurementsPoint3 = std::vector>; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 inline boost::optional GTSAM_DEPRECATED diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index f5869b145..6c0c6e83a 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -47,6 +47,7 @@ set(ignore gtsam::Pose3Vector gtsam::Rot3Vector gtsam::KeyVector + gtsam::BinaryMeasurementsPoint3 gtsam::BinaryMeasurementsUnit3 gtsam::DiscreteKey gtsam::KeyPairDoubleMap) @@ -124,6 +125,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::Pose3Vector gtsam::KeyVector gtsam::FixedLagSmootherKeyTimestampMapValue + gtsam::BinaryMeasurementsPoint3 gtsam::BinaryMeasurementsUnit3 gtsam::CameraSetCal3_S2 gtsam::CameraSetCal3Bundler diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h index 6de15217f..7a5084d4a 100644 --- a/python/gtsam/specializations/sfm.h +++ b/python/gtsam/specializations/sfm.h @@ -11,6 +11,8 @@ * and saves one copy operation. */ +py::bind_vector > >( + m_, "BinaryMeasurementsPoint3"); py::bind_vector > >( m_, "BinaryMeasurementsUnit3"); py::bind_map(m_, "KeyPairDoubleMap"); From e517c344640882fb3e68d97392228abe3e5a925f Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 7 May 2022 09:12:56 -0700 Subject: [PATCH 23/46] move betweenTranslations outside params --- gtsam/sfm/TranslationRecovery.cpp | 29 ++++++++++++----------- gtsam/sfm/TranslationRecovery.h | 26 +++++++-------------- tests/testTranslationRecovery.cpp | 39 ++++++++++++++----------------- 3 files changed, 42 insertions(+), 52 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index da258bbc0..ce332ed0b 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -79,28 +79,29 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const { graph.emplace_shared(edge.key1(), edge.key2(), edge.measured(), edge.noiseModel()); } - - // Add between factors for optional relative translations. - for (auto edge : params_.getBetweenTranslations()) { - Key k1 = getUniqueKey(edge.key1()), k2 = getUniqueKey(edge.key2()); - if (k1 != k2) { - graph.emplace_shared>(k1, k2, edge.measured(), - edge.noiseModel()); - } - } - return graph; } void TranslationRecovery::addPrior( + const std::vector> &betweenTranslations, const double scale, const boost::shared_ptr graph, const SharedNoiseModel &priorNoiseModel) const { auto edge = relativeTranslations_.begin(); if (edge == relativeTranslations_.end()) return; graph->emplace_shared>(edge->key1(), Point3(0, 0, 0), priorNoiseModel); + + // Add between factors for optional relative translations. + for (auto edge : betweenTranslations) { + Key k1 = getUniqueKey(edge.key1()), k2 = getUniqueKey(edge.key2()); + if (k1 != k2) { + graph->emplace_shared>(k1, k2, edge.measured(), + edge.noiseModel()); + } + } + // Add a scale prior only if no other between factors were added. - if (params_.getBetweenTranslations().empty()) { + if (betweenTranslations.empty()) { graph->emplace_shared>( edge->key2(), scale * edge->measured().point3(), edge->noiseModel()); } @@ -154,10 +155,12 @@ Values TranslationRecovery::initializeRandomly() const { return initializeRandomly(&kRandomNumberGenerator); } -Values TranslationRecovery::run(const double scale) const { +Values TranslationRecovery::run( + const std::vector> &betweenTranslations, + const double scale) const { boost::shared_ptr graph_ptr = boost::make_shared(buildGraph()); - addPrior(scale, graph_ptr); + addPrior(betweenTranslations, scale, graph_ptr); const Values initial = initializeRandomly(); LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params_.getLMParams()); Values result = lm.optimize(); diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index e555941e0..33de91bf1 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -31,15 +31,6 @@ namespace gtsam { // Parameters for the Translation Recovery problem. class TranslationRecoveryParams { public: - std::vector> getBetweenTranslations() const { - return betweenTranslations_; - } - - void setBetweenTranslations( - const std::vector> &betweenTranslations) { - betweenTranslations_ = betweenTranslations; - } - LevenbergMarquardtParams getLMParams() const { return lmParams_; } Values getInitialValues() const { return initial_; } @@ -51,10 +42,6 @@ class TranslationRecoveryParams { } private: - // Relative translations with a known scale used as between factors in the - // problem if provided. - std::vector> betweenTranslations_; - // LevenbergMarquardtParams for optimization. LevenbergMarquardtParams lmParams_; @@ -125,10 +112,11 @@ class TranslationRecovery { * @param graph factor graph to which prior is added. * @param priorNoiseModel the noise model to use with the prior. */ - void addPrior(const double scale, - const boost::shared_ptr graph, - const SharedNoiseModel &priorNoiseModel = - noiseModel::Isotropic::Sigma(3, 0.01)) const; + void addPrior( + const std::vector> &betweenTranslations, + const double scale, const boost::shared_ptr graph, + const SharedNoiseModel &priorNoiseModel = + noiseModel::Isotropic::Sigma(3, 0.01)) const; /** * @brief Create random initial translations. @@ -152,7 +140,9 @@ class TranslationRecovery { * The scale is only used if relativeTranslations in the params is empty. * @return Values */ - Values run(const double scale = 1.0) const; + Values run( + const std::vector> &betweenTranslations = {}, + const double scale = 1.0) const; /** * @brief Simulate translation direction measurements diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index b419d8c58..6c7233a37 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -68,7 +68,7 @@ TEST(TranslationRecovery, BAL) { // Run translation recovery const double scale = 2.0; - const auto result = algorithm.run(scale); + const auto result = algorithm.run(/*betweenTranslations=*/{}, scale); // Check result for first two translations, determined by prior EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); @@ -112,7 +112,7 @@ TEST(TranslationRecovery, TwoPoseTest) { EXPECT_LONGS_EQUAL(1, graph.size()); // Run translation recovery - const auto result = algorithm.run(/*scale=*/3.0); + const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/3.0); // Check result for first two translations, determined by prior EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); @@ -149,7 +149,7 @@ TEST(TranslationRecovery, ThreePoseTest) { const auto graph = algorithm.buildGraph(); EXPECT_LONGS_EQUAL(3, graph.size()); - const auto result = algorithm.run(/*scale=*/3.0); + const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/3.0); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); @@ -186,7 +186,7 @@ TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) { EXPECT_LONGS_EQUAL(1, graph.size()); // Run translation recovery - const auto result = algorithm.run(/*scale=*/3.0); + const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/3.0); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); @@ -227,7 +227,7 @@ TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) { EXPECT_LONGS_EQUAL(3, graph.size()); // Run translation recovery - const auto result = algorithm.run(/*scale=*/4.0); + const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/4.0); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); @@ -257,7 +257,7 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) { EXPECT_LONGS_EQUAL(0, graph.size()); // Run translation recovery - const auto result = algorithm.run(/*scale=*/4.0); + const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/4.0); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); @@ -282,16 +282,15 @@ TEST(TranslationRecovery, ThreePosesWithOneSoftConstraint) { poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); - auto relativeTranslations = - TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {0, 3}, {1, 3}}); + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {0, 3}, {1, 3}}); - TranslationRecoveryParams params; std::vector> betweenTranslations; - betweenTranslations.emplace_back(0, 3, Point3(1, -1, 0), noiseModel::Isotropic::Sigma(3, 1e-2)); - params.setBetweenTranslations(betweenTranslations); + betweenTranslations.emplace_back(0, 3, Point3(1, -1, 0), + noiseModel::Isotropic::Sigma(3, 1e-2)); - TranslationRecovery algorithm(relativeTranslations, params); - auto result = algorithm.run(); + TranslationRecovery algorithm(relativeTranslations); + auto result = algorithm.run(betweenTranslations); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); @@ -299,7 +298,6 @@ TEST(TranslationRecovery, ThreePosesWithOneSoftConstraint) { EXPECT(assert_equal(Point3(1, -1, 0), result.at(3), 1e-4)); } - TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) { // Create a dataset with 3 poses. // __ __ @@ -317,16 +315,15 @@ TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) { poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); - auto relativeTranslations = - TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {0, 3}, {1, 3}}); + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {0, 3}, {1, 3}}); - TranslationRecoveryParams params; std::vector> betweenTranslations; - betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0), noiseModel::Constrained::All(3, 1e2)); - params.setBetweenTranslations(betweenTranslations); + betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0), + noiseModel::Constrained::All(3, 1e2)); - TranslationRecovery algorithm(relativeTranslations, params); - auto result = algorithm.run(); + TranslationRecovery algorithm(relativeTranslations); + auto result = algorithm.run(betweenTranslations); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); From 0e8c5eb5a7bd93265199add2083ce81091ec43c6 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 7 May 2022 09:23:00 -0700 Subject: [PATCH 24/46] ../gtsfm/sfm/sfm.i --- gtsam/sfm/sfm.i | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 2b6f37a45..064bc1e63 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -263,12 +263,9 @@ class MFAS { #include class TranslationRecoveryParams { - gtsam::BinaryMeasurementsPoint3 getBetweenTranslations() const; gtsam::Values getInitialValues() const; gtsam::LevenbergMarquardtParams getLMParams() const; - void setBetweenTranslations( - const gtsam::BinaryMeasurementsPoint3& betweenTranslations); void setInitialValues(const gtsam::Values& values); void setLMParams(const gtsam::LevenbergMarquardtParams& lmParams); }; @@ -279,6 +276,9 @@ class TranslationRecovery { const gtsam::TranslationRecoveryParams& lmParams); TranslationRecovery(const gtsam::BinaryMeasurementsUnit3& relativeTranslations); // default params + gtsam::Values run(const gtsam::BinaryMeasurementsPoint3& betweenTranslations, + const double scale) const; + // default empty betweenTranslations gtsam::Values run(const double scale) const; gtsam::Values run() const; // default scale = 1.0 }; From b42d3a3d2ff9bf50f98e07ba9d6df6aff5782ed2 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 7 May 2022 11:24:31 -0700 Subject: [PATCH 25/46] change params to struct --- gtsam/sfm/TranslationRecovery.cpp | 14 ++++++-------- gtsam/sfm/TranslationRecovery.h | 20 ++++---------------- gtsam/sfm/sfm.i | 9 +++------ 3 files changed, 13 insertions(+), 30 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index ce332ed0b..ce8e9c079 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -84,7 +84,7 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const { void TranslationRecovery::addPrior( const std::vector> &betweenTranslations, - const double scale, const boost::shared_ptr graph, + const double scale, NonlinearFactorGraph *graph, const SharedNoiseModel &priorNoiseModel) const { auto edge = relativeTranslations_.begin(); if (edge == relativeTranslations_.end()) return; @@ -122,11 +122,10 @@ Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const { // Create a lambda expression that checks whether value exists and randomly // initializes if not. Values initial; - const Values inputInitial = params_.getInitialValues(); auto insert = [&](Key j) { if (initial.exists(j)) return; - if (inputInitial.exists(j)) { - initial.insert(j, inputInitial.at(j)); + if (params_.initial.exists(j)) { + initial.insert(j, params_.initial.at(j)); } else { initial.insert( j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng))); @@ -158,11 +157,10 @@ Values TranslationRecovery::initializeRandomly() const { Values TranslationRecovery::run( const std::vector> &betweenTranslations, const double scale) const { - boost::shared_ptr graph_ptr = - boost::make_shared(buildGraph()); - addPrior(betweenTranslations, scale, graph_ptr); + NonlinearFactorGraph graph = buildGraph(); + addPrior(betweenTranslations, scale, &graph); const Values initial = initializeRandomly(); - LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params_.getLMParams()); + LevenbergMarquardtOptimizer lm(graph, initial, params_.lmParams); Values result = lm.optimize(); return addSameTranslationNodes(result); } diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 33de91bf1..0b25ad7b4 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -29,24 +29,12 @@ namespace gtsam { // Parameters for the Translation Recovery problem. -class TranslationRecoveryParams { - public: - LevenbergMarquardtParams getLMParams() const { return lmParams_; } - - Values getInitialValues() const { return initial_; } - - void setInitialValues(const Values &values) { initial_ = values; } - - void setLMParams(const LevenbergMarquardtParams &lmParams) { - lmParams_ = lmParams; - } - - private: +struct TranslationRecoveryParams { // LevenbergMarquardtParams for optimization. - LevenbergMarquardtParams lmParams_; + LevenbergMarquardtParams lmParams; // Initial values, random intialization will be used if not provided. - Values initial_; + Values initial; }; // Set up an optimization problem for the unknown translations Ti in the world @@ -114,7 +102,7 @@ class TranslationRecovery { */ void addPrior( const std::vector> &betweenTranslations, - const double scale, const boost::shared_ptr graph, + const double scale, NonlinearFactorGraph *graph, const SharedNoiseModel &priorNoiseModel = noiseModel::Isotropic::Sigma(3, 0.01)) const; diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 064bc1e63..66280037c 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -263,11 +263,8 @@ class MFAS { #include class TranslationRecoveryParams { - gtsam::Values getInitialValues() const; - gtsam::LevenbergMarquardtParams getLMParams() const; - - void setInitialValues(const gtsam::Values& values); - void setLMParams(const gtsam::LevenbergMarquardtParams& lmParams); + gtsam::LevenbergMarquardtParams lmParams; + gtsam::Values initial; }; class TranslationRecovery { @@ -279,7 +276,7 @@ class TranslationRecovery { gtsam::Values run(const gtsam::BinaryMeasurementsPoint3& betweenTranslations, const double scale) const; // default empty betweenTranslations - gtsam::Values run(const double scale) const; + // gtsam::Values run(const double scale) const; gtsam::Values run() const; // default scale = 1.0 }; From c1a7cf21d58e2bb2e42db1838c492aa854a69b2e Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 7 May 2022 11:26:21 -0700 Subject: [PATCH 26/46] rename getUniqueKey --- gtsam/sfm/TranslationRecovery.cpp | 5 +++-- gtsam/sfm/TranslationRecovery.h | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index ce8e9c079..aa75b8fc6 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -93,7 +93,8 @@ void TranslationRecovery::addPrior( // Add between factors for optional relative translations. for (auto edge : betweenTranslations) { - Key k1 = getUniqueKey(edge.key1()), k2 = getUniqueKey(edge.key2()); + Key k1 = getSameTranslationRootNode(edge.key1()), + k2 = getSameTranslationRootNode(edge.key2()); if (k1 != k2) { graph->emplace_shared>(k1, k2, edge.measured(), edge.noiseModel()); @@ -107,7 +108,7 @@ void TranslationRecovery::addPrior( } } -Key TranslationRecovery::getUniqueKey(const Key i) const { +Key TranslationRecovery::getSameTranslationRootNode(const Key i) const { for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) { Key optimizedKey = optimizedAndDuplicateKeys.first; std::set duplicateKeys = optimizedAndDuplicateKeys.second; diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 0b25ad7b4..931d072c8 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -153,7 +153,7 @@ class TranslationRecovery { * @return Key of optimized variable - same as input if it does not have any * zero-translation edges. */ - Key getUniqueKey(const Key i) const; + Key getSameTranslationRootNode(const Key i) const; /** * @brief Adds nodes that were not optimized for because they were connected From 230bb8eb115254739faa6bd6609b8d6434d2b016 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 7 May 2022 18:15:44 -0700 Subject: [PATCH 27/46] move relativeTranslations to run() --- gtsam/sfm/TranslationRecovery.cpp | 171 ++++++++++++++++-------------- gtsam/sfm/TranslationRecovery.h | 65 +++++------- tests/testTranslationRecovery.cpp | 49 ++++----- 3 files changed, 140 insertions(+), 145 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index aa75b8fc6..122b17ce6 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -41,16 +41,13 @@ using namespace std; // In Wrappers we have no access to this so have a default ready. static std::mt19937 kRandomNumberGenerator(42); -TranslationRecovery::TranslationRecovery( - const TranslationRecovery::TranslationEdges &relativeTranslations, - const TranslationRecoveryParams ¶ms) - : params_(params) { - // Some relative translations may be zero. We treat nodes that have a zero - // relativeTranslation as a single node. - - // A DSFMap is used to find sets of nodes that have a zero relative - // translation. Add the nodes in each edge to the DSFMap, and merge nodes that - // are connected by a zero relative translation. +// Some relative translations may be zero. We treat nodes that have a zero +// relativeTranslation as a single node. +// A DSFMap is used to find sets of nodes that have a zero relative +// translation. Add the nodes in each edge to the DSFMap, and merge nodes that +// are connected by a zero relative translation. +DSFMap getSameTranslationDSFMap( + const std::vector> &relativeTranslations) { DSFMap sameTranslationDSF; for (const auto &edge : relativeTranslations) { Key key1 = sameTranslationDSF.find(edge.key1()); @@ -59,23 +56,52 @@ TranslationRecovery::TranslationRecovery( sameTranslationDSF.merge(key1, key2); } } - // Use only those edges for which two keys have a distinct root in the DSFMap. - for (const auto &edge : relativeTranslations) { - Key key1 = sameTranslationDSF.find(edge.key1()); - Key key2 = sameTranslationDSF.find(edge.key2()); - if (key1 == key2) continue; - relativeTranslations_.emplace_back(key1, key2, edge.measured(), - edge.noiseModel()); - } - // Store the DSF map for post-processing results. - sameTranslationNodes_ = sameTranslationDSF.sets(); + return sameTranslationDSF; } -NonlinearFactorGraph TranslationRecovery::buildGraph() const { +// Removes zero-translation edges from measurements, and combines the nodes in +// these edges into a single node. +template +std::vector> removeSameTranslationNodes( + const std::vector> &edges, + const DSFMap &sameTranslationDSFMap) { + std::vector> newEdges; + for (const auto &edge : edges) { + Key key1 = sameTranslationDSFMap.find(edge.key1()); + Key key2 = sameTranslationDSFMap.find(edge.key2()); + if (key1 == key2) continue; + newEdges.emplace_back(key1, key2, edge.measured(), edge.noiseModel()); + } + return newEdges; +} + +// Adds nodes that were not optimized for because they were connected +// to another node with a zero-translation edge in the input. +Values addSameTranslationNodes(const Values &result, + const DSFMap &sameTranslationDSFMap) { + Values final_result = result; + // Nodes that were not optimized are stored in sameTranslationNodes_ as a map + // from a key that was optimized to keys that were not optimized. Iterate over + // map and add results for keys not optimized. + for (const auto &optimizedAndDuplicateKeys : sameTranslationDSFMap.sets()) { + Key optimizedKey = optimizedAndDuplicateKeys.first; + std::set duplicateKeys = optimizedAndDuplicateKeys.second; + // Add the result for the duplicate key if it does not already exist. + for (const Key duplicateKey : duplicateKeys) { + if (final_result.exists(duplicateKey)) continue; + final_result.insert(duplicateKey, + final_result.at(optimizedKey)); + } + } + return final_result; +} + +NonlinearFactorGraph TranslationRecovery::buildGraph( + const std::vector> &relativeTranslations) const { NonlinearFactorGraph graph; // Add translation factors for input translation directions. - for (auto edge : relativeTranslations_) { + for (auto edge : relativeTranslations) { graph.emplace_shared(edge.key1(), edge.key2(), edge.measured(), edge.noiseModel()); } @@ -83,22 +109,20 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const { } void TranslationRecovery::addPrior( + const std::vector> &relativeTranslations, + const double scale, const std::vector> &betweenTranslations, - const double scale, NonlinearFactorGraph *graph, + NonlinearFactorGraph *graph, const SharedNoiseModel &priorNoiseModel) const { - auto edge = relativeTranslations_.begin(); - if (edge == relativeTranslations_.end()) return; + auto edge = relativeTranslations.begin(); + if (edge == relativeTranslations.end()) return; graph->emplace_shared>(edge->key1(), Point3(0, 0, 0), priorNoiseModel); // Add between factors for optional relative translations. for (auto edge : betweenTranslations) { - Key k1 = getSameTranslationRootNode(edge.key1()), - k2 = getSameTranslationRootNode(edge.key2()); - if (k1 != k2) { - graph->emplace_shared>(k1, k2, edge.measured(), - edge.noiseModel()); - } + graph->emplace_shared>( + edge.key1(), edge.key2(), edge.measured(), edge.noiseModel()); } // Add a scale prior only if no other between factors were added. @@ -108,17 +132,9 @@ void TranslationRecovery::addPrior( } } -Key TranslationRecovery::getSameTranslationRootNode(const Key i) const { - for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) { - Key optimizedKey = optimizedAndDuplicateKeys.first; - std::set duplicateKeys = optimizedAndDuplicateKeys.second; - if (i == optimizedKey || duplicateKeys.count(i)) return optimizedKey; - } - // Unlikely case, when i is not in the graph. - return i; -} - -Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const { +Values TranslationRecovery::initializeRandomly( + const std::vector> &relativeTranslations, + std::mt19937 *rng) const { uniform_real_distribution randomVal(-1, 1); // Create a lambda expression that checks whether value exists and randomly // initializes if not. @@ -135,54 +151,53 @@ Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const { }; // Loop over measurements and add a random translation - for (auto edge : relativeTranslations_) { + for (auto edge : relativeTranslations) { insert(edge.key1()); insert(edge.key2()); } - - // If there are no valid edges, but zero-distance edges exist, initialize one - // of the nodes in a connected component of zero-distance edges. - if (initial.empty() && !sameTranslationNodes_.empty()) { - for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) { - Key optimizedKey = optimizedAndDuplicateKeys.first; - initial.insert(optimizedKey, Point3(0, 0, 0)); - } - } return initial; } -Values TranslationRecovery::initializeRandomly() const { - return initializeRandomly(&kRandomNumberGenerator); +Values TranslationRecovery::initializeRandomly( + const std::vector> &relativeTranslations) const { + return initializeRandomly(relativeTranslations, &kRandomNumberGenerator); } Values TranslationRecovery::run( - const std::vector> &betweenTranslations, - const double scale) const { - NonlinearFactorGraph graph = buildGraph(); - addPrior(betweenTranslations, scale, &graph); - const Values initial = initializeRandomly(); - LevenbergMarquardtOptimizer lm(graph, initial, params_.lmParams); - Values result = lm.optimize(); - return addSameTranslationNodes(result); -} + const TranslationEdges &relativeTranslations, const double scale, + const std::vector> &betweenTranslations) const { + // Find edges that have a zero-translation, and recompute relativeTranslations + // and betweenTranslations by retaining only one node for every zero-edge. + DSFMap sameTranslationDSFMap = + getSameTranslationDSFMap(relativeTranslations); + const TranslationEdges nonzeroRelativeTranslations = + removeSameTranslationNodes(relativeTranslations, sameTranslationDSFMap); + const std::vector> nonzeroBetweenTranslations = + removeSameTranslationNodes(betweenTranslations, sameTranslationDSFMap); -Values TranslationRecovery::addSameTranslationNodes( - const Values &result) const { - Values final_result = result; - // Nodes that were not optimized are stored in sameTranslationNodes_ as a map - // from a key that was optimized to keys that were not optimized. Iterate over - // map and add results for keys not optimized. - for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) { - Key optimizedKey = optimizedAndDuplicateKeys.first; - std::set duplicateKeys = optimizedAndDuplicateKeys.second; - // Add the result for the duplicate key if it does not already exist. - for (const Key duplicateKey : duplicateKeys) { - if (final_result.exists(duplicateKey)) continue; - final_result.insert(duplicateKey, - final_result.at(optimizedKey)); + // Create graph of translation factors. + NonlinearFactorGraph graph = buildGraph(nonzeroRelativeTranslations); + + // Add global frame prior and scale (either from betweenTranslations or + // scale). + addPrior(nonzeroRelativeTranslations, scale, nonzeroBetweenTranslations, + &graph); + + // Uses initial values from params if provided. + Values initial = initializeRandomly(nonzeroRelativeTranslations); + + // If there are no valid edges, but zero-distance edges exist, initialize one + // of the nodes in a connected component of zero-distance edges. + if (initial.empty() && !sameTranslationDSFMap.sets().empty()) { + for (const auto &optimizedAndDuplicateKeys : sameTranslationDSFMap.sets()) { + Key optimizedKey = optimizedAndDuplicateKeys.first; + initial.insert(optimizedKey, Point3(0, 0, 0)); } } - return final_result; + + LevenbergMarquardtOptimizer lm(graph, initial, params_.lmParams); + Values result = lm.optimize(); + return addSameTranslationNodes(result, sameTranslationDSFMap); } TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements( diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 931d072c8..a9f823d1a 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -69,29 +69,25 @@ class TranslationRecovery { // Parameters. TranslationRecoveryParams params_; - // Map from a key in the graph to a set of keys that share the same - // translation. - std::map> sameTranslationNodes_; - public: /** * @brief Construct a new Translation Recovery object * - * @param relativeTranslations the relative translations, in world coordinate - * frames, vector of BinaryMeasurements of Unit3, where each key of a - * measurement is a point in 3D. - * @param params (optional) parameters for the recovery problem. + * @param params parameters for the recovery problem. */ - TranslationRecovery( - const TranslationEdges &relativeTranslations, - const TranslationRecoveryParams ¶ms = TranslationRecoveryParams()); + TranslationRecovery(const TranslationRecoveryParams ¶ms) + : params_(params) {} + + // Same as above, with default parameters. + TranslationRecovery() = default; /** * @brief Build the factor graph to do the optimization. * * @return NonlinearFactorGraph */ - NonlinearFactorGraph buildGraph() const; + NonlinearFactorGraph buildGraph( + const std::vector> &relativeTranslations) const; /** * @brief Add priors on ednpoints of first measurement edge. @@ -101,8 +97,10 @@ class TranslationRecovery { * @param priorNoiseModel the noise model to use with the prior. */ void addPrior( + const std::vector> &relativeTranslations, + const double scale, const std::vector> &betweenTranslations, - const double scale, NonlinearFactorGraph *graph, + NonlinearFactorGraph *graph, const SharedNoiseModel &priorNoiseModel = noiseModel::Isotropic::Sigma(3, 0.01)) const; @@ -112,25 +110,34 @@ class TranslationRecovery { * @param rng random number generator * @return Values */ - Values initializeRandomly(std::mt19937 *rng) const; + Values initializeRandomly( + const std::vector> &relativeTranslations, + std::mt19937 *rng) const; /** * @brief Version of initializeRandomly with a fixed seed. * * @return Values */ - Values initializeRandomly() const; + Values initializeRandomly( + const std::vector> &relativeTranslations) const; /** * @brief Build and optimize factor graph. * + * @param relativeTranslations the relative translations, in world coordinate + * frames, vector of BinaryMeasurements of Unit3, where each key of a + * measurement is a point in 3D. * @param scale scale for first relative translation which fixes gauge. - * The scale is only used if relativeTranslations in the params is empty. + * The scale is only used if betweenTranslations is empty. + * @param betweenTranslations relative translations (with scale) between 2 + * points in world coordinate frame known a priori. * @return Values */ - Values run( - const std::vector> &betweenTranslations = {}, - const double scale = 1.0) const; + Values run(const TranslationEdges &relativeTranslations, + const double scale = 1.0, + const std::vector> &betweenTranslations = + {}) const; /** * @brief Simulate translation direction measurements @@ -143,25 +150,5 @@ class TranslationRecovery { */ static TranslationEdges SimulateMeasurements( const Values &poses, const std::vector &edges); - - private: - /** - * @brief Gets the key of the variable being optimized among multiple input - * variables that have the same translation. - * - * @param i key of input variable. - * @return Key of optimized variable - same as input if it does not have any - * zero-translation edges. - */ - Key getSameTranslationRootNode(const Key i) const; - - /** - * @brief Adds nodes that were not optimized for because they were connected - * to another node with a zero-translation edge in the input. - * - * @param result optimization problem result - * @return translation estimates for all variables in the input. - */ - Values addSameTranslationNodes(const Values &result) const; }; } // namespace gtsam diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 6c7233a37..5dd319d30 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -62,13 +62,13 @@ TEST(TranslationRecovery, BAL) { unitTranslation.measured())); } - TranslationRecovery algorithm(relativeTranslations); - const auto graph = algorithm.buildGraph(); + TranslationRecovery algorithm; + const auto graph = algorithm.buildGraph(relativeTranslations); EXPECT_LONGS_EQUAL(3, graph.size()); // Run translation recovery const double scale = 2.0; - const auto result = algorithm.run(/*betweenTranslations=*/{}, scale); + const auto result = algorithm.run(relativeTranslations, scale); // Check result for first two translations, determined by prior EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); @@ -107,12 +107,12 @@ TEST(TranslationRecovery, TwoPoseTest) { unitTranslation.measured())); } - TranslationRecovery algorithm(relativeTranslations); - const auto graph = algorithm.buildGraph(); + TranslationRecovery algorithm; + const auto graph = algorithm.buildGraph(relativeTranslations); EXPECT_LONGS_EQUAL(1, graph.size()); // Run translation recovery - const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/3.0); + const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0); // Check result for first two translations, determined by prior EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); @@ -145,11 +145,11 @@ TEST(TranslationRecovery, ThreePoseTest) { unitTranslation.measured())); } - TranslationRecovery algorithm(relativeTranslations); - const auto graph = algorithm.buildGraph(); + TranslationRecovery algorithm; + const auto graph = algorithm.buildGraph(relativeTranslations); EXPECT_LONGS_EQUAL(3, graph.size()); - const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/3.0); + const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); @@ -180,13 +180,9 @@ TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) { unitTranslation.measured())); } - TranslationRecovery algorithm(relativeTranslations); - const auto graph = algorithm.buildGraph(); - // There is only 1 non-zero translation edge. - EXPECT_LONGS_EQUAL(1, graph.size()); - + TranslationRecovery algorithm; // Run translation recovery - const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/3.0); + const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); @@ -222,12 +218,10 @@ TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) { unitTranslation.measured())); } - TranslationRecovery algorithm(relativeTranslations); - const auto graph = algorithm.buildGraph(); - EXPECT_LONGS_EQUAL(3, graph.size()); + TranslationRecovery algorithm; // Run translation recovery - const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/4.0); + const auto result = algorithm.run(relativeTranslations, /*scale=*/4.0); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); @@ -251,13 +245,10 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) { unitTranslation.measured())); } - TranslationRecovery algorithm(relativeTranslations); - const auto graph = algorithm.buildGraph(); - // Graph size will be zero as there no 'non-zero distance' edges. - EXPECT_LONGS_EQUAL(0, graph.size()); + TranslationRecovery algorithm; // Run translation recovery - const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/4.0); + const auto result = algorithm.run(relativeTranslations, /*scale=*/4.0); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); @@ -289,8 +280,9 @@ TEST(TranslationRecovery, ThreePosesWithOneSoftConstraint) { betweenTranslations.emplace_back(0, 3, Point3(1, -1, 0), noiseModel::Isotropic::Sigma(3, 1e-2)); - TranslationRecovery algorithm(relativeTranslations); - auto result = algorithm.run(betweenTranslations); + TranslationRecovery algorithm; + auto result = + algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); @@ -322,8 +314,9 @@ TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) { betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0), noiseModel::Constrained::All(3, 1e2)); - TranslationRecovery algorithm(relativeTranslations); - auto result = algorithm.run(betweenTranslations); + TranslationRecovery algorithm; + auto result = + algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations); // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); From c0e5ce9ef9da661440d13e47953400d96be9ab54 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 7 May 2022 18:28:54 -0700 Subject: [PATCH 28/46] wrapper updates --- gtsam/sfm/sfm.i | 30 ++++++++++++++----- .../examples/TranslationAveragingExample.py | 2 +- .../gtsam/tests/test_TranslationRecovery.py | 10 +++++-- 3 files changed, 30 insertions(+), 12 deletions(-) diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 66280037c..122a9b5a6 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -265,19 +265,33 @@ class MFAS { class TranslationRecoveryParams { gtsam::LevenbergMarquardtParams lmParams; gtsam::Values initial; + TranslationRecoveryParams(); }; class TranslationRecovery { - TranslationRecovery( + TranslationRecovery(const gtsam::TranslationRecoveryParams& lmParams); + TranslationRecovery(); // default params. + void addPrior(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + const double scale, + const gtsam::BinaryMeasurementsPoint3& betweenTranslations, + gtsam::NonlinearFactorGraph @graph, + const gtsam::SharedNoiseModel& priorNoiseModel) const; + void addPrior(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + const double scale, + const gtsam::BinaryMeasurementsPoint3& betweenTranslations, + gtsam::NonlinearFactorGraph @graph) const; + gtsam::NonlinearFactorGraph buildGraph( + const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const; + gtsam::Values run( const gtsam::BinaryMeasurementsUnit3& relativeTranslations, - const gtsam::TranslationRecoveryParams& lmParams); - TranslationRecovery(const gtsam::BinaryMeasurementsUnit3& - relativeTranslations); // default params - gtsam::Values run(const gtsam::BinaryMeasurementsPoint3& betweenTranslations, - const double scale) const; + const double scale, + const gtsam::BinaryMeasurementsPoint3& betweenTranslations) const; // default empty betweenTranslations - // gtsam::Values run(const double scale) const; - gtsam::Values run() const; // default scale = 1.0 + gtsam::Values run(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + const double scale) const; + // default scale = 1.0, empty betweenTranslations + gtsam::Values run( + const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const; }; } // namespace gtsam diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index 054b61126..92a7d04e3 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -123,7 +123,7 @@ def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, w_iZj_inliers = filter_outliers(w_iZj_list) # Run the optimizer to obtain translations for normalized directions. - wtc_values = gtsam.TranslationRecovery(w_iZj_inliers).run() + wtc_values = gtsam.TranslationRecovery().run(w_iZj_inliers) wTc_values = gtsam.Values() for key in wRc_values.keys(): diff --git a/python/gtsam/tests/test_TranslationRecovery.py b/python/gtsam/tests/test_TranslationRecovery.py index 0fb0518b6..2875e897e 100644 --- a/python/gtsam/tests/test_TranslationRecovery.py +++ b/python/gtsam/tests/test_TranslationRecovery.py @@ -34,8 +34,10 @@ class TestTranslationRecovery(unittest.TestCase): def test_constructor(self): """Construct from binary measurements.""" - algorithm = gtsam.TranslationRecovery(gtsam.BinaryMeasurementsUnit3()) + algorithm = gtsam.TranslationRecovery() self.assertIsInstance(algorithm, gtsam.TranslationRecovery) + algorithm_params = gtsam.TranslationRecovery(gtsam.TranslationRecoveryParams()) + self.assertIsInstance(algorithm_params, gtsam.TranslationRecovery) def test_run(self): gt_poses = ExampleValues() @@ -44,10 +46,12 @@ class TestTranslationRecovery(unittest.TestCase): # Set verbosity to Silent for tests lmParams = gtsam.LevenbergMarquardtParams() lmParams.setVerbosityLM("silent") + params = gtsam.TranslationRecoveryParams() + params.lmParams = lmParams - algorithm = gtsam.TranslationRecovery(measurements, lmParams) + algorithm = gtsam.TranslationRecovery(params) scale = 2.0 - result = algorithm.run(scale) + result = algorithm.run(measurements, scale) w_aTc = gt_poses.atPose3(2).translation() - gt_poses.atPose3(0).translation() w_aTb = gt_poses.atPose3(1).translation() - gt_poses.atPose3(0).translation() From 3b77df652c8a8840ffa5785d061344553534b441 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 7 May 2022 18:36:00 -0700 Subject: [PATCH 29/46] docstring updates --- gtsam/sfm/TranslationRecovery.h | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index a9f823d1a..3462cce02 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -11,7 +11,7 @@ /** * @file TranslationRecovery.h - * @author Frank Dellaert + * @author Frank Dellaert, Akshay Krishnan * @date March 2020 * @brief Recovering translations in an epipolar graph when rotations are given. */ @@ -78,12 +78,16 @@ class TranslationRecovery { TranslationRecovery(const TranslationRecoveryParams ¶ms) : params_(params) {} - // Same as above, with default parameters. + /** + * @brief Default constructor. + */ TranslationRecovery() = default; /** * @brief Build the factor graph to do the optimization. * + * @param relativeTranslations unit translation directions between + * translations to be estimated * @return NonlinearFactorGraph */ NonlinearFactorGraph buildGraph( @@ -92,8 +96,12 @@ class TranslationRecovery { /** * @brief Add priors on ednpoints of first measurement edge. * + * @param relativeTranslations unit translation directions between + * translations to be estimated * @param scale scale for first relative translation which fixes gauge. * @param graph factor graph to which prior is added. + * @param betweenTranslations relative translations (with scale) between 2 + * points in world coordinate frame known a priori. * @param priorNoiseModel the noise model to use with the prior. */ void addPrior( @@ -105,8 +113,11 @@ class TranslationRecovery { noiseModel::Isotropic::Sigma(3, 0.01)) const; /** - * @brief Create random initial translations. + * @brief Create random initial translations. Uses inial values from params if + * provided. * + * @param relativeTranslations unit translation directions between + * translations to be estimated * @param rng random number generator * @return Values */ @@ -115,8 +126,11 @@ class TranslationRecovery { std::mt19937 *rng) const; /** - * @brief Version of initializeRandomly with a fixed seed. + * @brief Version of initializeRandomly with a fixed seed. Uses initial values + * from params if provided. * + * @param relativeTranslations unit translation directions between + * translations to be estimated * @return Values */ Values initializeRandomly( From e17eddf99f8eac95077887b871f1859f7d5eb802 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Mon, 9 May 2022 22:46:16 -0700 Subject: [PATCH 30/46] move values from params to run, remove params struct --- gtsam/sfm/TranslationRecovery.cpp | 20 ++++---- gtsam/sfm/TranslationRecovery.h | 46 +++++++++---------- gtsam/sfm/sfm.i | 12 ++--- .../gtsam/tests/test_TranslationRecovery.py | 4 +- 4 files changed, 41 insertions(+), 41 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 122b17ce6..810fe7de9 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -134,15 +134,15 @@ void TranslationRecovery::addPrior( Values TranslationRecovery::initializeRandomly( const std::vector> &relativeTranslations, - std::mt19937 *rng) const { + std::mt19937 *rng, const Values &initialValues) const { uniform_real_distribution randomVal(-1, 1); // Create a lambda expression that checks whether value exists and randomly // initializes if not. Values initial; auto insert = [&](Key j) { if (initial.exists(j)) return; - if (params_.initial.exists(j)) { - initial.insert(j, params_.initial.at(j)); + if (initialValues.exists(j)) { + initial.insert(j, initialValues.at(j)); } else { initial.insert( j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng))); @@ -159,13 +159,16 @@ Values TranslationRecovery::initializeRandomly( } Values TranslationRecovery::initializeRandomly( - const std::vector> &relativeTranslations) const { - return initializeRandomly(relativeTranslations, &kRandomNumberGenerator); + const std::vector> &relativeTranslations, + const Values &initialValues) const { + return initializeRandomly(relativeTranslations, &kRandomNumberGenerator, + initialValues); } Values TranslationRecovery::run( const TranslationEdges &relativeTranslations, const double scale, - const std::vector> &betweenTranslations) const { + const std::vector> &betweenTranslations, + const Values &initialValues) const { // Find edges that have a zero-translation, and recompute relativeTranslations // and betweenTranslations by retaining only one node for every zero-edge. DSFMap sameTranslationDSFMap = @@ -184,7 +187,8 @@ Values TranslationRecovery::run( &graph); // Uses initial values from params if provided. - Values initial = initializeRandomly(nonzeroRelativeTranslations); + Values initial = + initializeRandomly(nonzeroRelativeTranslations, initialValues); // If there are no valid edges, but zero-distance edges exist, initialize one // of the nodes in a connected component of zero-distance edges. @@ -195,7 +199,7 @@ Values TranslationRecovery::run( } } - LevenbergMarquardtOptimizer lm(graph, initial, params_.lmParams); + LevenbergMarquardtOptimizer lm(graph, initial, lmParams_); Values result = lm.optimize(); return addSameTranslationNodes(result, sameTranslationDSFMap); } diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 3462cce02..7863f5133 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -28,15 +28,6 @@ namespace gtsam { -// Parameters for the Translation Recovery problem. -struct TranslationRecoveryParams { - // LevenbergMarquardtParams for optimization. - LevenbergMarquardtParams lmParams; - - // Initial values, random intialization will be used if not provided. - Values initial; -}; - // Set up an optimization problem for the unknown translations Ti in the world // coordinate frame, given the known camera attitudes wRi with respect to the // world frame, and a set of (noisy) translation directions of type Unit3, @@ -67,16 +58,16 @@ class TranslationRecovery { TranslationEdges relativeTranslations_; // Parameters. - TranslationRecoveryParams params_; + LevenbergMarquardtParams lmParams_; public: /** * @brief Construct a new Translation Recovery object * - * @param params parameters for the recovery problem. + * @param lmParams parameters for optimization. */ - TranslationRecovery(const TranslationRecoveryParams ¶ms) - : params_(params) {} + TranslationRecovery(const LevenbergMarquardtParams &lmParams) + : lmParams_(lmParams) {} /** * @brief Default constructor. @@ -94,7 +85,11 @@ class TranslationRecovery { const std::vector> &relativeTranslations) const; /** - * @brief Add priors on ednpoints of first measurement edge. + * @brief Add 3 factors to the graph: + * - A prior on the first point to lie at (0, 0, 0) + * - If betweenTranslations is non-empty, between factors provided by it. + * - If betweenTranslations is empty, a prior on scale of the first + * relativeTranslations edge. * * @param relativeTranslations unit translation directions between * translations to be estimated @@ -113,28 +108,29 @@ class TranslationRecovery { noiseModel::Isotropic::Sigma(3, 0.01)) const; /** - * @brief Create random initial translations. Uses inial values from params if - * provided. + * @brief Create random initial translations. * * @param relativeTranslations unit translation directions between * translations to be estimated * @param rng random number generator + * @param intialValues (optional) initial values from a prior * @return Values */ Values initializeRandomly( const std::vector> &relativeTranslations, - std::mt19937 *rng) const; + std::mt19937 *rng, const Values &initialValues = Values()) const; /** - * @brief Version of initializeRandomly with a fixed seed. Uses initial values - * from params if provided. + * @brief Version of initializeRandomly with a fixed seed. * * @param relativeTranslations unit translation directions between * translations to be estimated + * @param initialValues (optional) initial values from a prior * @return Values */ Values initializeRandomly( - const std::vector> &relativeTranslations) const; + const std::vector> &relativeTranslations, + const Values &initialValues = Values()) const; /** * @brief Build and optimize factor graph. @@ -146,12 +142,14 @@ class TranslationRecovery { * The scale is only used if betweenTranslations is empty. * @param betweenTranslations relative translations (with scale) between 2 * points in world coordinate frame known a priori. + * @param initialValues intial values for optimization. Initializes randomly + * if not provided. * @return Values */ - Values run(const TranslationEdges &relativeTranslations, - const double scale = 1.0, - const std::vector> &betweenTranslations = - {}) const; + Values run( + const TranslationEdges &relativeTranslations, const double scale = 1.0, + const std::vector> &betweenTranslations = {}, + const Values &initialValues = Values()) const; /** * @brief Simulate translation direction measurements diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 122a9b5a6..33f23614a 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -262,14 +262,9 @@ class MFAS { }; #include -class TranslationRecoveryParams { - gtsam::LevenbergMarquardtParams lmParams; - gtsam::Values initial; - TranslationRecoveryParams(); -}; class TranslationRecovery { - TranslationRecovery(const gtsam::TranslationRecoveryParams& lmParams); + TranslationRecovery(const gtsam::LevenbergMarquardtParams& lmParams); TranslationRecovery(); // default params. void addPrior(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, const double scale, @@ -282,6 +277,11 @@ class TranslationRecovery { gtsam::NonlinearFactorGraph @graph) const; gtsam::NonlinearFactorGraph buildGraph( const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const; + gtsam::Values run(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + const double scale, + const gtsam::BinaryMeasurementsPoint3& betweenTranslations, + const gtsam::Values& initialValues) const; + // default random initial values gtsam::Values run( const gtsam::BinaryMeasurementsUnit3& relativeTranslations, const double scale, diff --git a/python/gtsam/tests/test_TranslationRecovery.py b/python/gtsam/tests/test_TranslationRecovery.py index 2875e897e..0908f0a69 100644 --- a/python/gtsam/tests/test_TranslationRecovery.py +++ b/python/gtsam/tests/test_TranslationRecovery.py @@ -46,10 +46,8 @@ class TestTranslationRecovery(unittest.TestCase): # Set verbosity to Silent for tests lmParams = gtsam.LevenbergMarquardtParams() lmParams.setVerbosityLM("silent") - params = gtsam.TranslationRecoveryParams() - params.lmParams = lmParams - algorithm = gtsam.TranslationRecovery(params) + algorithm = gtsam.TranslationRecovery(lmParams) scale = 2.0 result = algorithm.run(measurements, scale) From 50b77ceb551b60a35b2fd8bc272b753b9edb506d Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 10 May 2022 08:36:16 -0700 Subject: [PATCH 31/46] fixing merge conflict in wrapper --- gtsam/sfm/sfm.i | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index df0cb279b..83bd07b13 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -104,6 +104,7 @@ class BinaryMeasurementsPoint3 { size_t size() const; gtsam::BinaryMeasurement at(size_t idx) const; void push_back(const gtsam::BinaryMeasurement& measurement); +}; class BinaryMeasurementsRot3 { BinaryMeasurementsRot3(); From 29787373a6ec40cbb526eed1f3616fb892bdd621 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 10 May 2022 17:46:52 -0700 Subject: [PATCH 32/46] update python test constructor --- python/gtsam/tests/test_TranslationRecovery.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_TranslationRecovery.py b/python/gtsam/tests/test_TranslationRecovery.py index 0908f0a69..99fbce89e 100644 --- a/python/gtsam/tests/test_TranslationRecovery.py +++ b/python/gtsam/tests/test_TranslationRecovery.py @@ -36,7 +36,7 @@ class TestTranslationRecovery(unittest.TestCase): """Construct from binary measurements.""" algorithm = gtsam.TranslationRecovery() self.assertIsInstance(algorithm, gtsam.TranslationRecovery) - algorithm_params = gtsam.TranslationRecovery(gtsam.TranslationRecoveryParams()) + algorithm_params = gtsam.TranslationRecovery(gtsam.LevenbergMarquardtParams()) self.assertIsInstance(algorithm_params, gtsam.TranslationRecovery) def test_run(self): From de1827ca40b100cd08bfdb69341e9e7d44ed10a4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 May 2022 22:50:10 -0400 Subject: [PATCH 33/46] Added notebook with SenselessDev code. --- python/gtsam/notebooks/ellipses.ipynb | 55 +++++++++++++-------------- python/gtsam/utils/plot.py | 38 ++++++------------ 2 files changed, 37 insertions(+), 56 deletions(-) diff --git a/python/gtsam/notebooks/ellipses.ipynb b/python/gtsam/notebooks/ellipses.ipynb index d1ce9a015..06938f696 100644 --- a/python/gtsam/notebooks/ellipses.ipynb +++ b/python/gtsam/notebooks/ellipses.ipynb @@ -13,17 +13,18 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 5, "metadata": {}, "outputs": [], "source": [ "import scipy\n", - "import scipy.stats" + "import scipy.stats\n", + "import numpy as np" ] }, { "cell_type": "code", - "execution_count": 9, + "execution_count": 6, "metadata": {}, "outputs": [], "source": [ @@ -36,7 +37,7 @@ }, { "cell_type": "code", - "execution_count": 23, + "execution_count": 7, "metadata": {}, "outputs": [ { @@ -51,17 +52,17 @@ } ], "source": [ - "for dim in range(0, 4):\n", - " print(\"{}D\".format(dim), end=\"\")\n", + "for dof in range(0, 4):\n", + " print(\"{}D\".format(dof), end=\"\")\n", " for sigma in range(1, 6):\n", - " if dim == 0: print(\"\\t {} \".format(sigma), end=\"\")\n", - " else: print(\"\\t{:.5f}%\".format(sigma_to_pct(sigma, dim)), end=\"\")\n", + " if dof == 0: print(\"\\t {} \".format(sigma), end=\"\")\n", + " else: print(\"\\t{:.5f}%\".format(sigma_to_pct(sigma, dof)), end=\"\")\n", " print()" ] }, { "cell_type": "code", - "execution_count": 29, + "execution_count": 12, "metadata": {}, "outputs": [ { @@ -70,34 +71,30 @@ "text": [ "1D\n", "\n", - "sigma=1.0 -> 68.26895%\n", - "sigma=2.0 -> 95.44997%\n", - "sigma=2.5 -> 98.75807%\n", - "sigma=5.0 -> 99.99994%\n", + "pct=50.0 -> sigma=0.674489750196\n", + "pct=95.0 -> sigma=1.959963984540\n", + "pct=99.0 -> sigma=2.575829303549\n", "\n", "2D\n", "\n", - "sigma=1.0 -> 39.34693%\n", - "sigma=2.0 -> 86.46647%\n", - "sigma=2.5 -> 95.60631%\n", - "sigma=5.0 -> 99.99963%\n", + "pct=50.0 -> sigma=1.177410022515\n", + "pct=95.0 -> sigma=2.447746830681\n", + "pct=99.0 -> sigma=3.034854258770\n", "\n", "3D\n", "\n", - "sigma=1.0 -> 19.87480%\n", - "sigma=2.0 -> 73.85359%\n", - "sigma=2.5 -> 89.99392%\n", - "sigma=5.0 -> 99.99846%\n", + "pct=50.0 -> sigma=1.538172254455\n", + "pct=95.0 -> sigma=2.795483482915\n", + "pct=99.0 -> sigma=3.368214175219\n", "\n" ] } ], "source": [ - "for dim in range(1, 4):\n", - " print(\"{}D\\n\".format(dim))\n", - " for sigma in [1, 2, 2.5, 5]:\n", - " print(\"sigma={:.1f} -> {:.5f}%\".format(sigma, sigma_to_pct(sigma, dim)), end=\"\")\n", - " print()\n", + "for dof in range(1, 4):\n", + " print(\"{}D\\n\".format(dof))\n", + " for pct in [50, 95, 99]:\n", + " print(\"pct={:.1f} -> sigma={:.12f}\".format(pct, pct_to_sigma(pct, dof)))\n", " print()" ] }, @@ -111,10 +108,10 @@ ], "metadata": { "interpreter": { - "hash": "341996cd3f3db7b5e0d1eaea072c5502d80452314e72e6b77c40445f6e9ba101" + "hash": "4d608302ba82e7596903db5446e6fa05f049271852e8cc6e1cafaafe5fbd9fed" }, "kernelspec": { - "display_name": "Python 3 (ipykernel)", + "display_name": "Python 3.8.13 ('gtsfm-v1')", "language": "python", "name": "python3" }, @@ -128,7 +125,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.10.4" + "version": "3.8.13" } }, "nbformat": 4, diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index db78135b7..a4d19f72b 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -13,41 +13,25 @@ import gtsam from gtsam import Marginals, Point2, Point3, Pose2, Pose3, Values -# For translation between a scaling of the uncertainty ellipse and the percentage of -# inliers see discussion in https://github.com/borglab/gtsam/pull/1067 -# -# Both directions can be calculated by the following functions: -# def pct_to_sigma(pct, dof): -# return np.sqrt(scipy.stats.chi2.ppf(pct / 100., df=dof)) -# -# def sigma_to_pct(sigma, dof): -# return scipy.stats.chi2.cdf(sigma**2, df=dof) * 100. +# For translation between a scaling of the uncertainty ellipse and the +# percentage of inliers see discussion in +# [PR 1067](https://github.com/borglab/gtsam/pull/1067) +# and the notebook python/gtsam/notebooks/ellipses.ipynb (needs scipy). # # In the following, the default scaling is chosen for 95% inliers, which # translates to the following sigma values: -# 2D: pct_to_sigma(95, 2) -> 2.447746830680816 -# 3D: pct_to_sigma(95, 3) -> 2.7954834829151074 +# 1D: 1.959963984540 +# 2D: 2.447746830681 +# 3D: 2.795483482915 # # Further references are Stochastic Models, Estimation, and Control Vol 1 by Maybeck, -# page 366 and https://www.xarg.org/2018/04/how-to-plot-a-covariance-error-ellipse/ +# page 366 and https://www.xarg.org/2018/04/how-to-plot-a-covariance-error-ellipse/ # # For reference, here are the inlier percentages for some sigma values: # 1 2 3 4 5 # 1D 68.26895 95.44997 99.73002 99.99367 99.99994 # 2D 39.34693 86.46647 98.88910 99.96645 99.99963 # 3D 19.87480 73.85359 97.07091 99.88660 99.99846 -# -# This can be generated by: -# for dim in range(0, 4): -# print("{}D".format(dim), end="") -# for n_sigma in range(1, 6): -# if dim == 0: print("\t {} ".format(n_sigma), end="") -# else: print("\t{:.5f}".format(sigma_to_pct(n_sigma, dim)), end="") -# print() -# -# The translation routines are not included in GTSAM, because it would introduce -# scipy as a new dependency. - def set_axes_equal(fignum: int) -> None: """ @@ -123,7 +107,7 @@ def plot_covariance_ellipse_3d(axes, alpha: Transparency value for the plotted surface in the range [0, 1]. """ # this corresponds to 95%, see note above - k = 2.7954834829151074 + k = 2.795483482915 U, S, _ = np.linalg.svd(P) radii = k * np.sqrt(S) @@ -160,10 +144,10 @@ def plot_covariance_ellipse_2d(axes, which will be represented as an ellipse. """ - w, v = np.linalg.eig(covariance) + w, v = np.linalg.eigh(covariance) # this corresponds to 95%, see note above - k = 2.447746830680816 + k = 2.447746830681 angle = np.arctan2(v[1, 0], v[0, 0]) # We multiply k by 2 since k corresponds to the radius but Ellipse uses From b5e9dc04fd478c12bca67ba02f0b6e8216958db8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 May 2022 22:56:55 -0400 Subject: [PATCH 34/46] Make status a public field --- gtsam/geometry/triangulation.h | 66 ++++++++++++++-------------------- 1 file changed, 26 insertions(+), 40 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 49b5aef04..401fd2d0b 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -510,18 +511,18 @@ private: }; /** - * TriangulationResult is an optional point, along with the reasons why it is invalid. + * TriangulationResult is an optional point, along with the reasons why it is + * invalid. */ -class TriangulationResult: public boost::optional { - enum Status { - VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT - }; - Status status_; - TriangulationResult(Status s) : - status_(s) { - } -public: +class TriangulationResult : public boost::optional { + public: + enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT }; + Status status; + private: + TriangulationResult(Status s) : status(s) {} + + public: /** * Default constructor, only for serialization */ @@ -530,54 +531,38 @@ public: /** * Constructor */ - TriangulationResult(const Point3& p) : - status_(VALID) { - reset(p); - } + TriangulationResult(const Point3& p) : status(VALID) { reset(p); } static TriangulationResult Degenerate() { return TriangulationResult(DEGENERATE); } - static TriangulationResult Outlier() { - return TriangulationResult(OUTLIER); - } + static TriangulationResult Outlier() { return TriangulationResult(OUTLIER); } static TriangulationResult FarPoint() { return TriangulationResult(FAR_POINT); } static TriangulationResult BehindCamera() { return TriangulationResult(BEHIND_CAMERA); } - bool valid() const { - return status_ == VALID; - } - bool degenerate() const { - return status_ == DEGENERATE; - } - bool outlier() const { - return status_ == OUTLIER; - } - bool farPoint() const { - return status_ == FAR_POINT; - } - bool behindCamera() const { - return status_ == BEHIND_CAMERA; - } + bool valid() const { return status == VALID; } + bool degenerate() const { return status == DEGENERATE; } + bool outlier() const { return status == OUTLIER; } + bool farPoint() const { return status == FAR_POINT; } + bool behindCamera() const { return status == BEHIND_CAMERA; } // stream to output - friend std::ostream &operator<<(std::ostream &os, - const TriangulationResult& result) { + friend std::ostream& operator<<(std::ostream& os, + const TriangulationResult& result) { if (result) os << "point = " << *result << std::endl; else - os << "no point, status = " << result.status_ << std::endl; + os << "no point, status = " << result.status << std::endl; return os; } -private: - + private: /// Serialization function friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(status_); + template + void serialize(ARCHIVE& ar, const unsigned int version) { + ar& BOOST_SERIALIZATION_NVP(status); } }; @@ -644,6 +629,7 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, // Vector of Cameras - used by the Python/MATLAB wrapper using CameraSetCal3Bundler = CameraSet>; using CameraSetCal3_S2 = CameraSet>; +using CameraSetCal3DS2 = CameraSet>; using CameraSetCal3Fisheye = CameraSet>; using CameraSetCal3Unified = CameraSet>; using CameraSetSpherical = CameraSet; From 58cbdcddd52b8fb81f9f5fe2f4b9a3c310529d58 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 May 2022 22:57:38 -0400 Subject: [PATCH 35/46] Wrap TriangulationResult --- gtsam/geometry/geometry.i | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 0dc23c160..8d70fb679 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1089,6 +1089,11 @@ class StereoCamera { #include +virtual class TriangulationResult : boost::optional { + enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT }; + Status status; +}; + // Templates appear not yet supported for free functions - issue raised at // borglab/wrap#14 to add support gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, From e60f4ac0c6c426617ed2be875d8d64eee195e869 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 May 2022 22:58:07 -0400 Subject: [PATCH 36/46] Be consistent in wrapping templated triangulation versions --- gtsam/geometry/geometry.i | 89 +++++++++++++++++++++++++++++---------- 1 file changed, 66 insertions(+), 23 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 8d70fb679..c297ef82e 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1096,56 +1096,99 @@ virtual class TriangulationResult : boost::optional { // Templates appear not yet supported for free functions - issue raised at // borglab/wrap#14 to add support + +// Cal3_S2 versions gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, const gtsam::SharedNoiseModel& model = nullptr); -gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, - gtsam::Cal3DS2* sharedCal, - const gtsam::Point2Vector& measurements, - double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr); -gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, - gtsam::Cal3Bundler* sharedCal, - const gtsam::Point2Vector& measurements, - double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr); gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, const gtsam::SharedNoiseModel& model = nullptr); +gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, + gtsam::Cal3_S2* sharedCal, + const gtsam::Point2Vector& measurements, + const gtsam::Point3& initialEstimate); +gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3_S2& cameras, + const gtsam::Point2Vector& measurements, + const gtsam::Point3& initialEstimate); + +// Cal3DS2 versions +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3DS2* sharedCal, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize, + const gtsam::SharedNoiseModel& model = nullptr); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3DS2& cameras, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize, + const gtsam::SharedNoiseModel& model = nullptr); +gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, + gtsam::Cal3DS2* sharedCal, + const gtsam::Point2Vector& measurements, + const gtsam::Point3& initialEstimate); +gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3DS2& cameras, + const gtsam::Point2Vector& measurements, + const gtsam::Point3& initialEstimate); + +// Cal3Bundler versions +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3Bundler* sharedCal, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize, + const gtsam::SharedNoiseModel& model = nullptr); gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, const gtsam::SharedNoiseModel& model = nullptr); +gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, + gtsam::Cal3Bundler* sharedCal, + const gtsam::Point2Vector& measurements, + const gtsam::Point3& initialEstimate); +gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Bundler& cameras, + const gtsam::Point2Vector& measurements, + const gtsam::Point3& initialEstimate); + +// Cal3Fisheye versions +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3Fisheye* sharedCal, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize, + const gtsam::SharedNoiseModel& model = nullptr); gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, const gtsam::SharedNoiseModel& model = nullptr); +gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, + gtsam::Cal3Fisheye* sharedCal, + const gtsam::Point2Vector& measurements, + const gtsam::Point3& initialEstimate); +gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Fisheye& cameras, + const gtsam::Point2Vector& measurements, + const gtsam::Point3& initialEstimate); + +// Cal3Unified versions +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3Unified* sharedCal, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize, + const gtsam::SharedNoiseModel& model = nullptr); gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, const gtsam::SharedNoiseModel& model = nullptr); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, - gtsam::Cal3_S2* sharedCal, + gtsam::Cal3Unified* sharedCal, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate); -gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, - gtsam::Cal3DS2* sharedCal, - const gtsam::Point2Vector& measurements, - const gtsam::Point3& initialEstimate); -gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, - gtsam::Cal3Bundler* sharedCal, - const gtsam::Point2Vector& measurements, - const gtsam::Point3& initialEstimate); -gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3_S2& cameras, - const gtsam::Point2Vector& measurements, - const gtsam::Point3& initialEstimate); -gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Bundler& cameras, +gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Unified& cameras, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate); + + #include template class BearingRange { From 02828e2ec7aa62a117341c641de64e9bcc76f57f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 May 2022 23:11:40 -0400 Subject: [PATCH 37/46] Fix unrelated warning --- gtsam/linear/SubgraphBuilder.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp index 18e19cd20..de7ae7060 100644 --- a/gtsam/linear/SubgraphBuilder.cpp +++ b/gtsam/linear/SubgraphBuilder.cpp @@ -337,7 +337,6 @@ vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, DSFVector dsf(n); size_t count = 0; - double sum = 0.0; for (const size_t index : sortedIndices) { const GaussianFactor &gf = *gfg[index]; const auto keys = gf.keys(); @@ -347,7 +346,6 @@ vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, if (dsf.find(u) != dsf.find(v)) { dsf.merge(u, v); treeIndices.push_back(index); - sum += weights[index]; if (++count == n - 1) break; } } From 8333c8f15c9a081a1a993dcce9b25d2b30afa921 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 11 May 2022 00:04:42 -0400 Subject: [PATCH 38/46] Wrapped safe triangulation versions --- gtsam/geometry/geometry.i | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index c297ef82e..96249b6e0 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1094,6 +1094,19 @@ virtual class TriangulationResult : boost::optional { Status status; }; +class TriangulationParameters { + double rankTolerance; + bool enableEPI; + double landmarkDistanceThreshold; + double dynamicOutlierRejectionThreshold; + SharedNoiseModel noiseModel; + TriangulationParameters(const double rankTolerance = 1.0, + const bool enableEPI = false, + double landmarkDistanceThreshold = -1, + double dynamicOutlierRejectionThreshold = -1, + const gtsam::SharedNoiseModel& noiseModel = nullptr); +}; + // Templates appear not yet supported for free functions - issue raised at // borglab/wrap#14 to add support @@ -1114,6 +1127,10 @@ gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3_S2& cameras, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate); +gtsam::TriangulationResult triangulateSafe( + const gtsam::CameraSetCal3_S2& cameras, + const gtsam::Point2Vector& measurements, + const gtsam::TriangulationParameters& params); // Cal3DS2 versions gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, @@ -1132,6 +1149,10 @@ gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3DS2& cameras, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate); +gtsam::TriangulationResult triangulateSafe( + const gtsam::CameraSetCal3DS2& cameras, + const gtsam::Point2Vector& measurements, + const gtsam::TriangulationParameters& params); // Cal3Bundler versions gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, @@ -1150,6 +1171,10 @@ gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Bundler& cameras, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate); +gtsam::TriangulationResult triangulateSafe( + const gtsam::CameraSetCal3Bundler& cameras, + const gtsam::Point2Vector& measurements, + const gtsam::TriangulationParameters& params); // Cal3Fisheye versions gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, @@ -1168,6 +1193,10 @@ gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Fisheye& cameras, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate); +gtsam::TriangulationResult triangulateSafe( + const gtsam::CameraSetCal3Fisheye& cameras, + const gtsam::Point2Vector& measurements, + const gtsam::TriangulationParameters& params); // Cal3Unified versions gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, @@ -1186,6 +1215,10 @@ gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Unified& cameras, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate); +gtsam::TriangulationResult triangulateSafe( + const gtsam::CameraSetCal3Unified& cameras, + const gtsam::Point2Vector& measurements, + const gtsam::TriangulationParameters& params); From 287ea289209b6cee9c9dbb6d141fb46aa2d5ee98 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 11 May 2022 00:27:50 -0400 Subject: [PATCH 39/46] Camera sets --- gtsam/geometry/geometry.i | 10 +++++++--- python/gtsam/specializations/geometry.h | 2 ++ 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 96249b6e0..1e9444b02 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1040,7 +1040,11 @@ class Similarity3 { double scale() const; }; -template +template class CameraSet { CameraSet(); @@ -1088,10 +1092,10 @@ class StereoCamera { }; #include - -virtual class TriangulationResult : boost::optional { +class TriangulationResult { enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT }; Status status; + const gtsam::Point3& get() const; }; class TriangulationParameters { diff --git a/python/gtsam/specializations/geometry.h b/python/gtsam/specializations/geometry.h index 5a0ea35ef..99f40253f 100644 --- a/python/gtsam/specializations/geometry.h +++ b/python/gtsam/specializations/geometry.h @@ -21,6 +21,8 @@ py::bind_vector>(m_, "Pose3Pairs"); py::bind_vector>(m_, "Pose3Vector"); py::bind_vector>>( m_, "CameraSetCal3_S2"); +py::bind_vector>>( + m_, "CameraSetCal3DS2"); py::bind_vector>>( m_, "CameraSetCal3Bundler"); py::bind_vector>>( From db371c2fdb0ff463049ebee3f7574da4e98e9136 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 11 May 2022 00:28:01 -0400 Subject: [PATCH 40/46] Added safe triangulation tests --- python/gtsam/tests/test_Triangulation.py | 83 +++++++++++++++++++----- 1 file changed, 68 insertions(+), 15 deletions(-) diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index 0a258a0af..c079e76b8 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -8,26 +8,17 @@ See LICENSE for the license information Test Triangulation Authors: Frank Dellaert & Fan Jiang (Python) & Sushmita Warrier & John Lambert """ +# pylint: disable=no-name-in-module, invalid-name, no-member import unittest from typing import Iterable, List, Optional, Tuple, Union - import numpy as np import gtsam -from gtsam import ( - Cal3_S2, - Cal3Bundler, - CameraSetCal3_S2, - CameraSetCal3Bundler, - PinholeCameraCal3_S2, - PinholeCameraCal3Bundler, - Point2, - Point2Vector, - Point3, - Pose3, - Pose3Vector, - Rot3, -) +from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2, + CameraSetCal3Bundler, PinholeCameraCal3_S2, + PinholeCameraCal3Bundler, Point2, Point2Vector, Point3, + Pose3, Pose3Vector, Rot3, TriangulationParameters, + TriangulationResult) from gtsam.utils.test_case import GtsamTestCase UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2) @@ -218,6 +209,68 @@ class TestTriangulationExample(GtsamTestCase): # using the Huber loss we now have a quite small error!! nice! self.assertTrue(np.allclose(landmark, actual4, atol=0.05)) + def test_outliers_and_far_landmarks(self) -> None: + """Check safe triangulation function.""" + pose1, pose2 = self.poses + + K1 = Cal3_S2(1500, 1200, 0, 640, 480) + # create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + camera1 = PinholeCameraCal3_S2(pose1, K1) + + # create second camera 1 meter to the right of first camera + K2 = Cal3_S2(1600, 1300, 0, 650, 440) + camera2 = PinholeCameraCal3_S2(pose2, K2) + + # 1. Project two landmarks into two cameras and triangulate + z1 = camera1.project(self.landmark) + z2 = camera2.project(self.landmark) + + cameras = CameraSetCal3_S2() + measurements = Point2Vector() + + cameras.push_back(camera1) + cameras.push_back(camera2) + measurements.push_back(z1) + measurements.push_back(z2) + + landmarkDistanceThreshold = 10 # landmark is closer than that + # all default except landmarkDistanceThreshold: + params = TriangulationParameters(1.0, False, landmarkDistanceThreshold) + actual: TriangulationResult = gtsam.triangulateSafe( + cameras, measurements, params) + self.gtsamAssertEquals(actual, self.landmark, 1e-2) + self.assertTrue(actual.valid()) + + landmarkDistanceThreshold = 4 # landmark is farther than that + params2 = TriangulationParameters( + 1.0, False, landmarkDistanceThreshold) + actual = gtsam.triangulateSafe(cameras, measurements, params2) + self.assertTrue(actual.farPoint()) + + # 3. Add a slightly rotated third camera above with a wrong measurement + # (OUTLIER) + pose3 = pose1 * Pose3(Rot3.Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)) + K3 = Cal3_S2(700, 500, 0, 640, 480) + camera3 = PinholeCameraCal3_S2(pose3, K3) + z3 = camera3.project(self.landmark) + + cameras.push_back(camera3) + measurements.push_back(z3 + Point2(10, -10)) + + landmarkDistanceThreshold = 10 # landmark is closer than that + outlierThreshold = 100 # loose, the outlier is going to pass + params3 = TriangulationParameters(1.0, False, landmarkDistanceThreshold, + outlierThreshold) + actual = gtsam.triangulateSafe(cameras, measurements, params3) + self.assertTrue(actual.valid()) + + # now set stricter threshold for outlier rejection + outlierThreshold = 5 # tighter, the outlier is not going to pass + params4 = TriangulationParameters(1.0, False, landmarkDistanceThreshold, + outlierThreshold) + actual = gtsam.triangulateSafe(cameras, measurements, params4) + self.assertTrue(actual.outlier()) + if __name__ == "__main__": unittest.main() From 7bd0ae799de8d6ce52ff3edf346dfa5a9a3bd11a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 11 May 2022 10:11:25 -0400 Subject: [PATCH 41/46] Fix pybind11 issue --- gtsam/geometry/geometry.i | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 1e9444b02..d2771edbd 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1040,11 +1040,7 @@ class Similarity3 { double scale() const; }; -template +template class CameraSet { CameraSet(); From b78a4e64b19d10c989de67cc8b151d35d19d8a87 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 11 May 2022 10:11:57 -0400 Subject: [PATCH 42/46] Include all constructors, methods --- gtsam/geometry/geometry.i | 10 ++++++++++ python/gtsam/tests/test_Triangulation.py | 14 +++++++------- 2 files changed, 17 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index d2771edbd..8e3c93224 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1091,7 +1091,17 @@ class StereoCamera { class TriangulationResult { enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT }; Status status; + TriangulationResult(const gtsam::Point3& p); const gtsam::Point3& get() const; + static TriangulationResult Degenerate(); + static TriangulationResult Outlier(); + static TriangulationResult FarPoint(); + static TriangulationResult BehindCamera(); + bool valid() const; + bool degenerate() const; + bool outlier() const; + bool farPoint() const; + bool behindCamera() const; }; class TriangulationParameters { diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index c079e76b8..8630e1da7 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -228,17 +228,17 @@ class TestTriangulationExample(GtsamTestCase): cameras = CameraSetCal3_S2() measurements = Point2Vector() - cameras.push_back(camera1) - cameras.push_back(camera2) - measurements.push_back(z1) - measurements.push_back(z2) + cameras.append(camera1) + cameras.append(camera2) + measurements.append(z1) + measurements.append(z2) landmarkDistanceThreshold = 10 # landmark is closer than that # all default except landmarkDistanceThreshold: params = TriangulationParameters(1.0, False, landmarkDistanceThreshold) actual: TriangulationResult = gtsam.triangulateSafe( cameras, measurements, params) - self.gtsamAssertEquals(actual, self.landmark, 1e-2) + self.gtsamAssertEquals(actual.get(), self.landmark, 1e-2) self.assertTrue(actual.valid()) landmarkDistanceThreshold = 4 # landmark is farther than that @@ -254,8 +254,8 @@ class TestTriangulationExample(GtsamTestCase): camera3 = PinholeCameraCal3_S2(pose3, K3) z3 = camera3.project(self.landmark) - cameras.push_back(camera3) - measurements.push_back(z3 + Point2(10, -10)) + cameras.append(camera3) + measurements.append(z3 + Point2(10, -10)) landmarkDistanceThreshold = 10 # landmark is closer than that outlierThreshold = 100 # loose, the outlier is going to pass From 1d67019f54004b87493cf0a7437300fd5e464ad6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 11 May 2022 12:06:11 -0400 Subject: [PATCH 43/46] remove user flag for pip install --- python/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 05606c584..c14f02dda 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -188,7 +188,7 @@ endif() # Add custom target so we can install with `make python-install` set(GTSAM_PYTHON_INSTALL_TARGET python-install) add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} - COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . + COMMAND ${PYTHON_EXECUTABLE} -m pip install . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) From dab30890032ae829fa384dff2ad86a0f819342ba Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 11 May 2022 12:06:33 -0400 Subject: [PATCH 44/46] include patch version for Python interpreter to help disambiguate --- cmake/HandlePython.cmake | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/cmake/HandlePython.cmake b/cmake/HandlePython.cmake index 0c24824bc..16215986b 100644 --- a/cmake/HandlePython.cmake +++ b/cmake/HandlePython.cmake @@ -16,6 +16,7 @@ if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX) set(Python_VERSION_MAJOR ${PYTHON_VERSION_MAJOR}) set(Python_VERSION_MINOR ${PYTHON_VERSION_MINOR}) + set(Python_VERSION_PATCH ${PYTHON_VERSION_PATCH}) set(Python_EXECUTABLE ${PYTHON_EXECUTABLE}) else() @@ -31,11 +32,12 @@ if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX) set(Python_VERSION_MAJOR ${Python3_VERSION_MAJOR}) set(Python_VERSION_MINOR ${Python3_VERSION_MINOR}) + set(Python_VERSION_PATCH ${Python3_VERSION_PATCH}) endif() set(GTSAM_PYTHON_VERSION - "${Python_VERSION_MAJOR}.${Python_VERSION_MINOR}" + "${Python_VERSION_MAJOR}.${Python_VERSION_MINOR}.${Python_VERSION_PATCH}" CACHE STRING "The version of Python to build the wrappers against." FORCE) From 78c7a6b72cb011475e0aadeaabfe2acf247d4481 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 13 May 2022 10:24:49 -0400 Subject: [PATCH 45/46] Change `optimize_using` to simpler function call --- python/gtsam/tests/test_logging_optimizer.py | 10 ++--- python/gtsam/utils/logging_optimizer.py | 47 +++++++++++--------- 2 files changed, 30 insertions(+), 27 deletions(-) diff --git a/python/gtsam/tests/test_logging_optimizer.py b/python/gtsam/tests/test_logging_optimizer.py index c58e4f121..602aeffc9 100644 --- a/python/gtsam/tests/test_logging_optimizer.py +++ b/python/gtsam/tests/test_logging_optimizer.py @@ -66,9 +66,9 @@ class TestOptimizeComet(GtsamTestCase): # Wrapper function sets the hook and calls optimizer.optimize() for us. params = gtsam.GaussNewtonParams() - actual = optimize_using(gtsam.GaussNewtonOptimizer, hook)(self.graph, self.initial) + actual = optimize_using(gtsam.GaussNewtonOptimizer, hook, self.graph, self.initial) self.check(actual) - actual = optimize_using(gtsam.GaussNewtonOptimizer, hook)(self.graph, self.initial, params) + actual = optimize_using(gtsam.GaussNewtonOptimizer, hook, self.graph, self.initial, params) self.check(actual) actual = gtsam_optimize(gtsam.GaussNewtonOptimizer(self.graph, self.initial, params), params, hook) @@ -80,10 +80,10 @@ class TestOptimizeComet(GtsamTestCase): print(error) params = gtsam.LevenbergMarquardtParams() - actual = optimize_using(gtsam.LevenbergMarquardtOptimizer, hook)(self.graph, self.initial) + actual = optimize_using(gtsam.LevenbergMarquardtOptimizer, hook, self.graph, self.initial) self.check(actual) - actual = optimize_using(gtsam.LevenbergMarquardtOptimizer, hook)(self.graph, self.initial, - params) + actual = optimize_using(gtsam.LevenbergMarquardtOptimizer, hook, self.graph, self.initial, + params) self.check(actual) actual = gtsam_optimize(gtsam.LevenbergMarquardtOptimizer(self.graph, self.initial, params), params, hook) diff --git a/python/gtsam/utils/logging_optimizer.py b/python/gtsam/utils/logging_optimizer.py index f89208bc5..fe2f717d8 100644 --- a/python/gtsam/utils/logging_optimizer.py +++ b/python/gtsam/utils/logging_optimizer.py @@ -17,39 +17,42 @@ OPTIMIZER_PARAMS_MAP = { } -def optimize_using(OptimizerClass, hook) -> Callable[[Any], gtsam.Values]: +def optimize_using(OptimizerClass, hook, *args) -> gtsam.Values: """ Wraps the constructor and "optimize()" call for an Optimizer together and adds an iteration hook. Example usage: - solution = optimize_using(gtsam.GaussNewtonOptimizer, hook)(graph, init, params) + ```python + def hook(optimizer, error): + print("iteration {:}, error = {:}".format(optimizer.iterations(), error)) + solution = optimize_using(gtsam.GaussNewtonOptimizer, hook, graph, init, params) + ``` + Iteration hook's args are (optimizer, error) and return type should be None Args: OptimizerClass (T): A NonlinearOptimizer class (e.g. GaussNewtonOptimizer, - LevenbergMarquadrtOptimizer) + LevenbergMarquardtOptimizer) hook ([T, double] -> None): Function to callback after each iteration. Args are (optimizer, error) and return should be None. + *args: Arguments that would be passed into the OptimizerClass constructor, usually: + graph, init, [params] Returns: - (Callable[*, gtsam.Values]): Call the returned function with the usual NonlinearOptimizer - arguments (will be forwarded to constructor) and it will return a Values object - representing the solution. See example usage above. + (gtsam.Values): A Values object representing the optimization solution. """ - - def wrapped_optimize(*args): - for arg in args: - if isinstance(arg, gtsam.NonlinearOptimizerParams): - arg.iterationHook = lambda iteration, error_before, error_after: hook( - optimizer, error_after) - break - else: - params = OPTIMIZER_PARAMS_MAP[OptimizerClass]() - params.iterationHook = lambda iteration, error_before, error_after: hook( + # Add the iteration hook to the NonlinearOptimizerParams + for arg in args: + if isinstance(arg, gtsam.NonlinearOptimizerParams): + arg.iterationHook = lambda iteration, error_before, error_after: hook( optimizer, error_after) - args = (*args, params) - optimizer = OptimizerClass(*args) - hook(optimizer, optimizer.error()) - return optimizer.optimize() - - return wrapped_optimize + break + else: + params = OPTIMIZER_PARAMS_MAP[OptimizerClass]() + params.iterationHook = lambda iteration, error_before, error_after: hook( + optimizer, error_after) + args = (*args, params) + # Construct Optimizer and optimize + optimizer = OptimizerClass(*args) + hook(optimizer, optimizer.error()) # Call hook once with init values to match behavior below + return optimizer.optimize() def optimize(optimizer, check_convergence, hook): From 30412b8110f4b5479f3a4665f8f1fdd6ec91b910 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 13 May 2022 14:33:04 -0400 Subject: [PATCH 46/46] Bump version --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index cfb251663..74433f333 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ endif() set (GTSAM_VERSION_MAJOR 4) set (GTSAM_VERSION_MINOR 2) set (GTSAM_VERSION_PATCH 0) -set (GTSAM_PRERELEASE_VERSION "a6") +set (GTSAM_PRERELEASE_VERSION "a7") math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") if (${GTSAM_VERSION_PATCH} EQUAL 0)