diff --git a/CMakeLists.txt b/CMakeLists.txt index 834ce732e..186dabaf4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,8 +61,8 @@ option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the defau option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF) option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) -option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" ON) -option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" ON) +option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF) +option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF) option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" OFF) option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON) @@ -564,10 +564,10 @@ if(GTSAM_WITH_TBB AND NOT TBB_FOUND) message(WARNING "TBB was not found - this is ok, but note that GTSAM parallelization will be disabled. Set GTSAM_WITH_TBB to 'Off' to avoid this warning.") endif() if(GTSAM_WITH_EIGEN_MKL AND NOT MKL_FOUND) - message(WARNING "MKL was not found - this is ok, but note that MKL yields better performance. Set GTSAM_WITH_EIGEN_MKL to 'Off' to disable this warning.") + message(WARNING "MKL was not found - this is ok, but note that MKL will be disabled. Set GTSAM_WITH_EIGEN_MKL to 'Off' to disable this warning. See INSTALL.md for notes on performance.") endif() if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND) - message(WARNING "Your compiler does not support OpenMP - this is ok, but performance may be improved with OpenMP. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning.") + message(WARNING "Your compiler does not support OpenMP. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning. See INSTALL.md for notes on performance.") endif() if(GTSAM_BUILD_PYTHON AND GTSAM_PYTHON_WARNINGS) message(WARNING "${GTSAM_PYTHON_WARNINGS}") diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 366a58a09..a6cfee984 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -97,12 +97,24 @@ Note that in the Lie group case, the usual valid expressions for Retract and Loc For Lie groups, the `exponential map` above is the most obvious mapping: it associates straight lines in the tangent space with geodesics on the manifold -(and it's inverse, the log map). However, there are two cases in which we deviate from this: +(and it's inverse, the log map). However, there are several cases in which we deviate from this: However, the exponential map is unnecessarily expensive for use in optimization. Hence, in GTSAM there is the option to provide a cheaper chart by means of the `ChartAtOrigin` struct in a class. This is done for *SE(2)*, *SO(3)* and *SE(3)* (see `Pose2`, `Rot3`, `Pose3`) Most Lie groups we care about are *Matrix groups*, continuous sub-groups of *GL(n)*, the group of *n x n* invertible matrices. In this case, a lot of the derivatives calculations needed can be standardized, and this is done by the `LieGroup` superclass. You only need to provide an `AdjointMap` method. +A CRTP helper class `LieGroup` is available that can take a class and create some of the Lie group methods automatically. The class needs: + +* operator* : implements group operator +* inverse: implements group inverse +* AdjointMap: maps tangent vectors according to group element +* Expmap/Logmap: exponential map and its inverse +* ChartAtOrigin: struct where you define Retract/Local at origin + +To use, simply derive, but also say `using LieGroup::inverse` so you get an inverse with a derivative. + +Finally, to create the traits automatically you can use `internal::LieGroupTraits` + Vector Space ------------ diff --git a/INSTALL.md b/INSTALL.md index e19363c7e..3437d074d 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -23,18 +23,22 @@ $ make install disable the CMake flag GTSAM_WITH_TBB (enabled by default). On Ubuntu, TBB may be installed from the Ubuntu repositories, and for other platforms it may be downloaded from https://www.threadingbuildingblocks.org/ + - GTSAM may be configured to use MKL by toggling `GTSAM_WITH_EIGEN_MKL` and + `GTSAM_WITH_EIGEN_MKL_OPENMP` to `ON`; however, best performance is usually + achieved with MKL disabled. We therefore advise you to benchmark your problem + before using MKL. Tested compilers: - - GCC 4.2-4.7 - - OSX Clang 2.9-5.0 - - OSX GCC 4.2 - - MSVC 2010, 2012 + - GCC 4.2-7.3 + - OS X Clang 2.9-10.0 + - OS X GCC 4.2 + - MSVC 2010, 2012, 2017 Tested systems: - - Ubuntu 11.04 - 18.04 - - MacOS 10.6 - 10.9 + - Ubuntu 16.04 - 18.04 + - MacOS 10.6 - 10.14 - Windows 7, 8, 8.1, 10 Known issues: @@ -134,6 +138,27 @@ MEX_COMMAND: Path to the mex compiler. Defaults to assume the path is included i $MATLABROOT can be found by executing the command `matlabroot` in MATLAB +## Performance + +Here are some tips to get the best possible performance out of GTSAM. + +1. Build in `Release` mode. GTSAM will run up to 10x faster compared to `Debug` mode. +2. Enable TBB. On modern processors with multiple cores, this can easily speed up + optimization by 30-50%. Please note that this may not be true for very small + problems where the overhead of dispatching work to multiple threads outweighs + the benefit. We recommend that you benchmark your problem with/without TBB. +3. Add `-march=native` to `GTSAM_CMAKE_CXX_FLAGS`. A performance gain of + 25-30% can be expected on modern processors. Note that this affects the portability + of your executable. It may not run when copied to another system with older/different + processor architecture. + Also note that all dependent projects *must* be compiled with the same flag, or + seg-faults and other undefined behavior may result. +4. Possibly enable MKL. Please note that our benchmarks have shown that this helps only + in very limited cases, and actually hurts performance in the usual case. We therefore + recommend that you do *not* enable MKL, unless you have benchmarked it on + your problem and have verified that it improves performance. + + ## Debugging tips Another useful debugging symbol is _GLIBCXX_DEBUG, which enables debug checks and safe containers in the standard C++ library and makes problems much easier to find. diff --git a/THANKS b/THANKS index f84cfa185..f2b51f61d 100644 --- a/THANKS +++ b/THANKS @@ -1,5 +1,6 @@ GTSAM was made possible by the efforts of many collaborators at Georgia Tech, listed below with their current afffiliation, if they left Tech: +* Jeremy Aguilon, Facebook * Sungtae An * Doru Balcan, Bank of America * Chris Beall diff --git a/cmake/FindMKL.cmake b/cmake/FindMKL.cmake index cbe46a908..32e183baa 100644 --- a/cmake/FindMKL.cmake +++ b/cmake/FindMKL.cmake @@ -206,6 +206,15 @@ ELSEIF(MKL_ROOT_DIR) # UNIX and macOS ) ENDIF() + IF(NOT MKL_LAPACK_LIBRARY) + FIND_LIBRARY(MKL_LAPACK_LIBRARY + mkl_intel_lp64 + PATHS + ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} + ${MKL_ROOT_DIR}/lib/ + ) + ENDIF() + # iomp5 IF("${MKL_ARCH_DIR}" STREQUAL "32") IF(UNIX AND NOT APPLE) diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index e53c0b73e..a351ec52b 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -19,22 +19,25 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) # wrap gtsam_unstable if(GTSAM_BUILD_UNSTABLE) add_custom_target(gtsam_unstable_header DEPENDS "../gtsam_unstable/gtsam_unstable.h") - set(GTSAM_UNSTABLE_IMPORT "from .gtsam_unstable import *") wrap_and_install_library_cython("../gtsam_unstable/gtsam_unstable.h" # interface_header "from gtsam.gtsam cimport *" # extra imports - "${GTSAM_CYTHON_INSTALL_PATH}/gtsam" # install path + "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable" # install path gtsam_unstable # library to link with "gtsam_unstable;gtsam_unstable_header;cythonize_gtsam" # dependencies to be built before wrapping ) - # for some reasons cython gtsam_unstable can't find gtsam/gtsam.pxd without doing this - file(WRITE ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py "") endif() + file(READ "${PROJECT_SOURCE_DIR}/cython/requirements.txt" CYTHON_INSTALL_REQUIREMENTS) + file(READ "${PROJECT_SOURCE_DIR}/README.md" README_CONTENTS) + # Install the custom-generated __init__.py # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable - configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py) - install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam") + configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py COPYONLY) + configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py COPYONLY) + configure_file(${PROJECT_SOURCE_DIR}/cython/setup.py.in ${PROJECT_BINARY_DIR}/cython/setup.py) + install_cython_files("${PROJECT_BINARY_DIR}/cython/setup.py" "${GTSAM_CYTHON_INSTALL_PATH}") # install scripts and tests install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") + install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam_unstable" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") endif () diff --git a/cython/README.md b/cython/README.md index 8ba824f8d..b9ce2f012 100644 --- a/cython/README.md +++ b/cython/README.md @@ -17,10 +17,15 @@ named **gtsam_eigency**, to interface between C++'s Eigen and Python's numpy. The wrapped module will be installed to `GTSAM_CYTHON_INSTALL_PATH`, which is by default: `/cython` -- Modify your `PYTHONPATH` to include the `GTSAM_CYTHON_INSTALL_PATH`: +- To use the library without installing system-wide: modify your `PYTHONPATH` to include the `GTSAM_CYTHON_INSTALL_PATH`: ```bash export PYTHONPATH=$PYTHONPATH: ``` +- To install system-wide: run `make install` then navigate to `GTSAM_CYTHON_INSTALL_PATH` and run `python setup.py install` + - (the same command can be used to install into a virtual environment if it is active) + - note: if you don't want gtsam to install to a system directory such as `/usr/local`, pass `-DCMAKE_INSTALL_PREFIX="./install"` to cmake to install gtsam to a subdirectory of the build directory. + - if you run `setup.py` from the build directory rather than the installation directory, the script will warn you with the message: `setup.py is being run from an unexpected location`. + Before `make install` is run, not all the components of the package have been copied across, so running `setup.py` from the build directory would result in an incomplete package. UNIT TESTS ========== diff --git a/cython/gtsam/__init__.py b/cython/gtsam/__init__.py new file mode 100644 index 000000000..d40ee4502 --- /dev/null +++ b/cython/gtsam/__init__.py @@ -0,0 +1,26 @@ +from .gtsam import * + +try: + import gtsam_unstable + + + def _deprecated_wrapper(item, name): + def wrapper(*args, **kwargs): + from warnings import warn + message = ('importing the unstable item "{}" directly from gtsam is deprecated. '.format(name) + + 'Please import it from gtsam_unstable.') + warn(message) + return item(*args, **kwargs) + return wrapper + + + for name in dir(gtsam_unstable): + if not name.startswith('__'): + item = getattr(gtsam_unstable, name) + if callable(item): + item = _deprecated_wrapper(item, name) + globals()[name] = item + +except ImportError: + pass + diff --git a/cython/gtsam/__init__.py.in b/cython/gtsam/__init__.py.in deleted file mode 100644 index 85451c680..000000000 --- a/cython/gtsam/__init__.py.in +++ /dev/null @@ -1,2 +0,0 @@ -from .gtsam import * -${GTSAM_UNSTABLE_IMPORT} diff --git a/cython/gtsam/examples/PlanarManipulatorExample.py b/cython/gtsam/examples/PlanarManipulatorExample.py index af21e6ca5..73959b6c5 100644 --- a/cython/gtsam/examples/PlanarManipulatorExample.py +++ b/cython/gtsam/examples/PlanarManipulatorExample.py @@ -24,6 +24,7 @@ from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 import gtsam import gtsam.utils.plot as gtsam_plot from gtsam import Pose2 +from gtsam.utils.test_case import GtsamTestCase def vector3(x, y, z): @@ -202,7 +203,7 @@ Q1 = np.radians(vector3(-30, -45, -90)) Q2 = np.radians(vector3(-90, 90, 0)) -class TestPose2SLAMExample(unittest.TestCase): +class TestPose2SLAMExample(GtsamTestCase): """Unit tests for functions used below.""" def setUp(self): diff --git a/cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py b/cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py new file mode 100644 index 000000000..02c696905 --- /dev/null +++ b/cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py @@ -0,0 +1,35 @@ +""" +GTSAM Copyright 2010-2018, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +Authors: Frank Dellaert, et al. (see THANKS for the full author list) + +See LICENSE for the license information + +Initialize PoseSLAM with Chordal init +Author: Luca Carlone, Frank Dellaert (python port) +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function + +import numpy as np + +import gtsam + +# Read graph from file +g2oFile = gtsam.findExampleDataFile("pose3example.txt") + +is3D = True +graph, initial = gtsam.readG2o(g2oFile, is3D) + +# Add prior on the first key. TODO: assumes first key ios z +priorModel = gtsam.noiseModel_Diagonal.Variances( + np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) +firstKey = initial.keys().at(0) +graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel)) + +# Initializing Pose3 - chordal relaxation" +initialization = gtsam.InitializePose3.initialize(graph) + +print(initialization) diff --git a/cython/gtsam/tests/test_Cal3Unified.py b/cython/gtsam/tests/test_Cal3Unified.py index 3225d2ff9..fbf5f3565 100644 --- a/cython/gtsam/tests/test_Cal3Unified.py +++ b/cython/gtsam/tests/test_Cal3Unified.py @@ -1,9 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Cal3Unified unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np +import gtsam +from gtsam.utils.test_case import GtsamTestCase -class TestCal3Unified(unittest.TestCase): + +class TestCal3Unified(GtsamTestCase): def test_Cal3Unified(self): K = gtsam.Cal3Unified() @@ -11,12 +24,15 @@ class TestCal3Unified(unittest.TestCase): self.assertEqual(K.fx(), 1.) def test_retract(self): - expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1) - K = gtsam.Cal3Unified(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1) + expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, + 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1) + K = gtsam.Cal3Unified(100, 105, 0.0, 320, 240, + 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1) d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10, 1], order='F') actual = K.retract(d) - self.assertTrue(actual.equals(expected, 1e-9)) + self.gtsamAssertEquals(actual, expected) np.testing.assert_allclose(d, K.localCoordinates(actual)) + if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_JacobianFactor.py b/cython/gtsam/tests/test_JacobianFactor.py index bf63c839b..04433492b 100644 --- a/cython/gtsam/tests/test_JacobianFactor.py +++ b/cython/gtsam/tests/test_JacobianFactor.py @@ -1,8 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +JacobianFactor unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np -class TestJacobianFactor(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestJacobianFactor(GtsamTestCase): def test_eliminate(self): # Recommended way to specify a matrix (see cython/README) @@ -54,7 +68,7 @@ class TestJacobianFactor(unittest.TestCase): expectedCG = gtsam.GaussianConditional( x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel_Unit.Create(2)) # check if the result matches - self.assertTrue(actualCG.equals(expectedCG, 1e-4)) + self.gtsamAssertEquals(actualCG, expectedCG, 1e-4) # the expected linear factor Bl1 = np.array([[4.47214, 0.00], @@ -72,7 +86,7 @@ class TestJacobianFactor(unittest.TestCase): expectedLF = gtsam.JacobianFactor(l1, Bl1, x1, Bx1, b1, model2) # check if the result matches the combined (reduced) factor - self.assertTrue(lf.equals(expectedLF, 1e-4)) + self.gtsamAssertEquals(lf, expectedLF, 1e-4) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_KalmanFilter.py b/cython/gtsam/tests/test_KalmanFilter.py index 56f9e2573..94c41df72 100644 --- a/cython/gtsam/tests/test_KalmanFilter.py +++ b/cython/gtsam/tests/test_KalmanFilter.py @@ -1,8 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +KalmanFilter unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np -class TestKalmanFilter(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestKalmanFilter(GtsamTestCase): def test_KalmanFilter(self): F = np.eye(2) diff --git a/cython/gtsam/tests/test_LocalizationExample.py b/cython/gtsam/tests/test_LocalizationExample.py index c373f162c..6ce65f087 100644 --- a/cython/gtsam/tests/test_LocalizationExample.py +++ b/cython/gtsam/tests/test_LocalizationExample.py @@ -1,8 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Localization unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np -class TestLocalizationExample(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestLocalizationExample(GtsamTestCase): def test_LocalizationExample(self): # Create the graph (defined in pose2SLAM.h, derived from @@ -43,7 +57,7 @@ class TestLocalizationExample(unittest.TestCase): P = [None] * result.size() for i in range(0, result.size()): pose_i = result.atPose2(i) - self.assertTrue(pose_i.equals(groundTruth.atPose2(i), 1e-4)) + self.gtsamAssertEquals(pose_i, groundTruth.atPose2(i), 1e-4) P[i] = marginals.marginalCovariance(i) if __name__ == "__main__": diff --git a/cython/gtsam/tests/test_NonlinearOptimizer.py b/cython/gtsam/tests/test_NonlinearOptimizer.py new file mode 100644 index 000000000..efefb218a --- /dev/null +++ b/cython/gtsam/tests/test_NonlinearOptimizer.py @@ -0,0 +1,72 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for IMU testing scenarios. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +# pylint: disable=invalid-name, no-name-in-module + +from __future__ import print_function + +import unittest + +import gtsam +from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer, + GaussNewtonParams, LevenbergMarquardtOptimizer, + LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, + Point2, PriorFactorPoint2, Values) +from gtsam.utils.test_case import GtsamTestCase + +KEY1 = 1 +KEY2 = 2 + + +class TestScenario(GtsamTestCase): + def test_optimize(self): + """Do trivial test with three optimizer variants.""" + fg = NonlinearFactorGraph() + model = gtsam.noiseModel_Unit.Create(2) + 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) + + # 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) + + # optimize parameters + ordering = Ordering() + ordering.push_back(KEY1) + + # Gauss-Newton + gnParams = GaussNewtonParams() + gnParams.setOrdering(ordering) + actual1 = GaussNewtonOptimizer(fg, initial_values, gnParams).optimize() + self.assertAlmostEqual(0, fg.error(actual1)) + + # Levenberg-Marquardt + lmParams = LevenbergMarquardtParams.CeresDefaults() + lmParams.setOrdering(ordering) + actual2 = LevenbergMarquardtOptimizer( + fg, initial_values, lmParams).optimize() + self.assertAlmostEqual(0, fg.error(actual2)) + + # Dogleg + dlParams = DoglegParams() + dlParams.setOrdering(ordering) + actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize() + self.assertAlmostEqual(0, fg.error(actual3)) + + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_OdometryExample.py b/cython/gtsam/tests/test_OdometryExample.py index 1100e8334..c8ea95588 100644 --- a/cython/gtsam/tests/test_OdometryExample.py +++ b/cython/gtsam/tests/test_OdometryExample.py @@ -1,8 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Odometry unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np -class TestOdometryExample(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestOdometryExample(GtsamTestCase): def test_OdometryExample(self): # Create the graph (defined in pose2SLAM.h, derived from @@ -39,7 +53,7 @@ class TestOdometryExample(unittest.TestCase): # Check first pose equality pose_1 = result.atPose2(1) - self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4)) + self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_PlanarSLAMExample.py b/cython/gtsam/tests/test_PlanarSLAMExample.py index 046a93f35..ae813d35c 100644 --- a/cython/gtsam/tests/test_PlanarSLAMExample.py +++ b/cython/gtsam/tests/test_PlanarSLAMExample.py @@ -1,11 +1,25 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +PlanarSLAM unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam from math import pi + import numpy as np -class TestPose2SLAMExample(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase - def test_Pose2SLAMExample(self): + +class TestPlanarSLAM(GtsamTestCase): + + def test_PlanarSLAM(self): # Assumptions # - All values are axis aligned # - Robot poses are facing along the X axis (horizontal, to the right in images) @@ -56,7 +70,7 @@ class TestPose2SLAMExample(unittest.TestCase): P = marginals.marginalCovariance(1) pose_1 = result.atPose2(1) - self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4)) + self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4) diff --git a/cython/gtsam/tests/test_Pose2.py b/cython/gtsam/tests/test_Pose2.py index afd0c5773..9652b594a 100644 --- a/cython/gtsam/tests/test_Pose2.py +++ b/cython/gtsam/tests/test_Pose2.py @@ -1,12 +1,23 @@ -"""Pose2 unit tests.""" +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Pose2 unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest import numpy as np +import gtsam from gtsam import Pose2 +from gtsam.utils.test_case import GtsamTestCase -class TestPose2(unittest.TestCase): +class TestPose2(GtsamTestCase): """Test selected Pose2 methods.""" def test_adjoint(self): diff --git a/cython/gtsam/tests/test_Pose2SLAMExample.py b/cython/gtsam/tests/test_Pose2SLAMExample.py index bcaa7be4f..a79b6b18c 100644 --- a/cython/gtsam/tests/test_Pose2SLAMExample.py +++ b/cython/gtsam/tests/test_Pose2SLAMExample.py @@ -1,9 +1,23 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Pose2SLAM unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam from math import pi + import numpy as np -class TestPose2SLAMExample(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestPose2SLAMExample(GtsamTestCase): def test_Pose2SLAMExample(self): # Assumptions @@ -56,7 +70,7 @@ class TestPose2SLAMExample(unittest.TestCase): P = marginals.marginalCovariance(1) pose_1 = result.atPose2(1) - self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4)) + self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_Pose3.py b/cython/gtsam/tests/test_Pose3.py index b878a9551..3eb4067c9 100644 --- a/cython/gtsam/tests/test_Pose3.py +++ b/cython/gtsam/tests/test_Pose3.py @@ -1,13 +1,24 @@ -"""Pose3 unit tests.""" +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Pose3 unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import math import unittest import numpy as np +import gtsam from gtsam import Point3, Pose3, Rot3 +from gtsam.utils.test_case import GtsamTestCase -class TestPose3(unittest.TestCase): +class TestPose3(GtsamTestCase): """Test selected Pose3 methods.""" def test_between(self): @@ -16,14 +27,14 @@ class TestPose3(unittest.TestCase): T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3)) expected = T2.inverse().compose(T3) actual = T2.between(T3) - self.assertTrue(actual.equals(expected, 1e-6)) + self.gtsamAssertEquals(actual, expected, 1e-6) def test_transform_to(self): """Test transform_to method.""" transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0)) actual = transform.transform_to(Point3(3, 2, 10)) expected = Point3(2, 1, 10) - self.assertTrue(actual.equals(expected, 1e-6)) + self.gtsamAssertEquals(actual, expected, 1e-6) def test_range(self): """Test range method.""" diff --git a/cython/gtsam/tests/test_Pose3SLAMExample.py b/cython/gtsam/tests/test_Pose3SLAMExample.py index e33db2145..1e9eaac67 100644 --- a/cython/gtsam/tests/test_Pose3SLAMExample.py +++ b/cython/gtsam/tests/test_Pose3SLAMExample.py @@ -1,10 +1,24 @@ -import unittest -import numpy as np -import gtsam -from math import pi -from gtsam.utils.circlePose3 import * +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved -class TestPose3SLAMExample(unittest.TestCase): +See LICENSE for the license information + +PoseSLAM unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest +from math import pi + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase +from gtsam.utils.circlePose3 import * + + +class TestPose3SLAMExample(GtsamTestCase): def test_Pose3SLAMExample(self): # Create a hexagon of poses @@ -40,7 +54,7 @@ class TestPose3SLAMExample(unittest.TestCase): result = optimizer.optimizeSafely() pose_1 = result.atPose3(1) - self.assertTrue(pose_1.equals(p1, 1e-4)) + self.gtsamAssertEquals(pose_1, p1, 1e-4) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_PriorFactor.py b/cython/gtsam/tests/test_PriorFactor.py index 95ec2ae94..0acf50c2c 100644 --- a/cython/gtsam/tests/test_PriorFactor.py +++ b/cython/gtsam/tests/test_PriorFactor.py @@ -1,8 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +PriorFactor unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np -class TestPriorFactor(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestPriorFactor(GtsamTestCase): def test_PriorFactor(self): values = gtsam.Values() diff --git a/cython/gtsam/tests/test_SFMExample.py b/cython/gtsam/tests/test_SFMExample.py index 606b26a43..e8fa46186 100644 --- a/cython/gtsam/tests/test_SFMExample.py +++ b/cython/gtsam/tests/test_SFMExample.py @@ -1,11 +1,24 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +SFM unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam -from gtsam import symbol + import numpy as np + +import gtsam import gtsam.utils.visual_data_generator as generator +from gtsam import symbol +from gtsam.utils.test_case import GtsamTestCase -class TestSFMExample(unittest.TestCase): +class TestSFMExample(GtsamTestCase): def test_SFMExample(self): options = generator.Options() @@ -59,11 +72,11 @@ class TestSFMExample(unittest.TestCase): # Check optimized results, should be equal to ground truth for i in range(len(truth.cameras)): pose_i = result.atPose3(symbol(ord('x'), i)) - self.assertTrue(pose_i.equals(truth.cameras[i].pose(), 1e-5)) + self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) for j in range(len(truth.points)): point_j = result.atPoint3(symbol(ord('p'), j)) - self.assertTrue(point_j.equals(truth.points[j], 1e-5)) + self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_Scenario.py b/cython/gtsam/tests/test_Scenario.py index 4cca1400b..09601fba5 100644 --- a/cython/gtsam/tests/test_Scenario.py +++ b/cython/gtsam/tests/test_Scenario.py @@ -1,11 +1,27 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Scenario unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +from __future__ import print_function + import math import unittest + import numpy as np import gtsam +from gtsam.utils.test_case import GtsamTestCase + +# pylint: disable=invalid-name, E1101 -class TestScenario(unittest.TestCase): +class TestScenario(GtsamTestCase): def setUp(self): pass @@ -29,7 +45,8 @@ class TestScenario(unittest.TestCase): T30 = scenario.pose(T) np.testing.assert_almost_equal( np.array([math.pi, 0, math.pi]), T30.rotation().xyz()) - self.assert_(gtsam.Point3(0, 0, 2 * R).equals(T30.translation(), 1e-9)) + self.gtsamAssertEquals(gtsam.Point3( + 0, 0, 2.0 * R), T30.translation(), 1e-9) if __name__ == '__main__': diff --git a/cython/gtsam/tests/test_SimpleCamera.py b/cython/gtsam/tests/test_SimpleCamera.py index 7924a9b1c..efdfec561 100644 --- a/cython/gtsam/tests/test_SimpleCamera.py +++ b/cython/gtsam/tests/test_SimpleCamera.py @@ -1,18 +1,31 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +SimpleCamera unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import math -import numpy as np import unittest -from gtsam import Pose2, Point3, Rot3, Pose3, Cal3_S2, SimpleCamera +import numpy as np + +import gtsam +from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, SimpleCamera +from gtsam.utils.test_case import GtsamTestCase K = Cal3_S2(625, 625, 0, 0, 0) -class TestSimpleCamera(unittest.TestCase): +class TestSimpleCamera(GtsamTestCase): def test_constructor(self): pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5)) camera = SimpleCamera(pose1, K) - self.assertTrue(camera.calibration().equals(K, 1e-9)) - self.assertTrue(camera.pose().equals(pose1, 1e-9)) + self.gtsamAssertEquals(camera.calibration(), K, 1e-9) + self.gtsamAssertEquals(camera.pose(), pose1, 1e-9) def test_level2(self): # Create a level camera, looking in Y-direction @@ -25,7 +38,7 @@ class TestSimpleCamera(unittest.TestCase): z = Point3(0,1,0) wRc = Rot3(x,y,z) expected = Pose3(wRc,Point3(0.4,0.3,0.1)) - self.assertTrue(camera.pose().equals(expected, 1e-9)) + self.gtsamAssertEquals(camera.pose(), expected, 1e-9) if __name__ == "__main__": diff --git a/cython/gtsam/tests/test_StereoVOExample.py b/cython/gtsam/tests/test_StereoVOExample.py index dacd4a116..3f5f57522 100644 --- a/cython/gtsam/tests/test_StereoVOExample.py +++ b/cython/gtsam/tests/test_StereoVOExample.py @@ -1,10 +1,23 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Stereo VO unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam -from gtsam import symbol + import numpy as np +import gtsam +from gtsam import symbol +from gtsam.utils.test_case import GtsamTestCase -class TestStereoVOExample(unittest.TestCase): + +class TestStereoVOExample(GtsamTestCase): def test_StereoVOExample(self): ## Assumptions @@ -60,10 +73,10 @@ class TestStereoVOExample(unittest.TestCase): ## check equality for the first pose and point pose_x1 = result.atPose3(x1) - self.assertTrue(pose_x1.equals(first_pose,1e-4)) + self.gtsamAssertEquals(pose_x1, first_pose,1e-4) point_l1 = result.atPoint3(l1) - self.assertTrue(point_l1.equals(expected_l1,1e-4)) + self.gtsamAssertEquals(point_l1, expected_l1,1e-4) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_Values.py b/cython/gtsam/tests/test_Values.py index 08e133840..20634a21c 100644 --- a/cython/gtsam/tests/test_Values.py +++ b/cython/gtsam/tests/test_Values.py @@ -1,19 +1,34 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Values unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +# pylint: disable=invalid-name, E1101, E0611 import unittest + import numpy as np -from gtsam import Point2, Point3, Unit3, Rot2, Pose2, Rot3, Pose3 -from gtsam import Values, Cal3_S2, Cal3DS2, Cal3Bundler, EssentialMatrix, imuBias_ConstantBias +import gtsam +from gtsam import (Cal3_S2, Cal3Bundler, Cal3DS2, EssentialMatrix, Point2, + Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values, + imuBias_ConstantBias) +from gtsam.utils.test_case import GtsamTestCase -class TestValues(unittest.TestCase): +class TestValues(GtsamTestCase): def test_values(self): values = Values() E = EssentialMatrix(Rot3(), Unit3()) tol = 1e-9 - values.insert(0, Point2(0,0)) - values.insert(1, Point3(0,0,0)) + values.insert(0, Point2(0, 0)) + values.insert(1, Point3(0, 0, 0)) values.insert(2, Rot2()) values.insert(3, Pose2()) values.insert(4, Rot3()) @@ -34,36 +49,38 @@ class TestValues(unittest.TestCase): # The wrapper will automatically fix the type and storage order for you, # but for performance reasons, it's recommended to specify the correct # type and storage order. - vec = np.array([1., 2., 3.]) # for vectors, the order is not important, but dtype still is + # for vectors, the order is not important, but dtype still is + vec = np.array([1., 2., 3.]) values.insert(11, vec) mat = np.array([[1., 2.], [3., 4.]], order='F') values.insert(12, mat) # Test with dtype int and the default order='C' # This still works as the wrapper converts to the correct type and order for you # but is nornally not recommended! - mat2 = np.array([[1,2,],[3,5]]) + mat2 = np.array([[1, 2, ], [3, 5]]) values.insert(13, mat2) - self.assertTrue(values.atPoint2(0).equals(Point2(), tol)) - self.assertTrue(values.atPoint3(1).equals(Point3(), tol)) - self.assertTrue(values.atRot2(2).equals(Rot2(), tol)) - self.assertTrue(values.atPose2(3).equals(Pose2(), tol)) - self.assertTrue(values.atRot3(4).equals(Rot3(), tol)) - self.assertTrue(values.atPose3(5).equals(Pose3(), tol)) - self.assertTrue(values.atCal3_S2(6).equals(Cal3_S2(), tol)) - self.assertTrue(values.atCal3DS2(7).equals(Cal3DS2(), tol)) - self.assertTrue(values.atCal3Bundler(8).equals(Cal3Bundler(), tol)) - self.assertTrue(values.atEssentialMatrix(9).equals(E, tol)) - self.assertTrue(values.atimuBias_ConstantBias( - 10).equals(imuBias_ConstantBias(), tol)) + self.gtsamAssertEquals(values.atPoint2(0), Point2(0,0), tol) + self.gtsamAssertEquals(values.atPoint3(1), Point3(0,0,0), tol) + self.gtsamAssertEquals(values.atRot2(2), Rot2(), tol) + self.gtsamAssertEquals(values.atPose2(3), Pose2(), tol) + self.gtsamAssertEquals(values.atRot3(4), Rot3(), tol) + self.gtsamAssertEquals(values.atPose3(5), Pose3(), tol) + self.gtsamAssertEquals(values.atCal3_S2(6), Cal3_S2(), tol) + self.gtsamAssertEquals(values.atCal3DS2(7), Cal3DS2(), tol) + self.gtsamAssertEquals(values.atCal3Bundler(8), Cal3Bundler(), tol) + self.gtsamAssertEquals(values.atEssentialMatrix(9), E, tol) + self.gtsamAssertEquals(values.atimuBias_ConstantBias( + 10), imuBias_ConstantBias(), tol) # special cases for Vector and Matrix: actualVector = values.atVector(11) - self.assertTrue(np.allclose(vec, actualVector, tol)) + np.testing.assert_allclose(vec, actualVector, tol) actualMatrix = values.atMatrix(12) - self.assertTrue(np.allclose(mat, actualMatrix, tol)) + np.testing.assert_allclose(mat, actualMatrix, tol) actualMatrix2 = values.atMatrix(13) - self.assertTrue(np.allclose(mat2, actualMatrix2, tol)) + np.testing.assert_allclose(mat2, actualMatrix2, tol) + if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_VisualISAMExample.py b/cython/gtsam/tests/test_VisualISAMExample.py index 39bfa6eb4..99d7e6160 100644 --- a/cython/gtsam/tests/test_VisualISAMExample.py +++ b/cython/gtsam/tests/test_VisualISAMExample.py @@ -1,10 +1,25 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +visual_isam unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest + import numpy as np -from gtsam import symbol + +import gtsam import gtsam.utils.visual_data_generator as generator import gtsam.utils.visual_isam as visual_isam +from gtsam import symbol +from gtsam.utils.test_case import GtsamTestCase -class TestVisualISAMExample(unittest.TestCase): + +class TestVisualISAMExample(GtsamTestCase): def test_VisualISAMExample(self): # Data Options @@ -32,11 +47,11 @@ class TestVisualISAMExample(unittest.TestCase): for i in range(len(truth.cameras)): pose_i = result.atPose3(symbol(ord('x'), i)) - self.assertTrue(pose_i.equals(truth.cameras[i].pose(), 1e-5)) + self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) for j in range(len(truth.points)): point_j = result.atPoint3(symbol(ord('l'), j)) - self.assertTrue(point_j.equals(truth.points[j], 1e-5)) + self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_dataset.py b/cython/gtsam/tests/test_dataset.py new file mode 100644 index 000000000..60fb9450d --- /dev/null +++ b/cython/gtsam/tests/test_dataset.py @@ -0,0 +1,45 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for testing dataset access. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +# pylint: disable=invalid-name, no-name-in-module, no-member + +from __future__ import print_function + +import unittest + +import gtsam +from gtsam import BetweenFactorPose3, BetweenFactorPose3s +from gtsam.utils.test_case import GtsamTestCase + + +class TestDataset(GtsamTestCase): + """Tests for datasets.h wrapper.""" + + def setUp(self): + """Get some common paths.""" + self.pose3_example_g2o_file = gtsam.findExampleDataFile( + "pose3example.txt") + + def test_readG2o3D(self): + """Test reading directly into factor graph.""" + is3D = True + graph, initial = gtsam.readG2o(self.pose3_example_g2o_file, is3D) + self.assertEqual(graph.size(), 6) + self.assertEqual(initial.size(), 5) + + def test_parse3Dfactors(self): + """Test parsing into data structure.""" + factors = gtsam.parse3DFactors(self.pose3_example_g2o_file) + self.assertEqual(factors.size(), 6) + self.assertIsInstance(factors.at(0), BetweenFactorPose3) + + +if __name__ == '__main__': + unittest.main() diff --git a/cython/gtsam/tests/test_initialize_pose3.py b/cython/gtsam/tests/test_initialize_pose3.py new file mode 100644 index 000000000..3aa7e3470 --- /dev/null +++ b/cython/gtsam/tests/test_initialize_pose3.py @@ -0,0 +1,89 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for 3D SLAM initialization, using rotation relaxation. +Author: Luca Carlone and Frank Dellaert (Python) +""" +# pylint: disable=invalid-name, E1101, E0611 +import unittest + +import numpy as np + +import gtsam +from gtsam import NonlinearFactorGraph, Point3, Pose3, Rot3, Values +from gtsam.utils.test_case import GtsamTestCase + +x0, x1, x2, x3 = 0, 1, 2, 3 + + +class TestValues(GtsamTestCase): + + def setUp(self): + + model = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) + + # We consider a small graph: + # symbolic FG + # x2 0 1 + # / | \ 1 2 + # / | \ 2 3 + # x3 | x1 2 0 + # \ | / 0 3 + # \ | / + # x0 + # + p0 = Point3(0, 0, 0) + self.R0 = Rot3.Expmap(np.array([0.0, 0.0, 0.0])) + p1 = Point3(1, 2, 0) + self.R1 = Rot3.Expmap(np.array([0.0, 0.0, 1.570796])) + p2 = Point3(0, 2, 0) + self.R2 = Rot3.Expmap(np.array([0.0, 0.0, 3.141593])) + p3 = Point3(-1, 1, 0) + self.R3 = Rot3.Expmap(np.array([0.0, 0.0, 4.712389])) + + pose0 = Pose3(self.R0, p0) + pose1 = Pose3(self.R1, p1) + pose2 = Pose3(self.R2, p2) + pose3 = Pose3(self.R3, p3) + + g = NonlinearFactorGraph() + g.add(gtsam.BetweenFactorPose3(x0, x1, pose0.between(pose1), model)) + g.add(gtsam.BetweenFactorPose3(x1, x2, pose1.between(pose2), model)) + g.add(gtsam.BetweenFactorPose3(x2, x3, pose2.between(pose3), model)) + g.add(gtsam.BetweenFactorPose3(x2, x0, pose2.between(pose0), model)) + g.add(gtsam.BetweenFactorPose3(x0, x3, pose0.between(pose3), model)) + g.add(gtsam.PriorFactorPose3(x0, pose0, model)) + self.graph = g + + def test_buildPose3graph(self): + pose3graph = gtsam.InitializePose3.buildPose3graph(self.graph) + + def test_orientations(self): + pose3Graph = gtsam.InitializePose3.buildPose3graph(self.graph) + + initial = gtsam.InitializePose3.computeOrientationsChordal(pose3Graph) + + # comparison is up to M_PI, that's why we add some multiples of 2*M_PI + self.gtsamAssertEquals(initial.atRot3(x0), self.R0, 1e-6) + self.gtsamAssertEquals(initial.atRot3(x1), self.R1, 1e-6) + self.gtsamAssertEquals(initial.atRot3(x2), self.R2, 1e-6) + self.gtsamAssertEquals(initial.atRot3(x3), self.R3, 1e-6) + + def test_initializePoses(self): + g2oFile = gtsam.findExampleDataFile("pose3example-grid") + is3D = True + inputGraph, expectedValues = gtsam.readG2o(g2oFile, is3D) + priorModel = gtsam.noiseModel_Unit.Create(6) + inputGraph.add(gtsam.PriorFactorPose3(0, Pose3(), priorModel)) + + initial = gtsam.InitializePose3.initialize(inputGraph) + # TODO(frank): very loose !! + self.gtsamAssertEquals(initial, expectedValues, 0.1) + + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/utils/test_case.py b/cython/gtsam/utils/test_case.py new file mode 100644 index 000000000..7df1e6ee9 --- /dev/null +++ b/cython/gtsam/utils/test_case.py @@ -0,0 +1,27 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +TestCase class with GTSAM assert utils. +Author: Frank Dellaert +""" +import unittest + + +class GtsamTestCase(unittest.TestCase): + """TestCase class with GTSAM assert utils.""" + + def gtsamAssertEquals(self, actual, expected, tol=1e-9): + """ AssertEqual function that prints out actual and expected if not equal. + Usage: + self.gtsamAssertEqual(actual,expected) + Keyword Arguments: + tol {float} -- tolerance passed to 'equals', default 1e-9 + """ + equal = actual.equals(expected, tol) + if not equal: + raise self.failureException( + "Values are not equal:\n{}!={}".format(actual, expected)) diff --git a/cython/gtsam_unstable/__init__.py b/cython/gtsam_unstable/__init__.py new file mode 100644 index 000000000..3195e6da4 --- /dev/null +++ b/cython/gtsam_unstable/__init__.py @@ -0,0 +1 @@ +from .gtsam_unstable import * diff --git a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py new file mode 100644 index 000000000..786701e0f --- /dev/null +++ b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py @@ -0,0 +1,102 @@ +""" +GTSAM Copyright 2010-2018, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +Authors: Frank Dellaert, et al. (see THANKS for the full author list) + +See LICENSE for the license information + +Demonstration of the fixed-lag smoothers using a planar robot example +and multiple odometry-like sensors +Author: Frank Dellaert (C++), Jeremy Aguilon (Python) +""" + +import numpy as np + +import gtsam +import gtsam_unstable + + +def _timestamp_key_value(key, value): + """ + + """ + return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( + key, value + ) + + +def BatchFixedLagSmootherExample(): + """ + Runs a batch fixed smoother on an agent with two odometry + sensors that is simply moving to the + """ + + # Define a batch fixed lag smoother, which uses + # Levenberg-Marquardt to perform the nonlinear optimization + lag = 2.0 + smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag) + + # Create containers to store the factors and linearization points + # that will be sent to the smoothers + new_factors = gtsam.NonlinearFactorGraph() + new_values = gtsam.Values() + new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap() + + # Create a prior on the first pose, placing it at the origin + prior_mean = gtsam.Pose2(0, 0, 0) + prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) + X1 = 0 + new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise)) + new_values.insert(X1, prior_mean) + new_timestamps.insert(_timestamp_key_value(X1, 0.0)) + + delta_time = 0.25 + time = 0.25 + + while time <= 3.0: + previous_key = 1000 * (time - delta_time) + current_key = 1000 * time + + # assign current key to the current timestamp + new_timestamps.insert(_timestamp_key_value(current_key, time)) + + # Add a guess for this pose to the new values + # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s] + current_pose = gtsam.Pose2(time * 2, 0, 0) + new_values.insert(current_key, current_pose) + + # Add odometry factors from two different sources with different error + # stats + odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02) + odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.1, 0.1, 0.05])) + new_factors.push_back(gtsam.BetweenFactorPose2( + previous_key, current_key, odometry_measurement_1, odometry_noise_1 + )) + + odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01) + odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.05, 0.05, 0.05])) + new_factors.push_back(gtsam.BetweenFactorPose2( + previous_key, current_key, odometry_measurement_2, odometry_noise_2 + )) + + # Update the smoothers with the new factors. In this case, + # one iteration must pass for Levenberg-Marquardt to accurately + # estimate + if time >= 0.50: + smoother_batch.update(new_factors, new_values, new_timestamps) + print("Timestamp = " + str(time) + ", Key = " + str(current_key)) + print(smoother_batch.calculateEstimatePose2(current_key)) + + new_timestamps.clear() + new_values.clear() + new_factors.resize(0) + + time += delta_time + + +if __name__ == '__main__': + BatchFixedLagSmootherExample() + print("Example complete") diff --git a/cython/gtsam_unstable/examples/__init__.py b/cython/gtsam_unstable/examples/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/cython/gtsam_unstable/tests/__init__.py b/cython/gtsam_unstable/tests/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py new file mode 100644 index 000000000..8d3af311f --- /dev/null +++ b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py @@ -0,0 +1,135 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Cal3Unified unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest + +import numpy as np + +import gtsam +import gtsam_unstable +from gtsam.utils.test_case import GtsamTestCase + + +def _timestamp_key_value(key, value): + return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( + key, value + ) + + +class TestFixedLagSmootherExample(GtsamTestCase): + ''' + Tests the fixed lag smoother wrapper + ''' + + def test_FixedLagSmootherExample(self): + ''' + Simple test that checks for equality between C++ example + file and the Python implementation. See + gtsam_unstable/examples/FixedLagSmootherExample.cpp + ''' + # Define a batch fixed lag smoother, which uses + # Levenberg-Marquardt to perform the nonlinear optimization + lag = 2.0 + smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag) + + # Create containers to store the factors and linearization points + # that will be sent to the smoothers + new_factors = gtsam.NonlinearFactorGraph() + new_values = gtsam.Values() + new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap() + + # Create a prior on the first pose, placing it at the origin + prior_mean = gtsam.Pose2(0, 0, 0) + prior_noise = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.3, 0.3, 0.1])) + X1 = 0 + new_factors.push_back( + gtsam.PriorFactorPose2( + X1, prior_mean, prior_noise)) + new_values.insert(X1, prior_mean) + new_timestamps.insert(_timestamp_key_value(X1, 0.0)) + + delta_time = 0.25 + time = 0.25 + + i = 0 + + ground_truth = [ + gtsam.Pose2(0.995821, 0.0231012, 0.0300001), + gtsam.Pose2(1.49284, 0.0457247, 0.045), + gtsam.Pose2(1.98981, 0.0758879, 0.06), + gtsam.Pose2(2.48627, 0.113502, 0.075), + gtsam.Pose2(2.98211, 0.158558, 0.09), + gtsam.Pose2(3.47722, 0.211047, 0.105), + gtsam.Pose2(3.97149, 0.270956, 0.12), + gtsam.Pose2(4.4648, 0.338272, 0.135), + gtsam.Pose2(4.95705, 0.41298, 0.15), + gtsam.Pose2(5.44812, 0.495063, 0.165), + gtsam.Pose2(5.9379, 0.584503, 0.18), + ] + + # Iterates from 0.25s to 3.0s, adding 0.25s each loop + # In each iteration, the agent moves at a constant speed + # and its two odometers measure the change. The smoothed + # result is then compared to the ground truth + while time <= 3.0: + previous_key = 1000 * (time - delta_time) + current_key = 1000 * time + + # assign current key to the current timestamp + new_timestamps.insert(_timestamp_key_value(current_key, time)) + + # Add a guess for this pose to the new values + # Assume that the robot moves at 2 m/s. Position is time[s] * + # 2[m/s] + current_pose = gtsam.Pose2(time * 2, 0, 0) + new_values.insert(current_key, current_pose) + + # Add odometry factors from two different sources with different + # error stats + odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02) + odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.1, 0.1, 0.05])) + new_factors.push_back( + gtsam.BetweenFactorPose2( + previous_key, + current_key, + odometry_measurement_1, + odometry_noise_1)) + + odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01) + odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.05, 0.05, 0.05])) + new_factors.push_back( + gtsam.BetweenFactorPose2( + previous_key, + current_key, + odometry_measurement_2, + odometry_noise_2)) + + # Update the smoothers with the new factors. In this case, + # one iteration must pass for Levenberg-Marquardt to accurately + # estimate + if time >= 0.50: + smoother_batch.update(new_factors, new_values, new_timestamps) + + estimate = smoother_batch.calculateEstimatePose2(current_key) + self.assertTrue(estimate.equals(ground_truth[i], 1e-4)) + i += 1 + + new_timestamps.clear() + new_values.clear() + new_factors.resize(0) + + time += delta_time + + +if __name__ == "__main__": + unittest.main() diff --git a/cython/setup.py.in b/cython/setup.py.in new file mode 100644 index 000000000..d6af28762 --- /dev/null +++ b/cython/setup.py.in @@ -0,0 +1,49 @@ +import os +import sys +try: + from setuptools import setup, find_packages +except ImportError: + from distutils.core import setup, find_packages + +if 'SETUP_PY_NO_CHECK' not in os.environ: + script_path = os.path.abspath(os.path.realpath(__file__)) + install_path = os.path.abspath(os.path.realpath(os.path.join('${GTSAM_CYTHON_INSTALL_PATH}', 'setup.py'))) + if script_path != install_path: + print('setup.py is being run from an unexpected location: "{}"'.format(script_path)) + print('please run `make install` and run the script from there') + sys.exit(1) + + +packages = find_packages() + +setup( + name='gtsam', + description='Georgia Tech Smoothing And Mapping library', + url='https://bitbucket.org/gtborg/gtsam', + version='${GTSAM_VERSION_STRING}', # https://www.python.org/dev/peps/pep-0440/ + license='Simplified BSD license', + keywords='slam sam robotics localization mapping optimization', + long_description='''${README_CONTENTS}''', + # https://pypi.org/pypi?%3Aaction=list_classifiers + classifiers=[ + 'Development Status :: 5 - Production/Stable', + 'Intended Audience :: Education', + 'Intended Audience :: Developers', + 'Intended Audience :: Science/Research', + 'Operating System :: MacOS', + 'Operating System :: Microsoft :: Windows', + 'Operating System :: POSIX', + 'License :: OSI Approved :: BSD License', + 'Programming Language :: Python :: 2', + 'Programming Language :: Python :: 3', + ], + + packages=packages, + package_data={package: + [f for f in os.listdir(package.replace('.', os.path.sep)) if os.path.splitext(f)[1] in ('.so', '.dll')] + for package in packages + }, + install_requires=[line.strip() for line in ''' +${CYTHON_INSTALL_REQUIREMENTS} +'''.splitlines() if len(line.strip()) > 0 and not line.strip().startswith('#')] +) diff --git a/gtsam.h b/gtsam.h index 48768db40..014907037 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1218,14 +1218,30 @@ class VariableIndex { namespace noiseModel { #include virtual class Base { + void print(string s) const; + // Methods below are available for all noise models. However, can't add them + // because wrap (incorrectly) thinks robust classes derive from this Base as well. + // bool isConstrained() const; + // bool isUnit() const; + // size_t dim() const; + // Vector sigmas() const; }; virtual class Gaussian : gtsam::noiseModel::Base { static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); static gtsam::noiseModel::Gaussian* Covariance(Matrix R); - Matrix R() const; + bool equals(gtsam::noiseModel::Base& expected, double tol); - void print(string s) const; + + // access to noise model + Matrix R() const; + Matrix information() const; + Matrix covariance() const; + + // Whitening operations + Vector whiten(Vector v) const; + Vector unwhiten(Vector v) const; + Matrix Whiten(Matrix H) const; // enabling serialization functionality void serializable() const; @@ -1236,8 +1252,12 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian { static gtsam::noiseModel::Diagonal* Variances(Vector variances); static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); Matrix R() const; - void print(string s) const; + // access to noise model + Vector sigmas() const; + Vector invsigmas() const; + Vector precisions() const; + // enabling serialization functionality void serializable() const; }; @@ -1263,7 +1283,9 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal { static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); - void print(string s) const; + + // access to noise model + double sigma() const; // enabling serialization functionality void serializable() const; @@ -1271,7 +1293,6 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal { virtual class Unit : gtsam::noiseModel::Isotropic { static gtsam::noiseModel::Unit* Create(size_t dim); - void print(string s) const; // enabling serialization functionality void serializable() const; @@ -1279,11 +1300,11 @@ virtual class Unit : gtsam::noiseModel::Isotropic { namespace mEstimator { virtual class Base { + void print(string s) const; }; virtual class Null: gtsam::noiseModel::mEstimator::Base { Null(); - void print(string s) const; static gtsam::noiseModel::mEstimator::Null* Create(); // enabling serialization functionality @@ -1292,7 +1313,6 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base { virtual class Fair: gtsam::noiseModel::mEstimator::Base { Fair(double c); - void print(string s) const; static gtsam::noiseModel::mEstimator::Fair* Create(double c); // enabling serialization functionality @@ -1301,7 +1321,6 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base { virtual class Huber: gtsam::noiseModel::mEstimator::Base { Huber(double k); - void print(string s) const; static gtsam::noiseModel::mEstimator::Huber* Create(double k); // enabling serialization functionality @@ -1310,7 +1329,6 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base { virtual class Tukey: gtsam::noiseModel::mEstimator::Base { Tukey(double k); - void print(string s) const; static gtsam::noiseModel::mEstimator::Tukey* Create(double k); // enabling serialization functionality @@ -1322,7 +1340,6 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base { virtual class Robust : gtsam::noiseModel::Base { Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); - void print(string s) const; // enabling serialization functionality void serializable() const; @@ -2002,10 +2019,12 @@ virtual class NonlinearOptimizerParams { void setVerbosity(string s); string getLinearSolverType() const; - void setLinearSolverType(string solver); - void setOrdering(const gtsam::Ordering& ordering); + void setIterativeParams(gtsam::IterativeOptimizationParameters* params); + void setOrdering(const gtsam::Ordering& ordering); + string getOrderingType() const; + void setOrderingType(string ordering); bool isMultifrontal() const; bool isSequential() const; @@ -2026,15 +2045,32 @@ virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { LevenbergMarquardtParams(); - double getlambdaInitial() const; + bool getDiagonalDamping() const; double getlambdaFactor() const; + double getlambdaInitial() const; + double getlambdaLowerBound() const; double getlambdaUpperBound() const; + bool getUseFixedLambdaFactor(); + string getLogFile() const; string getVerbosityLM() const; - - void setlambdaInitial(double value); + + void setDiagonalDamping(bool flag); void setlambdaFactor(double value); + void setlambdaInitial(double value); + void setlambdaLowerBound(double value); void setlambdaUpperBound(double value); + void setUseFixedLambdaFactor(bool flag); + void setLogFile(string s); void setVerbosityLM(string s); + + static gtsam::LevenbergMarquardtParams LegacyDefaults(); + static gtsam::LevenbergMarquardtParams CeresDefaults(); + + static gtsam::LevenbergMarquardtParams EnsureHasOrdering( + gtsam::LevenbergMarquardtParams params, + const gtsam::NonlinearFactorGraph& graph); + static gtsam::LevenbergMarquardtParams ReplaceOrdering( + gtsam::LevenbergMarquardtParams params, const gtsam::Ordering& ordering); }; #include @@ -2480,6 +2516,36 @@ void save2D(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, string filename); +// std::vector::shared_ptr> +class BetweenFactorPose3s +{ + size_t size() const; + gtsam::BetweenFactorPose3* at(size_t i) const; +}; + +#include +class InitializePose3 { + static gtsam::Values computeOrientationsChordal( + const gtsam::NonlinearFactorGraph& pose3Graph); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess); + static gtsam::NonlinearFactorGraph buildPose3graph( + const gtsam::NonlinearFactorGraph& graph); + static gtsam::Values initializeOrientations( + const gtsam::NonlinearFactorGraph& graph); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& givenGuess, + bool useGradient); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); +}; + +gtsam::BetweenFactorPose3s parse3DFactors(string filename); +pair load3D(string filename); + pair readG2o(string filename); pair readG2o(string filename, bool is3D); void writeG2o(const gtsam::NonlinearFactorGraph& graph, diff --git a/gtsam/nonlinear/LevenbergMarquardtParams.h b/gtsam/nonlinear/LevenbergMarquardtParams.h index abb8c3c22..4962ad366 100644 --- a/gtsam/nonlinear/LevenbergMarquardtParams.h +++ b/gtsam/nonlinear/LevenbergMarquardtParams.h @@ -122,24 +122,35 @@ public: virtual ~LevenbergMarquardtParams() {} void print(const std::string& str = "") const override; - /// @name Getters/Setters, mainly for MATLAB. Use fields above in C++. + /// @name Getters/Setters, mainly for wrappers. Use fields above in C++. /// @{ bool getDiagonalDamping() const { return diagonalDamping; } double getlambdaFactor() const { return lambdaFactor; } double getlambdaInitial() const { return lambdaInitial; } double getlambdaLowerBound() const { return lambdaLowerBound; } double getlambdaUpperBound() const { return lambdaUpperBound; } + bool getUseFixedLambdaFactor() { return useFixedLambdaFactor; } std::string getLogFile() const { return logFile; } std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM);} + void setDiagonalDamping(bool flag) { diagonalDamping = flag; } void setlambdaFactor(double value) { lambdaFactor = value; } void setlambdaInitial(double value) { lambdaInitial = value; } void setlambdaLowerBound(double value) { lambdaLowerBound = value; } void setlambdaUpperBound(double value) { lambdaUpperBound = value; } - void setLogFile(const std::string& s) { logFile = s; } void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor = flag;} + void setLogFile(const std::string& s) { logFile = s; } void setVerbosityLM(const std::string& s) { verbosityLM = verbosityLMTranslator(s);} // @} + /// @name Clone + /// @{ + + /// @return a deep copy of this object + boost::shared_ptr clone() const { + return boost::shared_ptr(new LevenbergMarquardtParams(*this)); + } + + /// @} }; } diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 88fc0f850..8e4678590 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -54,47 +54,24 @@ public: } virtual void print(const std::string& str = "") const; - size_t getMaxIterations() const { - return maxIterations; - } - double getRelativeErrorTol() const { - return relativeErrorTol; - } - double getAbsoluteErrorTol() const { - return absoluteErrorTol; - } - double getErrorTol() const { - return errorTol; - } - std::string getVerbosity() const { - return verbosityTranslator(verbosity); - } + size_t getMaxIterations() const { return maxIterations; } + double getRelativeErrorTol() const { return relativeErrorTol; } + double getAbsoluteErrorTol() const { return absoluteErrorTol; } + double getErrorTol() const { return errorTol; } + std::string getVerbosity() const { return verbosityTranslator(verbosity); } - void setMaxIterations(int value) { - maxIterations = value; - } - void setRelativeErrorTol(double value) { - relativeErrorTol = value; - } - void setAbsoluteErrorTol(double value) { - absoluteErrorTol = value; - } - void setErrorTol(double value) { - errorTol = value; - } - void setVerbosity(const std::string &src) { + void setMaxIterations(int value) { maxIterations = value; } + void setRelativeErrorTol(double value) { relativeErrorTol = value; } + void setAbsoluteErrorTol(double value) { absoluteErrorTol = value; } + void setErrorTol(double value) { errorTol = value; } + void setVerbosity(const std::string& src) { verbosity = verbosityTranslator(src); } static Verbosity verbosityTranslator(const std::string &s) ; static std::string verbosityTranslator(Verbosity value) ; - // Successive Linearization Parameters - -public: - /** See NonlinearOptimizerParams::linearSolverType */ - enum LinearSolverType { MULTIFRONTAL_CHOLESKY, MULTIFRONTAL_QR, @@ -168,13 +145,9 @@ public: private: std::string linearSolverTranslator(LinearSolverType linearSolverType) const; - LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const; - std::string orderingTypeTranslator(Ordering::OrderingType type) const; - Ordering::OrderingType orderingTypeTranslator(const std::string& type) const; - }; // For backward compatibility: diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 3979996da..2f247811d 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -16,7 +16,7 @@ * @date August, 2014 */ -#include +#include #include #include #include @@ -29,69 +29,65 @@ using namespace std; namespace gtsam { -namespace InitializePose3 { -static const Matrix I9 = I_9x9; -static const Vector zero9 = Vector::Zero(9); -static const Matrix zero33 = Z_3x3; - -static const Key keyAnchor = symbol('Z', 9999999); +static const Key kAnchorKey = symbol('Z', 9999999); /* ************************************************************************* */ -GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { +GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const NonlinearFactorGraph& g) { GaussianFactorGraph linearGraph; - noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9); - for(const boost::shared_ptr& factor: g) { + for(const auto& factor: g) { Matrix3 Rij; + double rotationPrecision = 1.0; - boost::shared_ptr > pose3Between = - boost::dynamic_pointer_cast >(factor); - if (pose3Between) + auto pose3Between = boost::dynamic_pointer_cast >(factor); + if (pose3Between){ Rij = pose3Between->measured().rotation().matrix(); - else - std::cout << "Error in buildLinearOrientationGraph" << std::endl; + Vector precisions = Vector::Zero(6); + precisions[0] = 1.0; // vector of all zeros except first entry equal to 1 + pose3Between->noiseModel()->whitenInPlace(precisions); // gets marginal precision of first variable + rotationPrecision = precisions[0]; // rotations first + }else{ + cout << "Error in buildLinearOrientationGraph" << endl; + } - const KeyVector& keys = factor->keys(); + const auto& keys = factor->keys(); Key key1 = keys[0], key2 = keys[1]; Matrix M9 = Z_9x9; M9.block(0,0,3,3) = Rij; M9.block(3,3,3,3) = Rij; M9.block(6,6,3,3) = Rij; - linearGraph.add(key1, -I9, key2, M9, zero9, model); + linearGraph.add(key1, -I_9x9, key2, M9, Z_9x1, noiseModel::Isotropic::Precision(9, rotationPrecision)); } // prior on the anchor orientation - linearGraph.add(keyAnchor, I9, (Vector(9) << 1.0, 0.0, 0.0,/* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0).finished(), model); + linearGraph.add( + kAnchorKey, I_9x9, + (Vector(9) << 1.0, 0.0, 0.0, /* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0) + .finished(), + noiseModel::Isotropic::Precision(9, 1)); return linearGraph; } /* ************************************************************************* */ // Transform VectorValues into valid Rot3 -Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { +Values InitializePose3::normalizeRelaxedRotations( + const VectorValues& relaxedRot3) { gttic(InitializePose3_computeOrientationsChordal); Matrix ppm = Z_3x3; // plus plus minus ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1; Values validRot3; - for(const VectorValues::value_type& it: relaxedRot3) { + for(const auto& it: relaxedRot3) { Key key = it.first; - if (key != keyAnchor) { - const Vector& rotVector = it.second; - Matrix3 rotMat; - rotMat(0,0) = rotVector(0); rotMat(0,1) = rotVector(1); rotMat(0,2) = rotVector(2); - rotMat(1,0) = rotVector(3); rotMat(1,1) = rotVector(4); rotMat(1,2) = rotVector(5); - rotMat(2,0) = rotVector(6); rotMat(2,1) = rotVector(7); rotMat(2,2) = rotVector(8); + if (key != kAnchorKey) { + Matrix3 R; R << it.second; Matrix U, V; Vector s; - svd(rotMat, U, s, V); + svd(R.transpose(), U, s, V); Matrix3 normalizedRotMat = U * V.transpose(); - // std::cout << "rotMat \n" << rotMat << std::endl; - // std::cout << "U V' \n" << U * V.transpose() << std::endl; - // std::cout << "V \n" << V << std::endl; - if(normalizedRotMat.determinant() < 0) normalizedRotMat = U * ppm * V.transpose(); @@ -103,32 +99,29 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { } /* ************************************************************************* */ -// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node -NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { +NonlinearFactorGraph InitializePose3::buildPose3graph(const NonlinearFactorGraph& graph) { gttic(InitializePose3_buildPose3graph); NonlinearFactorGraph pose3Graph; - for(const boost::shared_ptr& factor: graph) { + for(const auto& factor: graph) { // recast to a between on Pose3 - boost::shared_ptr > pose3Between = - boost::dynamic_pointer_cast >(factor); + const auto pose3Between = boost::dynamic_pointer_cast >(factor); if (pose3Between) pose3Graph.add(pose3Between); // recast PriorFactor to BetweenFactor - boost::shared_ptr > pose3Prior = - boost::dynamic_pointer_cast >(factor); + const auto pose3Prior = boost::dynamic_pointer_cast >(factor); if (pose3Prior) - pose3Graph.emplace_shared >(keyAnchor, pose3Prior->keys()[0], + pose3Graph.emplace_shared >(kAnchorKey, pose3Prior->keys()[0], pose3Prior->prior(), pose3Prior->noiseModel()); } return pose3Graph; } /* ************************************************************************* */ -// Return the orientations of a graph including only BetweenFactors -Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { +Values InitializePose3::computeOrientationsChordal( + const NonlinearFactorGraph& pose3Graph) { gttic(InitializePose3_computeOrientationsChordal); // regularize measurements and plug everything in a factor graph @@ -142,14 +135,15 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { } /* ************************************************************************* */ -// Return the orientations of a graph including only BetweenFactors -Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter, const bool setRefFrame) { +Values InitializePose3::computeOrientationsGradient( + const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, + const size_t maxIter, const bool setRefFrame) { gttic(InitializePose3_computeOrientationsGradient); // this works on the inverse rotations, according to Tron&Vidal,2011 Values inverseRot; - inverseRot.insert(keyAnchor, Rot3()); - for(const Values::ConstKeyValuePair& key_value: givenGuess) { + inverseRot.insert(kAnchorKey, Rot3()); + for(const auto& key_value: givenGuess) { Key key = key_value.key; const Pose3& pose = givenGuess.at(key); inverseRot.insert(key, pose.rotation().inverse()); @@ -164,9 +158,9 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const // calculate max node degree & allocate gradient size_t maxNodeDeg = 0; VectorValues grad; - for(const Values::ConstKeyValuePair& key_value: inverseRot) { + for(const auto& key_value: inverseRot) { Key key = key_value.key; - grad.insert(key,Vector3::Zero()); + grad.insert(key,Z_3x1); size_t currNodeDeg = (adjEdgesMap.at(key)).size(); if(currNodeDeg > maxNodeDeg) maxNodeDeg = currNodeDeg; @@ -180,42 +174,37 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const double mu_max = maxNodeDeg * rho; double stepsize = 2/mu_max; // = 1/(a b dG) - std::cout <<" b " << b <<" f0 " << f0 <<" a " << a <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; double maxGrad; // gradient iterations size_t it; - for(it=0; it < maxIter; it++){ + for (it = 0; it < maxIter; it++) { ////////////////////////////////////////////////////////////////////////// // compute the gradient at each node - //std::cout << "it " << it <<" b " << b <<" f0 " << f0 <<" a " << a - // <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; maxGrad = 0; - for(const Values::ConstKeyValuePair& key_value: inverseRot) { + for (const auto& key_value : inverseRot) { Key key = key_value.key; - //std::cout << "---------------------------key " << DefaultKeyFormatter(key) << std::endl; - Vector gradKey = Vector3::Zero(); + Vector gradKey = Z_3x1; // collect the gradient for each edge incident on key - for(const size_t& factorId: adjEdgesMap.at(key)){ + for (const size_t& factorId : adjEdgesMap.at(key)) { Rot3 Rij = factorId2RotMap.at(factorId); Rot3 Ri = inverseRot.at(key); - if( key == (pose3Graph.at(factorId))->keys()[0] ){ - Key key1 = (pose3Graph.at(factorId))->keys()[1]; + auto factor = pose3Graph.at(factorId); + const auto& keys = factor->keys(); + if (key == keys[0]) { + Key key1 = keys[1]; Rot3 Rj = inverseRot.at(key1); - gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b); - //std::cout << "key1 " << DefaultKeyFormatter(key1) << " gradientTron(Ri, Rij * Rj, a, b) \n " << gradientTron(Ri, Rij * Rj, a, b) << std::endl; - }else if( key == (pose3Graph.at(factorId))->keys()[1] ){ - Key key0 = (pose3Graph.at(factorId))->keys()[0]; + gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b); + } else if (key == keys[1]) { + Key key0 = keys[0]; Rot3 Rj = inverseRot.at(key0); gradKey = gradKey + gradientTron(Ri, Rij.between(Rj), a, b); - //std::cout << "key0 " << DefaultKeyFormatter(key0) << " gradientTron(Ri, Rij.inverse() * Rj, a, b) \n " << gradientTron(Ri, Rij.between(Rj), a, b) << std::endl; - }else{ - std::cout << "Error in gradient computation" << std::endl; + } else { + cout << "Error in gradient computation" << endl; } - } // end of i-th gradient computation - grad.at(key) = stepsize * gradKey; + } // end of i-th gradient computation + grad.at(key) = stepsize * gradKey; double normGradKey = (gradKey).norm(); - //std::cout << "key " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl; if(normGradKey>maxGrad) maxGrad = normGradKey; } // end of loop over nodes @@ -230,14 +219,12 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const break; } // enf of gradient iterations - std::cout << "nr of gradient iterations " << it << "maxGrad " << maxGrad << std::endl; - // Return correct rotations - const Rot3& Rref = inverseRot.at(keyAnchor); // This will be set to the identity as so far we included no prior + const Rot3& Rref = inverseRot.at(kAnchorKey); // This will be set to the identity as so far we included no prior Values estimateRot; - for(const Values::ConstKeyValuePair& key_value: inverseRot) { + for(const auto& key_value: inverseRot) { Key key = key_value.key; - if (key != keyAnchor) { + if (key != kAnchorKey) { const Rot3& R = inverseRot.at(key); if(setRefFrame) estimateRot.insert(key, Rref.compose(R.inverse())); @@ -249,11 +236,11 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const } /* ************************************************************************* */ -void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph){ +void InitializePose3::createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, + const NonlinearFactorGraph& pose3Graph) { size_t factorId = 0; - for(const boost::shared_ptr& factor: pose3Graph) { - boost::shared_ptr > pose3Between = - boost::dynamic_pointer_cast >(factor); + for(const auto& factor: pose3Graph) { + auto pose3Between = boost::dynamic_pointer_cast >(factor); if (pose3Between){ Rot3 Rij = pose3Between->measured().rotation(); factorId2RotMap.insert(pair(factorId,Rij)); @@ -275,14 +262,14 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, adjEdgesMap.insert(pair >(key2, edge_id)); } }else{ - std::cout << "Error in computeOrientationsGradient" << std::endl; + cout << "Error in createSymbolicGraph" << endl; } factorId++; } } /* ************************************************************************* */ -Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) { +Vector3 InitializePose3::gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) { Vector3 logRot = Rot3::Logmap(R1.between(R2)); double th = logRot.norm(); @@ -292,10 +279,10 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl th = logRot.norm(); } // exclude small or invalid rotations - if (th > 1e-5 && th == th){ // nonzero valid rotations + if (th > 1e-5 && th == th) { // nonzero valid rotations logRot = logRot / th; - }else{ - logRot = Vector3::Zero(); + } else { + logRot = Z_3x1; th = 0.0; } @@ -304,8 +291,7 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl } /* ************************************************************************* */ -Values initializeOrientations(const NonlinearFactorGraph& graph) { - +Values InitializePose3::initializeOrientations(const NonlinearFactorGraph& graph) { // We "extract" the Pose3 subgraph of the original graph: this // is done to properly model priors and avoiding operating on a larger graph NonlinearFactorGraph pose3Graph = buildPose3graph(graph); @@ -315,29 +301,30 @@ Values initializeOrientations(const NonlinearFactorGraph& graph) { } ///* ************************************************************************* */ -Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { +Values InitializePose3::computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { gttic(InitializePose3_computePoses); // put into Values structure Values initialPose; - for(const Values::ConstKeyValuePair& key_value: initialRot){ + for (const auto& key_value : initialRot) { Key key = key_value.key; const Rot3& rot = initialRot.at(key); Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0)); initialPose.insert(key, initializedPose); } + // add prior noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); - initialPose.insert(keyAnchor, Pose3()); - pose3graph.emplace_shared >(keyAnchor, Pose3(), priorModel); + initialPose.insert(kAnchorKey, Pose3()); + pose3graph.emplace_shared >(kAnchorKey, Pose3(), priorModel); // Create optimizer GaussNewtonParams params; bool singleIter = true; - if(singleIter){ + if (singleIter) { params.maxIterations = 1; - }else{ - std::cout << " \n\n\n\n performing more than 1 GN iterations \n\n\n" <(key); estimate.insert(key, pose); } @@ -356,22 +343,9 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { } /* ************************************************************************* */ -Values initialize(const NonlinearFactorGraph& graph) { +Values InitializePose3::initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, + bool useGradient) { gttic(InitializePose3_initialize); - - // We "extract" the Pose3 subgraph of the original graph: this - // is done to properly model priors and avoiding operating on a larger graph - NonlinearFactorGraph pose3Graph = buildPose3graph(graph); - - // Get orientations from relative orientation measurements - Values valueRot3 = computeOrientationsChordal(pose3Graph); - - // Compute the full poses (1 GN iteration on full poses) - return computePoses(pose3Graph, valueRot3); -} - -/* ************************************************************************* */ -Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient) { Values initialValues; // We "extract" the Pose3 subgraph of the original graph: this @@ -380,27 +354,18 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, b // Get orientations from relative orientation measurements Values orientations; - if(useGradient) + if (useGradient) orientations = computeOrientationsGradient(pose3Graph, givenGuess); else orientations = computeOrientationsChordal(pose3Graph); -// orientations.print("orientations\n"); - // Compute the full poses (1 GN iteration on full poses) return computePoses(pose3Graph, orientations); - - // for(const Values::ConstKeyValuePair& key_value: orientations) { - // Key key = key_value.key; - // if (key != keyAnchor) { - // const Point3& pos = givenGuess.at(key).translation(); - // const Rot3& rot = orientations.at(key); - // Pose3 initializedPoses = Pose3(rot, pos); - // initialValues.insert(key, initializedPoses); - // } - // } - // return initialValues; } -} // end of namespace lago -} // end of namespace gtsam +/* ************************************************************************* */ +Values InitializePose3::initialize(const NonlinearFactorGraph& graph) { + return initialize(graph, Values(), false); +} + +} // namespace gtsam diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h index dba5ceac3..ce1b28854 100644 --- a/gtsam/slam/InitializePose3.h +++ b/gtsam/slam/InitializePose3.h @@ -20,40 +20,68 @@ #pragma once -#include +#include +#include #include #include -#include -#include +#include namespace gtsam { typedef std::map > KeyVectorMap; -typedef std::map KeyRotMap; +typedef std::map KeyRotMap; -namespace InitializePose3 { +struct GTSAM_EXPORT InitializePose3 { + static GaussianFactorGraph buildLinearOrientationGraph( + const NonlinearFactorGraph& g); -GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g); + static Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); -GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); + /** + * Return the orientations of a graph including only BetweenFactors + */ + static Values computeOrientationsChordal( + const NonlinearFactorGraph& pose3Graph); -GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph); + /** + * Return the orientations of a graph including only BetweenFactors + */ + static Values computeOrientationsGradient( + const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, + size_t maxIter = 10000, const bool setRefFrame = true); -GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, - const Values& givenGuess, size_t maxIter = 10000, const bool setRefFrame = true); + static void createSymbolicGraph(KeyVectorMap& adjEdgesMap, + KeyRotMap& factorId2RotMap, + const NonlinearFactorGraph& pose3Graph); -GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, - const NonlinearFactorGraph& pose3Graph); + static Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, + const double b); -GTSAM_EXPORT Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b); + /** + * Select the subgraph of betweenFactors and transforms priors into between + * wrt a fictitious node + */ + static NonlinearFactorGraph buildPose3graph( + const NonlinearFactorGraph& graph); -GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph); + static Values computePoses(NonlinearFactorGraph& pose3graph, + Values& initialRot); -GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot); + /** + * "extract" the Pose3 subgraph of the original graph, get orientations from + * relative orientation measurements using chordal method. + */ + static Values initializeOrientations(const NonlinearFactorGraph& graph); -GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph); + /** + * "extract" the Pose3 subgraph of the original graph, get orientations from + * relative orientation measurements (using either gradient or chordal + * method), and finish up with 1 GN iteration on full poses. + */ + static Values initialize(const NonlinearFactorGraph& graph, + const Values& givenGuess, bool useGradient = false); -GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient = false); - -} // end of namespace lago -} // end of namespace gtsam + /// Calls initialize above using Chordal method. + static Values initialize(const NonlinearFactorGraph& graph); +}; +} // end of namespace gtsam diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 34370d4e2..c32a049e2 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -409,17 +409,17 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, /* ************************************************************************* */ GraphAndValues readG2o(const string& g2oFile, const bool is3D, - KernelFunctionType kernelFunctionType) { - // just call load2D - int maxID = 0; - bool addNoise = false; - bool smart = true; - - if(is3D) + KernelFunctionType kernelFunctionType) { + if (is3D) { return load3D(g2oFile); - - return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart, - NoiseFormatG2O, kernelFunctionType); + } else { + // just call load2D + int maxID = 0; + bool addNoise = false; + bool smart = true; + return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart, + NoiseFormatG2O, kernelFunctionType); + } } /* ************************************************************************* */ @@ -510,15 +510,12 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, } /* ************************************************************************* */ -GraphAndValues load3D(const string& filename) { - +std::map parse3DPoses(const string& filename) { ifstream is(filename.c_str()); if (!is) - throw invalid_argument("load3D: can not find file " + filename); - - Values::shared_ptr initial(new Values); - NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); + throw invalid_argument("parse3DPoses: can not find file " + filename); + std::map poses; while (!is.eof()) { char buf[LINESIZE]; is.getline(buf, LINESIZE); @@ -530,22 +527,24 @@ GraphAndValues load3D(const string& filename) { Key id; double x, y, z, roll, pitch, yaw; ls >> id >> x >> y >> z >> roll >> pitch >> yaw; - Rot3 R = Rot3::Ypr(yaw,pitch,roll); - Point3 t = Point3(x, y, z); - initial->insert(id, Pose3(R,t)); + poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z})); } if (tag == "VERTEX_SE3:QUAT") { Key id; double x, y, z, qx, qy, qz, qw; ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw; - Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); - Point3 t = Point3(x, y, z); - initial->insert(id, Pose3(R,t)); + poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z})); } } - is.clear(); /* clears the end-of-file and error flags */ - is.seekg(0, ios::beg); + return poses; +} +/* ************************************************************************* */ +BetweenFactorPose3s parse3DFactors(const string& filename) { + ifstream is(filename.c_str()); + if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename); + + std::vector::shared_ptr> factors; while (!is.eof()) { char buf[LINESIZE]; is.getline(buf, LINESIZE); @@ -557,44 +556,55 @@ GraphAndValues load3D(const string& filename) { Key id1, id2; double x, y, z, roll, pitch, yaw; ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; - Rot3 R = Rot3::Ypr(yaw,pitch,roll); - Point3 t = Point3(x, y, z); - Matrix m = I_6x6; - for (int i = 0; i < 6; i++) - for (int j = i; j < 6; j++) - ls >> m(i, j); + Matrix m(6, 6); + for (size_t i = 0; i < 6; i++) + for (size_t j = i; j < 6; j++) ls >> m(i, j); SharedNoiseModel model = noiseModel::Gaussian::Information(m); - NonlinearFactor::shared_ptr factor( - new BetweenFactor(id1, id2, Pose3(R,t), model)); - graph->push_back(factor); + factors.emplace_back(new BetweenFactor( + id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model)); } if (tag == "EDGE_SE3:QUAT") { - Matrix m = I_6x6; Key id1, id2; double x, y, z, qx, qy, qz, qw; ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; - Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); - Point3 t = Point3(x, y, z); - for (int i = 0; i < 6; i++){ - for (int j = i; j < 6; j++){ + Matrix m(6, 6); + for (size_t i = 0; i < 6; i++) { + for (size_t j = i; j < 6; j++) { double mij; ls >> mij; m(i, j) = mij; m(j, i) = mij; } } - Matrix mgtsam = I_6x6; + Matrix mgtsam(6, 6); - mgtsam.block<3,3>(0,0) = m.block<3,3>(3,3); // cov rotation - mgtsam.block<3,3>(3,3) = m.block<3,3>(0,0); // cov translation - mgtsam.block<3,3>(0,3) = m.block<3,3>(0,3); // off diagonal - mgtsam.block<3,3>(3,0) = m.block<3,3>(3,0); // off diagonal + mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3); // cov rotation + mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0); // cov translation + mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3); // off diagonal + mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); - NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, Pose3(R,t), model)); - graph->push_back(factor); + factors.emplace_back(new BetweenFactor( + id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model)); } } + return factors; +} + +/* ************************************************************************* */ +GraphAndValues load3D(const string& filename) { + const auto factors = parse3DFactors(filename); + NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); + for (const auto& factor : factors) { + graph->push_back(factor); + } + + const auto poses = parse3DPoses(filename); + Values::shared_ptr initial(new Values); + for (const auto& key_pose : poses) { + initial->insert(key_pose.first, key_pose.second); + } + return make_pair(graph, initial); } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 7c9b54a6f..2106df48d 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include #include @@ -34,6 +35,7 @@ #include // for pair #include #include +#include namespace gtsam { @@ -153,9 +155,14 @@ GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, const std::string& filename); -/** - * Load TORO 3D Graph - */ +/// Parse edges in 3D TORO graph file into a set of BetweenFactors. +using BetweenFactorPose3s = std::vector::shared_ptr>; +GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename); + +/// Parse vertices in 3D TORO graph file into a map of Pose3s. +GTSAM_EXPORT std::map parse3DPoses(const std::string& filename); + +/// Load TORO 3D Graph GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); /// A measurement with its camera index diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 39c2d3722..9434280d4 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -16,8 +16,8 @@ */ -#include #include +#include #include #include #include @@ -162,65 +162,74 @@ TEST( dataSet, readG2o) } /* ************************************************************************* */ -TEST( dataSet, readG2o3D) -{ +TEST(dataSet, readG2o3D) { const string g2oFile = findExampleDataFile("pose3example"); + auto model = noiseModel::Isotropic::Precision(6, 10000); + + // Initialize 6 relative measurements with quaternion/point3 values: + const std::vector relative_poses = { + {{0.854230, 0.190253, 0.283162, -0.392318}, + {1.001367, 0.015390, 0.004948}}, + {{0.105373, 0.311512, 0.656877, -0.678505}, + {0.523923, 0.776654, 0.326659}}, + {{0.568551, 0.595795, -0.561677, 0.079353}, + {0.910927, 0.055169, -0.411761}}, + {{0.542221, -0.592077, 0.303380, -0.513226}, + {0.775288, 0.228798, -0.596923}}, + {{0.327419, -0.125250, -0.534379, 0.769122}, + {-0.577841, 0.628016, -0.543592}}, + {{0.083672, 0.104639, 0.627755, 0.766795}, + {-0.623267, 0.086928, 0.773222}}, + }; + + // Initialize 5 poses with quaternion/point3 values: + const std::vector poses = { + {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}}, + {{0.854230, 0.190253, 0.283162, -0.392318}, + {1.001367, 0.015390, 0.004948}}, + {{0.421446, -0.351729, -0.597838, 0.584174}, + {1.993500, 0.023275, 0.003793}}, + {{0.067024, 0.331798, -0.200659, 0.919323}, + {2.004291, 1.024305, 0.018047}}, + {{0.765488, -0.035697, -0.462490, 0.445933}, + {0.999908, 1.055073, 0.020212}}, + }; + + // Specify connectivity: + using KeyPair = pair; + std::vector edges = {{0, 1}, {1, 2}, {2, 3}, {3, 4}, {1, 4}, {3, 0}}; + + // Created expected factor graph + size_t i = 0; + NonlinearFactorGraph expectedGraph; + for (const auto& keys : edges) { + expectedGraph.emplace_shared>( + keys.first, keys.second, relative_poses[i++], model); + } + + // Check factor parsing + const auto actualFactors = parse3DFactors(g2oFile); + for (size_t i : {0, 1, 2, 3, 4, 5}) { + EXPECT(assert_equal( + *boost::dynamic_pointer_cast>(expectedGraph[i]), + *actualFactors[i], 1e-5)); + } + + // Check pose parsing + const auto actualPoses = parse3DPoses(g2oFile); + for (size_t j : {0, 1, 2, 3, 4}) { + EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5)); + } + + // Check graph version NonlinearFactorGraph::shared_ptr actualGraph; Values::shared_ptr actualValues; bool is3D = true; boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); - - Values expectedValues; - Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); - Point3 p0 = Point3(0.000000, 0.000000, 0.000000); - expectedValues.insert(0, Pose3(R0, p0)); - - Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); - Point3 p1 = Point3(1.001367, 0.015390, 0.004948); - expectedValues.insert(1, Pose3(R1, p1)); - - Rot3 R2 = Rot3::Quaternion(0.421446, -0.351729, -0.597838, 0.584174 ); - Point3 p2 = Point3(1.993500, 0.023275, 0.003793); - expectedValues.insert(2, Pose3(R2, p2)); - - Rot3 R3 = Rot3::Quaternion(0.067024, 0.331798, -0.200659, 0.919323); - Point3 p3 = Point3(2.004291, 1.024305, 0.018047); - expectedValues.insert(3, Pose3(R3, p3)); - - Rot3 R4 = Rot3::Quaternion(0.765488, -0.035697, -0.462490, 0.445933); - Point3 p4 = Point3(0.999908, 1.055073, 0.020212); - expectedValues.insert(4, Pose3(R4, p4)); - - EXPECT(assert_equal(expectedValues,*actualValues,1e-5)); - - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(6) << 10000.0,10000.0,10000.0,10000.0,10000.0,10000.0).finished()); - NonlinearFactorGraph expectedGraph; - - Point3 p01 = Point3(1.001367, 0.015390, 0.004948); - Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); - expectedGraph.emplace_shared >(0, 1, Pose3(R01,p01), model); - - Point3 p12 = Point3(0.523923, 0.776654, 0.326659); - Rot3 R12 = Rot3::Quaternion(0.105373 , 0.311512, 0.656877, -0.678505 ); - expectedGraph.emplace_shared >(1, 2, Pose3(R12,p12), model); - - Point3 p23 = Point3(0.910927, 0.055169, -0.411761); - Rot3 R23 = Rot3::Quaternion(0.568551 , 0.595795, -0.561677, 0.079353 ); - expectedGraph.emplace_shared >(2, 3, Pose3(R23,p23), model); - - Point3 p34 = Point3(0.775288, 0.228798, -0.596923); - Rot3 R34 = Rot3::Quaternion(0.542221 , -0.592077, 0.303380, -0.513226 ); - expectedGraph.emplace_shared >(3, 4, Pose3(R34,p34), model); - - Point3 p14 = Point3(-0.577841, 0.628016, -0.543592); - Rot3 R14 = Rot3::Quaternion(0.327419 , -0.125250, -0.534379, 0.769122 ); - expectedGraph.emplace_shared >(1, 4, Pose3(R14,p14), model); - - Point3 p30 = Point3(-0.623267, 0.086928, 0.773222); - Rot3 R30 = Rot3::Quaternion(0.083672 , 0.104639, 0.627755, 0.766795 ); - expectedGraph.emplace_shared >(3, 0, Pose3(R30,p30), model); - - EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); + EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5)); + for (size_t j : {0, 1, 2, 3, 4}) { + EXPECT(assert_equal(poses[j], actualValues->at(j), 1e-5)); + } } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index e6f08286a..6fe8b3d7c 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -70,6 +70,17 @@ NonlinearFactorGraph graph() { g.add(PriorFactor(x0, pose0, model)); return g; } + +NonlinearFactorGraph graph2() { + NonlinearFactorGraph g; + g.add(BetweenFactor(x0, x1, pose0.between(pose1), noiseModel::Isotropic::Precision(6, 1.0))); + g.add(BetweenFactor(x1, x2, pose1.between(pose2), noiseModel::Isotropic::Precision(6, 1.0))); + g.add(BetweenFactor(x2, x3, pose2.between(pose3), noiseModel::Isotropic::Precision(6, 1.0))); + g.add(BetweenFactor(x2, x0, Pose3(Rot3::Ypr(0.1,0,0.1), Point3()), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero information + g.add(BetweenFactor(x0, x3, Pose3(Rot3::Ypr(0.5,-0.2,0.2), Point3(10,20,30)), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero informatoin + g.add(PriorFactor(x0, pose0, model)); + return g; +} } /* *************************************************************************** */ @@ -91,6 +102,19 @@ TEST( InitializePose3, orientations ) { EXPECT(assert_equal(simple::R3, initial.at(x3), 1e-6)); } +/* *************************************************************************** */ +TEST( InitializePose3, orientationsPrecisions ) { + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph2()); + + Values initial = InitializePose3::computeOrientationsChordal(pose3Graph); + + // comparison is up to M_PI, that's why we add some multiples of 2*M_PI + EXPECT(assert_equal(simple::R0, initial.at(x0), 1e-6)); + EXPECT(assert_equal(simple::R1, initial.at(x1), 1e-6)); + EXPECT(assert_equal(simple::R2, initial.at(x2), 1e-6)); + EXPECT(assert_equal(simple::R3, initial.at(x3), 1e-6)); +} + /* *************************************************************************** */ TEST( InitializePose3, orientationsGradientSymbolicGraph ) { NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp index 931e85c1a..1376aca40 100644 --- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp +++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp @@ -111,23 +111,27 @@ int main(int argc, char** argv) { noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); - // Update the smoothers with the new factors - smootherBatch.update(newFactors, newValues, newTimestamps); - smootherISAM2.update(newFactors, newValues, newTimestamps); - for(size_t i = 1; i < 2; ++i) { // Optionally perform multiple iSAM2 iterations - smootherISAM2.update(); + // Update the smoothers with the new factors. In this example, batch smoother needs one iteration + // to accurately converge. The ISAM smoother doesn't, but we only start getting estiates when + // both are ready for simplicity. + if (time >= 0.50) { + smootherBatch.update(newFactors, newValues, newTimestamps); + smootherISAM2.update(newFactors, newValues, newTimestamps); + for(size_t i = 1; i < 2; ++i) { // Optionally perform multiple iSAM2 iterations + smootherISAM2.update(); + } + + // Print the optimized current pose + cout << setprecision(5) << "Timestamp = " << time << endl; + smootherBatch.calculateEstimate(currentKey).print("Batch Estimate:"); + smootherISAM2.calculateEstimate(currentKey).print("iSAM2 Estimate:"); + cout << endl; + + // Clear contains for the next iteration + newTimestamps.clear(); + newValues.clear(); + newFactors.resize(0); } - - // Print the optimized current pose - cout << setprecision(5) << "Timestamp = " << time << endl; - smootherBatch.calculateEstimate(currentKey).print("Batch Estimate:"); - smootherISAM2.calculateEstimate(currentKey).print("iSAM2 Estimate:"); - cout << endl; - - // Clear contains for the next iteration - newTimestamps.clear(); - newValues.clear(); - newFactors.resize(0); } // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 39c910826..d77895d86 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -505,132 +505,130 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor { //************************************************************************* // nonlinear //************************************************************************* -// #include -// gtsam::GaussianFactorGraph* summarizeGraphSequential( -// const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices); -// gtsam::GaussianFactorGraph* summarizeGraphSequential( -// const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys); +#include +class FixedLagSmootherKeyTimestampMapValue { + FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp); + FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); +}; -// #include -// class FixedLagSmootherKeyTimestampMapValue { -// FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& key, double timestamp); -// FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); -// }; -// -// class FixedLagSmootherKeyTimestampMap { -// FixedLagSmootherKeyTimestampMap(); -// FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other); -// -// // Note: no print function -// -// // common STL methods -// size_t size() const; -// bool empty() const; -// void clear(); -// -// double at(const gtsam::Key& key) const; -// void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value); -// }; -// -// class FixedLagSmootherResult { -// size_t getIterations() const; -// size_t getNonlinearVariables() const; -// size_t getLinearVariables() const; -// double getError() const; -// }; -// -// #include -// virtual class FixedLagSmoother { -// void print(string s) const; -// bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const; -// -// gtsam::FixedLagSmootherKeyTimestampMap timestamps() const; -// double smootherLag() const; -// -// gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps); -// gtsam::Values calculateEstimate() const; -// }; -// -// #include -// virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { -// BatchFixedLagSmoother(); -// BatchFixedLagSmoother(double smootherLag); -// BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); -// -// gtsam::LevenbergMarquardtParams params() const; -// }; -// -// #include -// virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { -// IncrementalFixedLagSmoother(); -// IncrementalFixedLagSmoother(double smootherLag); -// IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); -// -// gtsam::ISAM2Params params() const; -// }; -// -// #include -// virtual class ConcurrentFilter { -// void print(string s) const; -// bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const; -// }; -// -// virtual class ConcurrentSmoother { -// void print(string s) const; -// bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const; -// }; -// -// // Synchronize function -// void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother); -// -// #include -// class ConcurrentBatchFilterResult { -// size_t getIterations() const; -// size_t getLambdas() const; -// size_t getNonlinearVariables() const; -// size_t getLinearVariables() const; -// double getError() const; -// }; -// -// virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter { -// ConcurrentBatchFilter(); -// ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters); -// -// gtsam::NonlinearFactorGraph getFactors() const; -// gtsam::Values getLinearizationPoint() const; -// gtsam::Ordering getOrdering() const; -// gtsam::VectorValues getDelta() const; -// -// gtsam::ConcurrentBatchFilterResult update(); -// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors); -// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); -// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove); -// gtsam::Values calculateEstimate() const; -// }; -// -// #include -// class ConcurrentBatchSmootherResult { -// size_t getIterations() const; -// size_t getLambdas() const; -// size_t getNonlinearVariables() const; -// size_t getLinearVariables() const; -// double getError() const; -// }; -// -// virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { -// ConcurrentBatchSmoother(); -// ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters); -// -// gtsam::NonlinearFactorGraph getFactors() const; -// gtsam::Values getLinearizationPoint() const; -// gtsam::Ordering getOrdering() const; -// gtsam::VectorValues getDelta() const; -// -// gtsam::ConcurrentBatchSmootherResult update(); -// gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors); -// gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); -// gtsam::Values calculateEstimate() const; -// }; +class FixedLagSmootherKeyTimestampMap { + FixedLagSmootherKeyTimestampMap(); + FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + double at(const size_t key) const; + void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value); +}; + +class FixedLagSmootherResult { + size_t getIterations() const; + size_t getNonlinearVariables() const; + size_t getLinearVariables() const; + double getError() const; +}; + +#include +virtual class FixedLagSmoother { + void print(string s) const; + bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const; + + gtsam::FixedLagSmootherKeyTimestampMap timestamps() const; + double smootherLag() const; + + gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps); + gtsam::Values calculateEstimate() const; +}; + +#include +virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { + BatchFixedLagSmoother(); + BatchFixedLagSmoother(double smootherLag); + BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); + + gtsam::LevenbergMarquardtParams params() const; + template + VALUE calculateEstimate(size_t key) const; +}; + +#include +virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { + IncrementalFixedLagSmoother(); + IncrementalFixedLagSmoother(double smootherLag); + IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); + + gtsam::ISAM2Params params() const; +}; + +#include +virtual class ConcurrentFilter { + void print(string s) const; + bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const; +}; + +virtual class ConcurrentSmoother { + void print(string s) const; + bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const; +}; + +// Synchronize function +void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother); + +#include +class ConcurrentBatchFilterResult { + size_t getIterations() const; + size_t getLambdas() const; + size_t getNonlinearVariables() const; + size_t getLinearVariables() const; + double getError() const; +}; + +virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter { + ConcurrentBatchFilter(); + ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters); + + gtsam::NonlinearFactorGraph getFactors() const; + gtsam::Values getLinearizationPoint() const; + gtsam::Ordering getOrdering() const; + gtsam::VectorValues getDelta() const; + + gtsam::ConcurrentBatchFilterResult update(); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove); + gtsam::Values calculateEstimate() const; +}; + +#include +class ConcurrentBatchSmootherResult { + size_t getIterations() const; + size_t getLambdas() const; + size_t getNonlinearVariables() const; + size_t getLinearVariables() const; + double getError() const; +}; + +virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { + ConcurrentBatchSmoother(); + ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters); + + gtsam::NonlinearFactorGraph getFactors() const; + gtsam::Values getLinearizationPoint() const; + gtsam::Ordering getOrdering() const; + gtsam::VectorValues getDelta() const; + + gtsam::ConcurrentBatchSmootherResult update(); + gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors); + gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); + gtsam::Values calculateEstimate() const; +}; //************************************************************************* // slam