Merge branch 'develop' into python_examples
mbway
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