Merge branch 'develop' into python_examples
						commit
						af9165197c
					
				|  | @ -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}") | ||||
|  |  | |||
|  | @ -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<Class,N>::inverse` so you get an inverse with a derivative. | ||||
| 
 | ||||
| Finally, to create the traits automatically you can use `internal::LieGroupTraits<Class>` | ||||
| 
 | ||||
| Vector Space | ||||
| ------------ | ||||
| 
 | ||||
|  |  | |||
							
								
								
									
										37
									
								
								INSTALL.md
								
								
								
								
							
							
						
						
									
										37
									
								
								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. | ||||
|  |  | |||
							
								
								
									
										1
									
								
								THANKS
								
								
								
								
							
							
						
						
									
										1
									
								
								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 | ||||
|  |  | |||
|  | @ -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) | ||||
|  |  | |||
|  | @ -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 () | ||||
|  |  | |||
|  | @ -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: `<your CMAKE_INSTALL_PREFIX>/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:<GTSAM_CYTHON_INSTALL_PATH> | ||||
| ``` | ||||
| - 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 | ||||
| ========== | ||||
|  |  | |||
|  | @ -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 | ||||
| 
 | ||||
|  | @ -1,2 +0,0 @@ | |||
| from .gtsam import * | ||||
| ${GTSAM_UNSTABLE_IMPORT} | ||||
|  | @ -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): | ||||
|  |  | |||
|  | @ -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) | ||||
|  | @ -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() | ||||
|  |  | |||
|  | @ -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() | ||||
|  |  | |||
|  | @ -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) | ||||
|  |  | |||
|  | @ -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__": | ||||
|  |  | |||
|  | @ -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() | ||||
|  | @ -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() | ||||
|  |  | |||
|  | @ -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) | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -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): | ||||
|  |  | |||
|  | @ -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() | ||||
|  |  | |||
|  | @ -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.""" | ||||
|  |  | |||
|  | @ -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() | ||||
|  |  | |||
|  | @ -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() | ||||
|  |  | |||
|  | @ -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() | ||||
|  |  | |||
|  | @ -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__': | ||||
|  |  | |||
|  | @ -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__": | ||||
|  |  | |||
|  | @ -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() | ||||
|  |  | |||
|  | @ -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() | ||||
|  |  | |||
|  | @ -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() | ||||
|  |  | |||
|  | @ -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() | ||||
|  | @ -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() | ||||
|  | @ -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)) | ||||
|  | @ -0,0 +1 @@ | |||
| from .gtsam_unstable import * | ||||
|  | @ -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") | ||||
|  | @ -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() | ||||
|  | @ -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('#')] | ||||
| ) | ||||
							
								
								
									
										96
									
								
								gtsam.h
								
								
								
								
							
							
						
						
									
										96
									
								
								gtsam.h
								
								
								
								
							|  | @ -1218,14 +1218,30 @@ class VariableIndex { | |||
| namespace noiseModel { | ||||
| #include <gtsam/linear/NoiseModel.h> | ||||
| 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 <gtsam/nonlinear/DoglegOptimizer.h> | ||||
|  | @ -2480,6 +2516,36 @@ void save2D(const gtsam::NonlinearFactorGraph& graph, | |||
|     const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, | ||||
|     string filename); | ||||
| 
 | ||||
| // std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
 | ||||
| class BetweenFactorPose3s | ||||
| { | ||||
|   size_t size() const; | ||||
|   gtsam::BetweenFactorPose3* at(size_t i) const; | ||||
| }; | ||||
| 
 | ||||
| #include <gtsam/slam/InitializePose3.h> | ||||
| 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<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename); | ||||
| 
 | ||||
| pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename); | ||||
| pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename, bool is3D); | ||||
| void writeG2o(const gtsam::NonlinearFactorGraph& graph, | ||||
|  |  | |||
|  | @ -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<NonlinearOptimizerParams> clone() const { | ||||
|     return boost::shared_ptr<NonlinearOptimizerParams>(new LevenbergMarquardtParams(*this)); | ||||
|   } | ||||
| 
 | ||||
|   /// @}
 | ||||
| }; | ||||
| 
 | ||||
| } | ||||
|  |  | |||
|  | @ -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:
 | ||||
|  |  | |||
|  | @ -16,7 +16,7 @@ | |||
|  *  @date   August, 2014 | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam/slam/InitializePose3.h> | ||||
| #include <gtsam/slam/InitializePose3.h>  | ||||
| #include <gtsam/slam/PriorFactor.h> | ||||
| #include <gtsam/slam/BetweenFactor.h> | ||||
| #include <gtsam/nonlinear/GaussNewtonOptimizer.h> | ||||
|  | @ -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<NonlinearFactor>& factor: g) { | ||||
|   for(const auto& factor: g) { | ||||
|     Matrix3 Rij; | ||||
|     double rotationPrecision = 1.0; | ||||
| 
 | ||||
|     boost::shared_ptr<BetweenFactor<Pose3> > pose3Between = | ||||
|         boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor); | ||||
|     if (pose3Between) | ||||
|     auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(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<NonlinearFactor>& factor: graph) { | ||||
|   for(const auto& factor: graph) { | ||||
| 
 | ||||
|     // recast to a between on Pose3
 | ||||
|     boost::shared_ptr<BetweenFactor<Pose3> > pose3Between = | ||||
|         boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor); | ||||
|     const auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor); | ||||
|     if (pose3Between) | ||||
|       pose3Graph.add(pose3Between); | ||||
| 
 | ||||
|     // recast PriorFactor<Pose3> to BetweenFactor<Pose3>
 | ||||
|     boost::shared_ptr<PriorFactor<Pose3> > pose3Prior = | ||||
|         boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor); | ||||
|     const auto pose3Prior = boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor); | ||||
|     if (pose3Prior) | ||||
|       pose3Graph.emplace_shared<BetweenFactor<Pose3> >(keyAnchor, pose3Prior->keys()[0], | ||||
|       pose3Graph.emplace_shared<BetweenFactor<Pose3> >(kAnchorKey, pose3Prior->keys()[0], | ||||
|               pose3Prior->prior(), pose3Prior->noiseModel()); | ||||
|   } | ||||
|   return pose3Graph; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // Return the orientations of a graph including only BetweenFactors<Pose3>
 | ||||
| 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<Pose3>
 | ||||
| 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<Pose3>(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<Rot3>(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<Rot3>(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<Rot3>(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<Rot3>(keyAnchor); // This will be set to the identity as so far we included no prior
 | ||||
|   const Rot3& Rref = inverseRot.at<Rot3>(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<Rot3>(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<NonlinearFactor>& factor: pose3Graph) { | ||||
|     boost::shared_ptr<BetweenFactor<Pose3> > pose3Between = | ||||
|         boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor); | ||||
|   for(const auto& factor: pose3Graph) { | ||||
|     auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor); | ||||
|     if (pose3Between){ | ||||
|       Rot3 Rij = pose3Between->measured().rotation(); | ||||
|       factorId2RotMap.insert(pair<Key, Rot3 >(factorId,Rij)); | ||||
|  | @ -275,14 +262,14 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, | |||
|         adjEdgesMap.insert(pair<Key, vector<size_t> >(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<Rot3>(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<PriorFactor<Pose3> >(keyAnchor, Pose3(), priorModel); | ||||
|   initialPose.insert(kAnchorKey, Pose3()); | ||||
|   pose3graph.emplace_shared<PriorFactor<Pose3> >(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" <<std::endl; | ||||
|   } else { | ||||
|     cout << " \n\n\n\n  performing more than 1 GN iterations \n\n\n" << endl; | ||||
|     params.setVerbosity("TERMINATION"); | ||||
|   } | ||||
|   GaussNewtonOptimizer optimizer(pose3graph, initialPose, params); | ||||
|  | @ -345,9 +332,9 @@ Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) { | |||
| 
 | ||||
|   // put into Values structure
 | ||||
|   Values estimate; | ||||
|   for(const Values::ConstKeyValuePair& key_value: GNresult) { | ||||
|   for (const auto& key_value : GNresult) { | ||||
|     Key key = key_value.key; | ||||
|     if (key != keyAnchor) { | ||||
|     if (key != kAnchorKey) { | ||||
|       const Pose3& pose = GNresult.at<Pose3>(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<Pose3>(key).translation();
 | ||||
|   //      const Rot3& rot = orientations.at<Rot3>(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
 | ||||
|  |  | |||
|  | @ -20,40 +20,68 @@ | |||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| #include <gtsam/geometry/Rot3.h> | ||||
| #include <gtsam/inference/graph.h> | ||||
| #include <gtsam/linear/GaussianFactorGraph.h> | ||||
| #include <gtsam/linear/VectorValues.h> | ||||
| #include <gtsam/inference/graph.h> | ||||
| #include <gtsam/geometry/Rot3.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| typedef std::map<Key, std::vector<size_t> > KeyVectorMap; | ||||
| typedef std::map<Key, Rot3 > KeyRotMap; | ||||
| typedef std::map<Key, Rot3> 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<Pose3> | ||||
|    */ | ||||
|   static Values computeOrientationsChordal( | ||||
|       const NonlinearFactorGraph& pose3Graph); | ||||
| 
 | ||||
| GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph); | ||||
|   /**
 | ||||
|    * Return the orientations of a graph including only BetweenFactors<Pose3> | ||||
|    */ | ||||
|   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
 | ||||
|  |  | |||
|  | @ -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<Key, Pose3> 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<Key, Pose3> 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<BetweenFactor<Pose3>::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<Pose3>(id1, id2, Pose3(R,t), model)); | ||||
|       graph->push_back(factor); | ||||
|       factors.emplace_back(new BetweenFactor<Pose3>( | ||||
|           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<Pose3>(id1, id2, Pose3(R,t), model)); | ||||
|       graph->push_back(factor); | ||||
|       factors.emplace_back(new BetweenFactor<Pose3>( | ||||
|           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); | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -18,6 +18,7 @@ | |||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/slam/BetweenFactor.h> | ||||
| #include <gtsam/geometry/Cal3Bundler.h> | ||||
| #include <gtsam/geometry/PinholeCamera.h> | ||||
| #include <gtsam/geometry/Point2.h> | ||||
|  | @ -34,6 +35,7 @@ | |||
| #include <utility> // for pair | ||||
| #include <vector> | ||||
| #include <iosfwd> | ||||
| #include <map> | ||||
| 
 | ||||
| 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<gtsam::BetweenFactor<Pose3>::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<Key, Pose3> parse3DPoses(const std::string& filename); | ||||
| 
 | ||||
| /// Load TORO 3D Graph
 | ||||
| GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); | ||||
| 
 | ||||
| /// A measurement with its camera index
 | ||||
|  |  | |||
|  | @ -16,8 +16,8 @@ | |||
|  */ | ||||
| 
 | ||||
| 
 | ||||
| #include <gtsam/slam/BetweenFactor.h> | ||||
| #include <gtsam/slam/dataset.h> | ||||
| #include <gtsam/slam/BetweenFactor.h> | ||||
| #include <gtsam/geometry/Pose2.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/base/TestableAssertions.h> | ||||
|  | @ -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<Pose3> 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<Pose3> 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<Key, Key>; | ||||
|   std::vector<KeyPair> 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<BetweenFactor<Pose3>>( | ||||
|         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<BetweenFactor<Pose3>>(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<BetweenFactor<Pose3> >(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<BetweenFactor<Pose3> >(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<BetweenFactor<Pose3> >(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<BetweenFactor<Pose3> >(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<BetweenFactor<Pose3> >(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<BetweenFactor<Pose3> >(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<Pose3>(j), 1e-5)); | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -70,6 +70,17 @@ NonlinearFactorGraph graph() { | |||
|   g.add(PriorFactor<Pose3>(x0, pose0, model)); | ||||
|   return g; | ||||
| } | ||||
| 
 | ||||
| NonlinearFactorGraph graph2() { | ||||
|   NonlinearFactorGraph g; | ||||
|   g.add(BetweenFactor<Pose3>(x0, x1, pose0.between(pose1), noiseModel::Isotropic::Precision(6, 1.0))); | ||||
|   g.add(BetweenFactor<Pose3>(x1, x2, pose1.between(pose2), noiseModel::Isotropic::Precision(6, 1.0))); | ||||
|   g.add(BetweenFactor<Pose3>(x2, x3, pose2.between(pose3), noiseModel::Isotropic::Precision(6, 1.0))); | ||||
|   g.add(BetweenFactor<Pose3>(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<Pose3>(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<Pose3>(x0, pose0, model)); | ||||
|   return g; | ||||
| } | ||||
| } | ||||
| 
 | ||||
| /* *************************************************************************** */ | ||||
|  | @ -91,6 +102,19 @@ TEST( InitializePose3, orientations ) { | |||
|   EXPECT(assert_equal(simple::R3, initial.at<Rot3>(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<Rot3>(x0), 1e-6)); | ||||
|   EXPECT(assert_equal(simple::R1, initial.at<Rot3>(x1), 1e-6)); | ||||
|   EXPECT(assert_equal(simple::R2, initial.at<Rot3>(x2), 1e-6)); | ||||
|   EXPECT(assert_equal(simple::R3, initial.at<Rot3>(x3), 1e-6)); | ||||
| } | ||||
| 
 | ||||
| /* *************************************************************************** */ | ||||
| TEST( InitializePose3, orientationsGradientSymbolicGraph ) { | ||||
|   NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); | ||||
|  |  | |||
|  | @ -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<Pose2>(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<Pose2>(currentKey).print("Batch Estimate:"); | ||||
|       smootherISAM2.calculateEstimate<Pose2>(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<Pose2>(currentKey).print("Batch Estimate:"); | ||||
|     smootherISAM2.calculateEstimate<Pose2>(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
 | ||||
|  |  | |||
|  | @ -505,132 +505,130 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor { | |||
| //*************************************************************************
 | ||||
| // nonlinear
 | ||||
| //*************************************************************************
 | ||||
| // #include <gtsam_unstable/nonlinear/sequentialSummarization.h>
 | ||||
| // 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 <gtsam_unstable/nonlinear/FixedLagSmoother.h> | ||||
| class FixedLagSmootherKeyTimestampMapValue { | ||||
|   FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp); | ||||
|   FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); | ||||
| }; | ||||
| 
 | ||||
| // #include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
 | ||||
| // 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 <gtsam_unstable/nonlinear/FixedLagSmoother.h>
 | ||||
| // 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 <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
 | ||||
| // virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
 | ||||
| //   BatchFixedLagSmoother();
 | ||||
| //   BatchFixedLagSmoother(double smootherLag);
 | ||||
| //   BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params);
 | ||||
| // 
 | ||||
| //   gtsam::LevenbergMarquardtParams params() const;
 | ||||
| // };
 | ||||
| // 
 | ||||
| // #include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
 | ||||
| // virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
 | ||||
| //   IncrementalFixedLagSmoother();
 | ||||
| //   IncrementalFixedLagSmoother(double smootherLag);
 | ||||
| //   IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params);
 | ||||
| // 
 | ||||
| //   gtsam::ISAM2Params params() const;
 | ||||
| // };
 | ||||
| // 
 | ||||
| // #include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
 | ||||
| // 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 <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
 | ||||
| // 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 <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
 | ||||
| // 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 <gtsam_unstable/nonlinear/FixedLagSmoother.h> | ||||
| 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 <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h> | ||||
| virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { | ||||
|   BatchFixedLagSmoother(); | ||||
|   BatchFixedLagSmoother(double smootherLag); | ||||
|   BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); | ||||
| 
 | ||||
|   gtsam::LevenbergMarquardtParams params() const; | ||||
|   template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3, | ||||
|                      gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, | ||||
|                      Vector, Matrix}> | ||||
|   VALUE calculateEstimate(size_t key) const; | ||||
| }; | ||||
| 
 | ||||
| #include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h> | ||||
| virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { | ||||
|   IncrementalFixedLagSmoother(); | ||||
|   IncrementalFixedLagSmoother(double smootherLag); | ||||
|   IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); | ||||
| 
 | ||||
|   gtsam::ISAM2Params params() const; | ||||
| }; | ||||
| 
 | ||||
| #include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h> | ||||
| 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 <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h> | ||||
| 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 <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h> | ||||
| 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
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue