From bb6aea3b8f535c16053823b0c69e48fd26f93d6a Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 18 Aug 2020 10:55:45 -0400 Subject: [PATCH] Remove python folder temporarily --- python/CMakeLists.txt | 104 ----- python/README.md | 87 ---- python/gtsam/__init__.py | 27 -- .../gtsam/examples/DogLegOptimizerExample.py | 118 ------ python/gtsam/examples/GPSFactorExample.py | 56 --- python/gtsam/examples/ImuFactorExample.py | 180 -------- .../gtsam/examples/ImuFactorISAM2Example.py | 166 -------- python/gtsam/examples/OdometryExample.py | 69 --- .../examples/PlanarManipulatorExample.py | 332 --------------- python/gtsam/examples/PlanarSLAMExample.py | 81 ---- python/gtsam/examples/Pose2SLAMExample.py | 96 ----- python/gtsam/examples/Pose2SLAMExample_g2o.py | 88 ---- python/gtsam/examples/Pose3SLAMExample_g2o.py | 71 ---- ...Pose3SLAMExample_initializePose3Chordal.py | 35 -- .../gtsam/examples/PreintegrationExample.py | 141 ------ python/gtsam/examples/README.md | 52 --- python/gtsam/examples/SFMExample.py | 122 ------ python/gtsam/examples/SFMdata.py | 40 -- python/gtsam/examples/SimpleRotation.py | 84 ---- python/gtsam/examples/VisualISAM2Example.py | 153 ------- python/gtsam/examples/VisualISAMExample.py | 105 ----- python/gtsam/examples/__init__.py | 0 python/gtsam/gtsam.tpl | 30 -- python/gtsam/imuBias.py | 1 - python/gtsam/noiseModel.py | 1 - python/gtsam/preamble.h | 4 - python/gtsam/specializations.h | 4 - python/gtsam/symbol_shorthand.py | 1 - python/gtsam/tests/testScenarioRunner.py | 47 -- python/gtsam/tests/test_Cal3Unified.py | 38 -- python/gtsam/tests/test_FrobeniusFactor.py | 56 --- .../gtsam/tests/test_GaussianFactorGraph.py | 92 ---- python/gtsam/tests/test_JacobianFactor.py | 92 ---- python/gtsam/tests/test_KalmanFilter.py | 83 ---- python/gtsam/tests/test_KarcherMeanFactor.py | 80 ---- .../gtsam/tests/test_LocalizationExample.py | 64 --- python/gtsam/tests/test_NonlinearOptimizer.py | 83 ---- python/gtsam/tests/test_OdometryExample.py | 59 --- python/gtsam/tests/test_PlanarSLAMExample.py | 78 ---- python/gtsam/tests/test_Pose2.py | 32 -- python/gtsam/tests/test_Pose2SLAMExample.py | 76 ---- python/gtsam/tests/test_Pose3.py | 70 --- python/gtsam/tests/test_Pose3SLAMExample.py | 60 --- python/gtsam/tests/test_PriorFactor.py | 61 --- python/gtsam/tests/test_SFMExample.py | 83 ---- python/gtsam/tests/test_SO4.py | 59 --- python/gtsam/tests/test_SOn.py | 59 --- python/gtsam/tests/test_Scenario.py | 56 --- python/gtsam/tests/test_SimpleCamera.py | 45 -- python/gtsam/tests/test_StereoVOExample.py | 82 ---- python/gtsam/tests/test_Triangulation.py | 80 ---- python/gtsam/tests/test_Values.py | 85 ---- python/gtsam/tests/test_VisualISAMExample.py | 57 --- python/gtsam/tests/test_dataset.py | 45 -- python/gtsam/tests/test_dsf_map.py | 40 -- python/gtsam/tests/test_initialize_pose3.py | 88 ---- python/gtsam/tests/test_logging_optimizer.py | 108 ----- python/gtsam/utils/__init__.py | 0 python/gtsam/utils/circlePose3.py | 35 -- python/gtsam/utils/logging_optimizer.py | 52 --- python/gtsam/utils/plot.py | 401 ------------------ python/gtsam/utils/test_case.py | 31 -- python/gtsam/utils/visual_data_generator.py | 119 ------ python/gtsam/utils/visual_isam.py | 131 ------ python/gtsam_unstable/__init__.py | 1 - .../examples/FixedLagSmootherExample.py | 92 ---- .../examples/TimeOfArrivalExample.py | 128 ------ python/gtsam_unstable/examples/__init__.py | 0 python/gtsam_unstable/gtsam_unstable.tpl | 32 -- python/gtsam_unstable/preamble.h | 0 python/gtsam_unstable/specializations.h | 0 python/gtsam_unstable/tests/__init__.py | 0 .../tests/test_FixedLagSmootherExample.py | 128 ------ python/requirements.txt | 2 - python/setup.py.in | 49 --- 75 files changed, 5407 deletions(-) delete mode 100644 python/CMakeLists.txt delete mode 100644 python/README.md delete mode 100644 python/gtsam/__init__.py delete mode 100644 python/gtsam/examples/DogLegOptimizerExample.py delete mode 100644 python/gtsam/examples/GPSFactorExample.py delete mode 100644 python/gtsam/examples/ImuFactorExample.py delete mode 100644 python/gtsam/examples/ImuFactorISAM2Example.py delete mode 100644 python/gtsam/examples/OdometryExample.py delete mode 100644 python/gtsam/examples/PlanarManipulatorExample.py delete mode 100644 python/gtsam/examples/PlanarSLAMExample.py delete mode 100644 python/gtsam/examples/Pose2SLAMExample.py delete mode 100644 python/gtsam/examples/Pose2SLAMExample_g2o.py delete mode 100644 python/gtsam/examples/Pose3SLAMExample_g2o.py delete mode 100644 python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py delete mode 100644 python/gtsam/examples/PreintegrationExample.py delete mode 100644 python/gtsam/examples/README.md delete mode 100644 python/gtsam/examples/SFMExample.py delete mode 100644 python/gtsam/examples/SFMdata.py delete mode 100644 python/gtsam/examples/SimpleRotation.py delete mode 100644 python/gtsam/examples/VisualISAM2Example.py delete mode 100644 python/gtsam/examples/VisualISAMExample.py delete mode 100644 python/gtsam/examples/__init__.py delete mode 100644 python/gtsam/gtsam.tpl delete mode 100644 python/gtsam/imuBias.py delete mode 100644 python/gtsam/noiseModel.py delete mode 100644 python/gtsam/preamble.h delete mode 100644 python/gtsam/specializations.h delete mode 100644 python/gtsam/symbol_shorthand.py delete mode 100644 python/gtsam/tests/testScenarioRunner.py delete mode 100644 python/gtsam/tests/test_Cal3Unified.py delete mode 100644 python/gtsam/tests/test_FrobeniusFactor.py delete mode 100644 python/gtsam/tests/test_GaussianFactorGraph.py delete mode 100644 python/gtsam/tests/test_JacobianFactor.py delete mode 100644 python/gtsam/tests/test_KalmanFilter.py delete mode 100644 python/gtsam/tests/test_KarcherMeanFactor.py delete mode 100644 python/gtsam/tests/test_LocalizationExample.py delete mode 100644 python/gtsam/tests/test_NonlinearOptimizer.py delete mode 100644 python/gtsam/tests/test_OdometryExample.py delete mode 100644 python/gtsam/tests/test_PlanarSLAMExample.py delete mode 100644 python/gtsam/tests/test_Pose2.py delete mode 100644 python/gtsam/tests/test_Pose2SLAMExample.py delete mode 100644 python/gtsam/tests/test_Pose3.py delete mode 100644 python/gtsam/tests/test_Pose3SLAMExample.py delete mode 100644 python/gtsam/tests/test_PriorFactor.py delete mode 100644 python/gtsam/tests/test_SFMExample.py delete mode 100644 python/gtsam/tests/test_SO4.py delete mode 100644 python/gtsam/tests/test_SOn.py delete mode 100644 python/gtsam/tests/test_Scenario.py delete mode 100644 python/gtsam/tests/test_SimpleCamera.py delete mode 100644 python/gtsam/tests/test_StereoVOExample.py delete mode 100644 python/gtsam/tests/test_Triangulation.py delete mode 100644 python/gtsam/tests/test_Values.py delete mode 100644 python/gtsam/tests/test_VisualISAMExample.py delete mode 100644 python/gtsam/tests/test_dataset.py delete mode 100644 python/gtsam/tests/test_dsf_map.py delete mode 100644 python/gtsam/tests/test_initialize_pose3.py delete mode 100644 python/gtsam/tests/test_logging_optimizer.py delete mode 100644 python/gtsam/utils/__init__.py delete mode 100644 python/gtsam/utils/circlePose3.py delete mode 100644 python/gtsam/utils/logging_optimizer.py delete mode 100644 python/gtsam/utils/plot.py delete mode 100644 python/gtsam/utils/test_case.py delete mode 100644 python/gtsam/utils/visual_data_generator.py delete mode 100644 python/gtsam/utils/visual_isam.py delete mode 100644 python/gtsam_unstable/__init__.py delete mode 100644 python/gtsam_unstable/examples/FixedLagSmootherExample.py delete mode 100644 python/gtsam_unstable/examples/TimeOfArrivalExample.py delete mode 100644 python/gtsam_unstable/examples/__init__.py delete mode 100644 python/gtsam_unstable/gtsam_unstable.tpl delete mode 100644 python/gtsam_unstable/preamble.h delete mode 100644 python/gtsam_unstable/specializations.h delete mode 100644 python/gtsam_unstable/tests/__init__.py delete mode 100644 python/gtsam_unstable/tests/test_FixedLagSmootherExample.py delete mode 100644 python/requirements.txt delete mode 100644 python/setup.py.in diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt deleted file mode 100644 index 81a6c6c4d..000000000 --- a/python/CMakeLists.txt +++ /dev/null @@ -1,104 +0,0 @@ -set(GTSAM_PYTHON_BUILD_DIRECTORY ${PROJECT_BINARY_DIR}/python) - -if(GTSAM_BUILD_PYTHON) - # Generate setup.py. - file(READ "${PROJECT_SOURCE_DIR}/README.md" README_CONTENTS) - configure_file(${PROJECT_SOURCE_DIR}/python/setup.py.in - ${GTSAM_PYTHON_BUILD_DIRECTORY}/setup.py) - - set(WRAP_USE_CUSTOM_PYTHON_LIBRARY ${GTSAM_USE_CUSTOM_PYTHON_LIBRARY}) - set(WRAP_PYTHON_VERSION ${GTSAM_PYTHON_VERSION}) - - include(PybindWrap) - - add_custom_target(gtsam_header DEPENDS "${PROJECT_SOURCE_DIR}/gtsam/gtsam.i") - add_custom_target(gtsam_unstable_header DEPENDS "${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i") - - # ignoring the non-concrete types (type aliases) - set(ignore - gtsam::Point2 - gtsam::Point3 - gtsam::LieVector - gtsam::LieMatrix - gtsam::ISAM2ThresholdMapValue - gtsam::FactorIndices - gtsam::FactorIndexSet - gtsam::BetweenFactorPose3s - gtsam::Point2Vector - gtsam::Pose3Vector - gtsam::KeyVector) - - pybind_wrap(gtsam_py # target - ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i # interface_header - "gtsam.cpp" # generated_cpp - "gtsam" # module_name - "gtsam" # top_namespace - "${ignore}" # ignore_classes - ${PROJECT_SOURCE_DIR}/python/gtsam/gtsam.tpl - gtsam # libs - "gtsam;gtsam_header" # dependencies - ON # use_boost - ) - - set_target_properties(gtsam_py PROPERTIES - INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" - INSTALL_RPATH_USE_LINK_PATH TRUE - OUTPUT_NAME "gtsam" - LIBRARY_OUTPUT_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam" - DEBUG_POSTFIX "" # Otherwise you will have a wrong name - RELWITHDEBINFO_POSTFIX "" # Otherwise you will have a wrong name - ) - - set(GTSAM_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam) - - # Symlink all tests .py files to build folder. - create_symlinks("${CMAKE_CURRENT_SOURCE_DIR}/gtsam" - "${GTSAM_MODULE_PATH}") - - if(GTSAM_UNSTABLE_BUILD_PYTHON) - set(ignore - gtsam::Point2 - gtsam::Point3 - gtsam::LieVector - gtsam::LieMatrix - gtsam::ISAM2ThresholdMapValue - gtsam::FactorIndices - gtsam::FactorIndexSet - gtsam::BetweenFactorPose3s - gtsam::Point2Vector - gtsam::Pose3Vector - gtsam::KeyVector - gtsam::FixedLagSmootherKeyTimestampMapValue) - pybind_wrap(gtsam_unstable_py # target - ${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header - "gtsam_unstable.cpp" # generated_cpp - "gtsam_unstable" # module_name - "gtsam" # top_namespace - "${ignore}" # ignore_classes - ${PROJECT_SOURCE_DIR}/python/gtsam_unstable/gtsam_unstable.tpl - gtsam_unstable # libs - "gtsam_unstable;gtsam_unstable_header" # dependencies - ON # use_boost - ) - - set_target_properties(gtsam_unstable_py PROPERTIES - INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" - INSTALL_RPATH_USE_LINK_PATH TRUE - OUTPUT_NAME "gtsam_unstable" - LIBRARY_OUTPUT_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable" - DEBUG_POSTFIX "" # Otherwise you will have a wrong name - RELWITHDEBINFO_POSTFIX "" # Otherwise you will have a wrong name - ) - - set(GTSAM_UNSTABLE_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable) - - # Symlink all tests .py files to build folder. - create_symlinks("${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable" - "${GTSAM_UNSTABLE_MODULE_PATH}") - endif() - - set(GTSAM_PYTHON_INSTALL_TARGET python-install) - add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} - COMMAND ${PYTHON_EXECUTABLE} ${GTSAM_PYTHON_BUILD_DIRECTORY}/setup.py install - WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) -endif() diff --git a/python/README.md b/python/README.md deleted file mode 100644 index e418cbede..000000000 --- a/python/README.md +++ /dev/null @@ -1,87 +0,0 @@ -# README - -# Python Wrapper - -This is the Python wrapper around the GTSAM C++ library. We use Cython to generate the bindings to the underlying C++ code. - -## Requirements - -- If you want to build the GTSAM python library for a specific python version (eg 3.6), - use the `-DGTSAM_PYTHON_VERSION=3.6` option when running `cmake` otherwise the default interpreter will be used. -- If the interpreter is inside an environment (such as an anaconda environment or virtualenv environment), - then the environment should be active while building GTSAM. -- This wrapper needs `Cython(>=0.25.2)`, `backports_abc(>=0.5)`, and `numpy(>=1.11.0)`. These can be installed as follows: - - ```bash - pip install -r /cython/requirements.txt - ``` - -- For compatibility with GTSAM's Eigen version, it contains its own cloned version of [Eigency](https://github.com/wouterboomsma/eigency.git), - named `gtsam_eigency`, to interface between C++'s Eigen and Python's numpy. - -## Install - -- Run cmake with the `GTSAM_INSTALL_CYTHON_TOOLBOX` cmake flag enabled to configure building the wrapper. The wrapped module will be built and copied to the directory defined by `GTSAM_CYTHON_INSTALL_PATH`, which is by default `/cython` in Release mode and `/cython` for other modes. - -- Build GTSAM and the wrapper with `make`. - -- To install, simply run `make python-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. - -- You can also directly run `make python-install` without running `make`, and it will compile all the dependencies accordingly. - -## Unit Tests - -The Cython toolbox also has a small set of unit tests located in the -test directory. To run them: - - ```bash - cd - python -m unittest discover - ``` - -## Utils - -TODO - -## Examples - -TODO - -## Writing Your Own Scripts - -See the tests for examples. - -### Some Important Notes: - -- Vector/Matrix: - - - GTSAM expects double-precision floating point vectors and matrices. - Hence, you should pass numpy matrices with `dtype=float`, or `float64`. - - Also, GTSAM expects _column-major_ matrices, unlike the default storage - scheme in numpy. Hence, you should pass column-major matrices to GTSAM using - the flag order='F'. And you always get column-major matrices back. - For more details, see [this link](https://github.com/wouterboomsma/eigency#storage-layout---why-arrays-are-sometimes-transposed). - - Passing row-major matrices of different dtype, e.g. `int`, will also work - as the wrapper converts them to column-major and dtype float for you, - using numpy.array.astype(float, order='F', copy=False). - However, this will result a copy if your matrix is not in the expected type - and storage order. - -- Inner namespace: Classes in inner namespace will be prefixed by \_ in Python. - - Examples: `noiseModel_Gaussian`, `noiseModel_mEstimator_Tukey` - -- Casting from a base class to a derive class must be done explicitly. - - Examples: - - ```python - noiseBase = factor.noiseModel() - noiseGaussian = dynamic_cast_noiseModel_Gaussian_noiseModel_Base(noiseBase) - ``` - -## Wrapping Custom GTSAM-based Project - -Please refer to the template project and the corresponding tutorial available [here](https://github.com/borglab/GTSAM-project-python). diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py deleted file mode 100644 index e6fd8c9c8..000000000 --- a/python/gtsam/__init__.py +++ /dev/null @@ -1,27 +0,0 @@ -from .gtsam import * - - -def _init(): - """This function is to add shims for the long-gone Point2 and Point3 types""" - - import numpy as np - - global Point2 # export function - - def Point2(x=0, y=0): - """Shim for the deleted Point2 type.""" - return np.array([x, y], dtype=float) - - global Point3 # export function - - def Point3(x=0, y=0, z=0): - """Shim for the deleted Point3 type.""" - return np.array([x, y, z], dtype=float) - - # for interactive debugging - if __name__ == "__main__": - # we want all definitions accessible - globals().update(locals()) - - -_init() diff --git a/python/gtsam/examples/DogLegOptimizerExample.py b/python/gtsam/examples/DogLegOptimizerExample.py deleted file mode 100644 index 26f4fef84..000000000 --- a/python/gtsam/examples/DogLegOptimizerExample.py +++ /dev/null @@ -1,118 +0,0 @@ -""" -GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, -Atlanta, Georgia 30332-0415 -All Rights Reserved - -See LICENSE for the license information - -Example comparing DoglegOptimizer with Levenberg-Marquardt. -Author: Frank Dellaert -""" -# pylint: disable=no-member, invalid-name - -import math -import argparse - -import gtsam -import matplotlib.pyplot as plt -import numpy as np - - -def run(args): - """Test Dogleg vs LM, inspired by issue #452.""" - - # print parameters - print("num samples = {}, deltaInitial = {}".format( - args.num_samples, args.delta)) - - # Ground truth solution - T11 = gtsam.Pose2(0, 0, 0) - T12 = gtsam.Pose2(1, 0, 0) - T21 = gtsam.Pose2(0, 1, 0) - T22 = gtsam.Pose2(1, 1, 0) - - # Factor graph - graph = gtsam.NonlinearFactorGraph() - - # Priors - prior = gtsam.noiseModel.Isotropic.Sigma(3, 1) - graph.add(gtsam.PriorFactorPose2(11, T11, prior)) - graph.add(gtsam.PriorFactorPose2(21, T21, prior)) - - # Odometry - model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.01, 0.01, 0.3])) - graph.add(gtsam.BetweenFactorPose2(11, 12, T11.between(T12), model)) - graph.add(gtsam.BetweenFactorPose2(21, 22, T21.between(T22), model)) - - # Range - model_rho = gtsam.noiseModel.Isotropic.Sigma(1, 0.01) - graph.add(gtsam.RangeFactorPose2(12, 22, 1.0, model_rho)) - - params = gtsam.DoglegParams() - params.setDeltaInitial(args.delta) # default is 10 - - # Add progressively more noise to ground truth - sigmas = [0.01, 0.1, 0.2, 0.5, 1, 2, 5, 10, 20] - n = len(sigmas) - p_dl, s_dl, p_lm, s_lm = [0]*n, [0]*n, [0]*n, [0]*n - for i, sigma in enumerate(sigmas): - dl_fails, lm_fails = 0, 0 - # Attempt num_samples optimizations for both DL and LM - for _attempt in range(args.num_samples): - initial = gtsam.Values() - initial.insert(11, T11.retract(np.random.normal(0, sigma, 3))) - initial.insert(12, T12.retract(np.random.normal(0, sigma, 3))) - initial.insert(21, T21.retract(np.random.normal(0, sigma, 3))) - initial.insert(22, T22.retract(np.random.normal(0, sigma, 3))) - - # Run dogleg optimizer - dl = gtsam.DoglegOptimizer(graph, initial, params) - result = dl.optimize() - dl_fails += graph.error(result) > 1e-9 - - # Run - lm = gtsam.LevenbergMarquardtOptimizer(graph, initial) - result = lm.optimize() - lm_fails += graph.error(result) > 1e-9 - - # Calculate Bayes estimate of success probability - # using a beta prior of alpha=0.5, beta=0.5 - alpha, beta = 0.5, 0.5 - v = args.num_samples+alpha+beta - p_dl[i] = (args.num_samples-dl_fails+alpha)/v - p_lm[i] = (args.num_samples-lm_fails+alpha)/v - - def stddev(p): - """Calculate standard deviation.""" - return math.sqrt(p*(1-p)/(1+v)) - - s_dl[i] = stddev(p_dl[i]) - s_lm[i] = stddev(p_lm[i]) - - fmt = "sigma= {}:\tDL success {:.2f}% +/- {:.2f}%, LM success {:.2f}% +/- {:.2f}%" - print(fmt.format(sigma, - 100*p_dl[i], 100*s_dl[i], - 100*p_lm[i], 100*s_lm[i])) - - if args.plot: - fig, ax = plt.subplots() - dl_plot = plt.errorbar(sigmas, p_dl, yerr=s_dl, label="Dogleg") - lm_plot = plt.errorbar(sigmas, p_lm, yerr=s_lm, label="LM") - plt.title("Dogleg emprical success vs. LM") - plt.legend(handles=[dl_plot, lm_plot]) - ax.set_xlim(0, sigmas[-1]+1) - ax.set_ylim(0, 1) - plt.show() - - -if __name__ == "__main__": - parser = argparse.ArgumentParser( - description="Compare Dogleg and LM success rates") - parser.add_argument("-n", "--num_samples", type=int, default=1000, - help="Number of samples for each sigma") - parser.add_argument("-d", "--delta", type=float, default=10.0, - help="Initial delta for dogleg") - parser.add_argument("-p", "--plot", action="store_true", - help="Flag to plot results") - args = parser.parse_args() - run(args) diff --git a/python/gtsam/examples/GPSFactorExample.py b/python/gtsam/examples/GPSFactorExample.py deleted file mode 100644 index 0bc0d1bf3..000000000 --- a/python/gtsam/examples/GPSFactorExample.py +++ /dev/null @@ -1,56 +0,0 @@ -""" -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 - -Simple robot motion example, with prior and one GPS measurements -Author: Mandy Xie -""" -# pylint: disable=invalid-name, E1101 - -from __future__ import print_function - -import numpy as np - -import gtsam - -import matplotlib.pyplot as plt -import gtsam.utils.plot as gtsam_plot - -# ENU Origin is where the plane was in hold next to runway -lat0 = 33.86998 -lon0 = -84.30626 -h0 = 274 - -# Create noise models -GPS_NOISE = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) -PRIOR_NOISE = gtsam.noiseModel.Isotropic.Sigma(6, 0.25) - -# Create an empty nonlinear factor graph -graph = gtsam.NonlinearFactorGraph() - -# Add a prior on the first point, setting it to the origin -# A prior factor consists of a mean and a noise model (covariance matrix) -priorMean = gtsam.Pose3() # prior at origin -graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE)) - -# Add GPS factors -gps = gtsam.Point3(lat0, lon0, h0) -graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE)) -print("\nFactor Graph:\n{}".format(graph)) - -# Create the data structure to hold the initialEstimate estimate to the solution -# For illustrative purposes, these have been deliberately set to incorrect values -initial = gtsam.Values() -initial.insert(1, gtsam.Pose3()) -print("\nInitial Estimate:\n{}".format(initial)) - -# optimize using Levenberg-Marquardt optimization -params = gtsam.LevenbergMarquardtParams() -optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) -result = optimizer.optimize() -print("\nFinal Result:\n{}".format(result)) - diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py deleted file mode 100644 index eec7c5ebd..000000000 --- a/python/gtsam/examples/ImuFactorExample.py +++ /dev/null @@ -1,180 +0,0 @@ -""" -GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, -Atlanta, Georgia 30332-0415 -All Rights Reserved - -See LICENSE for the license information - -A script validating and demonstrating the ImuFactor inference. - -Author: Frank Dellaert, Varun Agrawal -""" - -from __future__ import print_function - -import argparse -import math - -import gtsam -import matplotlib.pyplot as plt -import numpy as np -from gtsam.symbol_shorthand import B, V, X -from gtsam.utils.plot import plot_pose3 -from mpl_toolkits.mplot3d import Axes3D - -from PreintegrationExample import POSES_FIG, PreintegrationExample - -BIAS_KEY = B(0) - - -np.set_printoptions(precision=3, suppress=True) - - -class ImuFactorExample(PreintegrationExample): - - def __init__(self, twist_scenario="sick_twist"): - self.velocity = np.array([2, 0, 0]) - self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) - self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) - - # Choose one of these twists to change scenario: - twist_scenarios = dict( - zero_twist=(np.zeros(3), np.zeros(3)), - forward_twist=(np.zeros(3), self.velocity), - loop_twist=(np.array([0, -math.radians(30), 0]), self.velocity), - sick_twist=(np.array([math.radians(30), -math.radians(30), 0]), - self.velocity) - ) - - accBias = np.array([-0.3, 0.1, 0.2]) - gyroBias = np.array([0.1, 0.3, -0.1]) - bias = gtsam.imuBias.ConstantBias(accBias, gyroBias) - - dt = 1e-2 - super(ImuFactorExample, self).__init__(twist_scenarios[twist_scenario], - bias, dt) - - def addPrior(self, i, graph): - state = self.scenario.navState(i) - graph.push_back(gtsam.PriorFactorPose3( - X(i), state.pose(), self.priorNoise)) - graph.push_back(gtsam.PriorFactorVector( - V(i), state.velocity(), self.velNoise)) - - def run(self, T=12, compute_covariances=False, verbose=True): - graph = gtsam.NonlinearFactorGraph() - - # initialize data structure for pre-integrated IMU measurements - pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias) - - T = 12 - num_poses = T # assumes 1 factor per second - initial = gtsam.Values() - initial.insert(BIAS_KEY, self.actualBias) - - # simulate the loop - i = 0 # state index - initial_state_i = self.scenario.navState(0) - initial.insert(X(i), initial_state_i.pose()) - initial.insert(V(i), initial_state_i.velocity()) - - # add prior on beginning - self.addPrior(0, graph) - - for k, t in enumerate(np.arange(0, T, self.dt)): - # get measurements and add them to PIM - measuredOmega = self.runner.measuredAngularVelocity(t) - measuredAcc = self.runner.measuredSpecificForce(t) - pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt) - - # Plot IMU many times - if k % 10 == 0: - self.plotImu(t, measuredOmega, measuredAcc) - - if (k+1) % int(1 / self.dt) == 0: - # Plot every second - self.plotGroundTruthPose(t, scale=1) - plt.title("Ground Truth Trajectory") - - # create IMU factor every second - factor = gtsam.ImuFactor(X(i), V(i), - X(i + 1), V(i + 1), - BIAS_KEY, pim) - graph.push_back(factor) - - if verbose: - print(factor) - print(pim.predict(actual_state_i, self.actualBias)) - - pim.resetIntegration() - - rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3)*0.1) - translationNoise = gtsam.Point3(*np.random.randn(3)*1) - poseNoise = gtsam.Pose3(rotationNoise, translationNoise) - - actual_state_i = self.scenario.navState(t + self.dt) - print("Actual state at {0}:\n{1}".format( - t+self.dt, actual_state_i)) - - noisy_state_i = gtsam.NavState( - actual_state_i.pose().compose(poseNoise), - actual_state_i.velocity() + np.random.randn(3)*0.1) - - initial.insert(X(i+1), noisy_state_i.pose()) - initial.insert(V(i+1), noisy_state_i.velocity()) - i += 1 - - # add priors on end - # self.addPrior(num_poses - 1, graph) - - initial.print_("Initial values:") - - # optimize using Levenberg-Marquardt optimization - params = gtsam.LevenbergMarquardtParams() - params.setVerbosityLM("SUMMARY") - optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) - result = optimizer.optimize() - - result.print_("Optimized values:") - - if compute_covariances: - # Calculate and print marginal covariances - marginals = gtsam.Marginals(graph, result) - print("Covariance on bias:\n", - marginals.marginalCovariance(BIAS_KEY)) - for i in range(num_poses): - print("Covariance on pose {}:\n{}\n".format( - i, marginals.marginalCovariance(X(i)))) - print("Covariance on vel {}:\n{}\n".format( - i, marginals.marginalCovariance(V(i)))) - - # Plot resulting poses - i = 0 - while result.exists(X(i)): - pose_i = result.atPose3(X(i)) - plot_pose3(POSES_FIG+1, pose_i, 1) - i += 1 - plt.title("Estimated Trajectory") - - gtsam.utils.plot.set_axes_equal(POSES_FIG+1) - - print("Bias Values", result.atConstantBias(BIAS_KEY)) - - plt.ioff() - plt.show() - - -if __name__ == '__main__': - parser = argparse.ArgumentParser("ImuFactorExample.py") - parser.add_argument("--twist_scenario", - default="sick_twist", - choices=("zero_twist", "forward_twist", "loop_twist", "sick_twist")) - parser.add_argument("--time", "-T", default=12, - type=int, help="Total time in seconds") - parser.add_argument("--compute_covariances", - default=False, action='store_true') - parser.add_argument("--verbose", default=False, action='store_true') - args = parser.parse_args() - - ImuFactorExample(args.twist_scenario).run( - args.time, args.compute_covariances, args.verbose) diff --git a/python/gtsam/examples/ImuFactorISAM2Example.py b/python/gtsam/examples/ImuFactorISAM2Example.py deleted file mode 100644 index 98f6218ea..000000000 --- a/python/gtsam/examples/ImuFactorISAM2Example.py +++ /dev/null @@ -1,166 +0,0 @@ -""" -iSAM2 example with ImuFactor. -Author: Robert Truax (C++), Frank Dellaert, Varun Agrawal -""" -# pylint: disable=invalid-name, E1101 - -from __future__ import print_function - -import math - -import gtsam -import matplotlib.pyplot as plt -import numpy as np -from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2, - ConstantTwistScenario, ImuFactor, NonlinearFactorGraph, - PinholeCameraCal3_S2, Point3, Pose3, - PriorFactorConstantBias, PriorFactorPose3, - PriorFactorVector, Rot3, Values) -from gtsam.symbol_shorthand import B, V, X -from gtsam.utils import plot -from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 - - -def vector3(x, y, z): - """Create 3d double numpy array.""" - return np.array([x, y, z], dtype=np.float) - - -g = 9.81 -n_gravity = vector3(0, 0, -g) - - -def preintegration_parameters(): - # IMU preintegration parameters - # Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis - PARAMS = gtsam.PreintegrationParams.MakeSharedU(g) - I = np.eye(3) - PARAMS.setAccelerometerCovariance(I * 0.1) - PARAMS.setGyroscopeCovariance(I * 0.1) - PARAMS.setIntegrationCovariance(I * 0.1) - PARAMS.setUse2ndOrderCoriolis(False) - PARAMS.setOmegaCoriolis(vector3(0, 0, 0)) - - BIAS_COVARIANCE = gtsam.noiseModel.Isotropic.Variance(6, 0.1) - DELTA = Pose3(Rot3.Rodrigues(0, 0, 0), - Point3(0.05, -0.10, 0.20)) - - return PARAMS, BIAS_COVARIANCE, DELTA - - -def get_camera(radius): - up = Point3(0, 0, 1) - target = Point3(0, 0, 0) - position = Point3(radius, 0, 0) - camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2()) - return camera - - -def get_scenario(radius, pose_0, angular_velocity, delta_t): - """Create the set of ground-truth landmarks and poses""" - - angular_velocity_vector = vector3(0, -angular_velocity, 0) - linear_velocity_vector = vector3(radius * angular_velocity, 0, 0) - scenario = ConstantTwistScenario( - angular_velocity_vector, linear_velocity_vector, pose_0) - - return scenario - - -def IMU_example(): - """Run iSAM 2 example with IMU factor.""" - - # Start with a camera on x-axis looking at origin - radius = 30 - camera = get_camera(radius) - pose_0 = camera.pose() - - delta_t = 1.0/18 # makes for 10 degrees per step - angular_velocity = math.radians(180) # rad/sec - scenario = get_scenario(radius, pose_0, angular_velocity, delta_t) - - PARAMS, BIAS_COVARIANCE, DELTA = preintegration_parameters() - - # Create a factor graph - graph = NonlinearFactorGraph() - - # Create (incremental) ISAM2 solver - isam = ISAM2() - - # Create the initial estimate to the solution - # Intentionally initialize the variables off from the ground truth - initialEstimate = Values() - - # Add a prior on pose x0. This indirectly specifies where the origin is. - # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - noise = gtsam.noiseModel.Diagonal.Sigmas( - np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3])) - graph.push_back(PriorFactorPose3(X(0), pose_0, noise)) - - # Add imu priors - biasKey = B(0) - biasnoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) - biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias.ConstantBias(), - biasnoise) - graph.push_back(biasprior) - initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias()) - velnoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) - - # Calculate with correct initial velocity - n_velocity = vector3(0, angular_velocity * radius, 0) - velprior = PriorFactorVector(V(0), n_velocity, velnoise) - graph.push_back(velprior) - initialEstimate.insert(V(0), n_velocity) - - accum = gtsam.PreintegratedImuMeasurements(PARAMS) - - # Simulate poses and imu measurements, adding them to the factor graph - for i in range(80): - t = i * delta_t # simulation time - if i == 0: # First time add two poses - pose_1 = scenario.pose(delta_t) - initialEstimate.insert(X(0), pose_0.compose(DELTA)) - initialEstimate.insert(X(1), pose_1.compose(DELTA)) - elif i >= 2: # Add more poses as necessary - pose_i = scenario.pose(t) - initialEstimate.insert(X(i), pose_i.compose(DELTA)) - - if i > 0: - # Add Bias variables periodically - if i % 5 == 0: - biasKey += 1 - factor = BetweenFactorConstantBias( - biasKey - 1, biasKey, gtsam.imuBias.ConstantBias(), BIAS_COVARIANCE) - graph.add(factor) - initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias()) - - # Predict acceleration and gyro measurements in (actual) body frame - nRb = scenario.rotation(t).matrix() - bRn = np.transpose(nRb) - measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity) - measuredOmega = scenario.omega_b(t) - accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t) - - # Add Imu Factor - imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum) - graph.add(imufac) - - # insert new velocity, which is wrong - initialEstimate.insert(V(i), n_velocity) - accum.resetIntegration() - - # Incremental solution - isam.update(graph, initialEstimate) - result = isam.calculateEstimate() - plot.plot_incremental_trajectory(0, result, - start=i, scale=3, time_interval=0.01) - - # reset - graph = NonlinearFactorGraph() - initialEstimate.clear() - - plt.show() - - -if __name__ == '__main__': - IMU_example() diff --git a/python/gtsam/examples/OdometryExample.py b/python/gtsam/examples/OdometryExample.py deleted file mode 100644 index 8b519ce9a..000000000 --- a/python/gtsam/examples/OdometryExample.py +++ /dev/null @@ -1,69 +0,0 @@ -""" -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 - -Simple robot motion example, with prior and two odometry measurements -Author: Frank Dellaert -""" -# pylint: disable=invalid-name, E1101 - -from __future__ import print_function - -import numpy as np - -import gtsam - -import matplotlib.pyplot as plt -import gtsam.utils.plot as gtsam_plot - -# Create noise models -ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) -PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) - -# Create an empty nonlinear factor graph -graph = gtsam.NonlinearFactorGraph() - -# Add a prior on the first pose, setting it to the origin -# A prior factor consists of a mean and a noise model (covariance matrix) -priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin -graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE)) - -# Add odometry factors -odometry = gtsam.Pose2(2.0, 0.0, 0.0) -# For simplicity, we will use the same noise model for each odometry factor -# Create odometry (Between) factors between consecutive poses -graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE)) -graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE)) -print("\nFactor Graph:\n{}".format(graph)) - -# Create the data structure to hold the initialEstimate estimate to the solution -# For illustrative purposes, these have been deliberately set to incorrect values -initial = gtsam.Values() -initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) -initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) -initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1)) -print("\nInitial Estimate:\n{}".format(initial)) - -# optimize using Levenberg-Marquardt optimization -params = gtsam.LevenbergMarquardtParams() -optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) -result = optimizer.optimize() -print("\nFinal Result:\n{}".format(result)) - -# 5. Calculate and print marginal covariances for all variables -marginals = gtsam.Marginals(graph, result) -for i in range(1, 4): - print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i))) - -fig = plt.figure(0) -for i in range(1, 4): - gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i)) -plt.axis('equal') -plt.show() - - - diff --git a/python/gtsam/examples/PlanarManipulatorExample.py b/python/gtsam/examples/PlanarManipulatorExample.py deleted file mode 100644 index 9af4f7fcc..000000000 --- a/python/gtsam/examples/PlanarManipulatorExample.py +++ /dev/null @@ -1,332 +0,0 @@ -""" -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 - -Kinematics of three-link manipulator with GTSAM poses and product of exponential maps. -Author: Frank Dellaert -""" -# pylint: disable=invalid-name, E1101 - -from __future__ import print_function - -import math -import unittest -from functools import reduce - -import matplotlib.pyplot as plt -import numpy as np -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): - """Create 3D double numpy array.""" - return np.array([x, y, z], dtype=np.float) - - -def compose(*poses): - """Compose all Pose2 transforms given as arguments from left to right.""" - return reduce((lambda x, y: x.compose(y)), poses) - - -def vee(M): - """Pose2 vee operator.""" - return vector3(M[0, 2], M[1, 2], M[1, 0]) - - -def delta(g0, g1): - """Difference between x,y,,theta components of SE(2) poses.""" - return vector3(g1.x() - g0.x(), g1.y() - g0.y(), g1.theta() - g0.theta()) - - -def trajectory(g0, g1, N=20): - """ Create an interpolated trajectory in SE(2), treating x,y, and theta separately. - g0 and g1 are the initial and final pose, respectively. - N is the number of *intervals* - Returns N+1 poses - """ - e = delta(g0, g1) - return [Pose2(g0.x()+e[0]*t, g0.y()+e[1]*t, g0.theta()+e[2]*t) for t in np.linspace(0, 1, N)] - - -class ThreeLinkArm(object): - """Three-link arm class.""" - - def __init__(self): - self.L1 = 3.5 - self.L2 = 3.5 - self.L3 = 2.5 - self.xi1 = vector3(0, 0, 1) - self.xi2 = vector3(self.L1, 0, 1) - self.xi3 = vector3(self.L1+self.L2, 0, 1) - self.sXt0 = Pose2(0, self.L1+self.L2 + self.L3, math.radians(90)) - - def fk(self, q): - """ Forward kinematics. - Takes numpy array of joint angles, in radians. - """ - sXl1 = Pose2(0, 0, math.radians(90)) - l1Zl1 = Pose2(0, 0, q[0]) - l1Xl2 = Pose2(self.L1, 0, 0) - l2Zl2 = Pose2(0, 0, q[1]) - l2Xl3 = Pose2(self.L2, 0, 0) - l3Zl3 = Pose2(0, 0, q[2]) - l3Xt = Pose2(self.L3, 0, 0) - return compose(sXl1, l1Zl1, l1Xl2, l2Zl2, l2Xl3, l3Zl3, l3Xt) - - def jacobian(self, q): - """ Calculate manipulator Jacobian. - Takes numpy array of joint angles, in radians. - """ - a = q[0]+q[1] - b = a+q[2] - return np.array([[-self.L1*math.cos(q[0]) - self.L2*math.cos(a)-self.L3*math.cos(b), - -self.L1*math.cos(a)-self.L3*math.cos(b), - - self.L3*math.cos(b)], - [-self.L1*math.sin(q[0]) - self.L2*math.sin(a)-self.L3*math.sin(b), - -self.L1*math.sin(a)-self.L3*math.sin(b), - - self.L3*math.sin(b)], - [1, 1, 1]], np.float) - - def poe(self, q): - """ Forward kinematics. - Takes numpy array of joint angles, in radians. - """ - l1Zl1 = Pose2.Expmap(self.xi1 * q[0]) - l2Zl2 = Pose2.Expmap(self.xi2 * q[1]) - l3Zl3 = Pose2.Expmap(self.xi3 * q[2]) - return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0) - - def con(self, q): - """ Forward kinematics, conjugation form. - Takes numpy array of joint angles, in radians. - """ - def expmap(x, y, theta): - """Implement exponential map via conjugation with axis (x,y).""" - return compose(Pose2(x, y, 0), Pose2(0, 0, theta), Pose2(-x, -y, 0)) - - l1Zl1 = expmap(0.0, 0.0, q[0]) - l2Zl2 = expmap(0.0, self.L1, q[1]) - l3Zl3 = expmap(0.0, self.L1+self.L2, q[2]) - return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0) - - def ik(self, sTt_desired, e=1e-9): - """ Inverse kinematics. - Takes desired Pose2 of tool T with respect to base S. - Optional: mu, gradient descent rate; e: error norm threshold - """ - q = np.radians(vector3(30, -30, 45)) # well within workspace - error = vector3(100, 100, 100) - - while np.linalg.norm(error) > e: - error = delta(sTt_desired, self.fk(q)) - J = self.jacobian(q) - q -= np.dot(np.linalg.pinv(J), error) - - # return result in interval [-pi,pi) - return np.remainder(q+math.pi, 2*math.pi)-math.pi - - def manipulator_jacobian(self, q): - """ Calculate manipulator Jacobian. - Takes numpy array of joint angles, in radians. - Returns the manipulator Jacobian of differential twists. When multiplied with - a vector of joint velocities, will yield a single differential twist which is - the spatial velocity d(sTt)/dt * inv(sTt) of the end-effector pose. - Just like always, differential twists can be hatted and multiplied with spatial - coordinates of a point to give the spatial velocity of the point. - """ - l1Zl1 = Pose2.Expmap(self.xi1 * q[0]) - l2Zl2 = Pose2.Expmap(self.xi2 * q[1]) - # l3Zl3 = Pose2.Expmap(self.xi3 * q[2]) - - p1 = self.xi1 - # p1 = Pose2().Adjoint(self.xi1) - - sTl1 = l1Zl1 - p2 = sTl1.Adjoint(self.xi2) - - sTl2 = compose(l1Zl1, l2Zl2) - p3 = sTl2.Adjoint(self.xi3) - - differential_twists = [p1, p2, p3] - return np.stack(differential_twists, axis=1) - - def plot(self, fignum, q): - """ Plot arm. - Takes figure number, and numpy array of joint angles, in radians. - """ - fig = plt.figure(fignum) - axes = fig.gca() - - sXl1 = Pose2(0, 0, math.radians(90)) - p1 = sXl1.translation() - gtsam_plot.plot_pose2_on_axes(axes, sXl1) - - def plot_line(p, g, color): - q = g.translation() - line = np.append(p[np.newaxis], q[np.newaxis], axis=0) - axes.plot(line[:, 0], line[:, 1], color) - return q - - l1Zl1 = Pose2(0, 0, q[0]) - l1Xl2 = Pose2(self.L1, 0, 0) - sTl2 = compose(sXl1, l1Zl1, l1Xl2) - p2 = plot_line(p1, sTl2, 'r-') - gtsam_plot.plot_pose2_on_axes(axes, sTl2) - - l2Zl2 = Pose2(0, 0, q[1]) - l2Xl3 = Pose2(self.L2, 0, 0) - sTl3 = compose(sTl2, l2Zl2, l2Xl3) - p3 = plot_line(p2, sTl3, 'g-') - gtsam_plot.plot_pose2_on_axes(axes, sTl3) - - l3Zl3 = Pose2(0, 0, q[2]) - l3Xt = Pose2(self.L3, 0, 0) - sTt = compose(sTl3, l3Zl3, l3Xt) - plot_line(p3, sTt, 'b-') - gtsam_plot.plot_pose2_on_axes(axes, sTt) - - -# Create common example configurations. -Q0 = vector3(0, 0, 0) -Q1 = np.radians(vector3(-30, -45, -90)) -Q2 = np.radians(vector3(-90, 90, 0)) - - -class TestPose2SLAMExample(GtsamTestCase): - """Unit tests for functions used below.""" - - def setUp(self): - self.arm = ThreeLinkArm() - - def assertPose2Equals(self, actual, expected, tol=1e-2): - """Helper function that prints out actual and expected if not equal.""" - equal = actual.equals(expected, tol) - if not equal: - raise self.failureException( - "Poses are not equal:\n{}!={}".format(actual, expected)) - - def test_fk_arm(self): - """Make sure forward kinematics is correct for some known test configurations.""" - # at rest - expected = Pose2(0, 2*3.5 + 2.5, math.radians(90)) - sTt = self.arm.fk(Q0) - self.assertIsInstance(sTt, Pose2) - self.assertPose2Equals(sTt, expected) - - # -30, -45, -90 - expected = Pose2(5.78, 1.52, math.radians(-75)) - sTt = self.arm.fk(Q1) - self.assertPose2Equals(sTt, expected) - - def test_jacobian(self): - """Test Jacobian calculation.""" - # at rest - expected = np.array([[-9.5, -6, -2.5], [0, 0, 0], [1, 1, 1]], np.float) - J = self.arm.jacobian(Q0) - np.testing.assert_array_almost_equal(J, expected) - - # at -90, 90, 0 - expected = np.array([[-6, -6, -2.5], [3.5, 0, 0], [1, 1, 1]], np.float) - J = self.arm.jacobian(Q2) - np.testing.assert_array_almost_equal(J, expected) - - def test_con_arm(self): - """Make sure POE is correct for some known test configurations.""" - # at rest - expected = Pose2(0, 2*3.5 + 2.5, math.radians(90)) - sTt = self.arm.con(Q0) - self.assertIsInstance(sTt, Pose2) - self.assertPose2Equals(sTt, expected) - - # -30, -45, -90 - expected = Pose2(5.78, 1.52, math.radians(-75)) - sTt = self.arm.con(Q1) - self.assertPose2Equals(sTt, expected) - - def test_poe_arm(self): - """Make sure POE is correct for some known test configurations.""" - # at rest - expected = Pose2(0, 2*3.5 + 2.5, math.radians(90)) - sTt = self.arm.poe(Q0) - self.assertIsInstance(sTt, Pose2) - self.assertPose2Equals(sTt, expected) - - # -30, -45, -90 - expected = Pose2(5.78, 1.52, math.radians(-75)) - sTt = self.arm.poe(Q1) - self.assertPose2Equals(sTt, expected) - - def test_ik(self): - """Check iterative inverse kinematics function.""" - # at rest - actual = self.arm.ik(Pose2(0, 2*3.5 + 2.5, math.radians(90))) - np.testing.assert_array_almost_equal(actual, Q0, decimal=2) - - # -30, -45, -90 - sTt_desired = Pose2(5.78, 1.52, math.radians(-75)) - actual = self.arm.ik(sTt_desired) - self.assertPose2Equals(self.arm.fk(actual), sTt_desired) - np.testing.assert_array_almost_equal(actual, Q1, decimal=2) - - def test_manipulator_jacobian(self): - """Test Jacobian calculation.""" - # at rest - expected = np.array([[0, 3.5, 7], [0, 0, 0], [1, 1, 1]], np.float) - J = self.arm.manipulator_jacobian(Q0) - np.testing.assert_array_almost_equal(J, expected) - - # at -90, 90, 0 - expected = np.array( - [[0, 0, 3.5], [0, -3.5, -3.5], [1, 1, 1]], np.float) - J = self.arm.manipulator_jacobian(Q2) - np.testing.assert_array_almost_equal(J, expected) - - -def run_example(): - """ Use trajectory interpolation and then trajectory tracking a la Murray - to move a 3-link arm on a straight line. - """ - # Create arm - arm = ThreeLinkArm() - - # Get initial pose using forward kinematics - q = np.radians(vector3(30, -30, 45)) - sTt_initial = arm.fk(q) - - # Create interpolated trajectory in task space to desired goal pose - sTt_goal = Pose2(2.4, 4.3, math.radians(0)) - poses = trajectory(sTt_initial, sTt_goal, 50) - - # Setup figure and plot initial pose - fignum = 0 - fig = plt.figure(fignum) - axes = fig.gca() - axes.set_xlim(-5, 5) - axes.set_ylim(0, 10) - gtsam_plot.plot_pose2(fignum, arm.fk(q)) - - # For all poses in interpolated trajectory, calculate dq to move to next pose. - # We do this by calculating the local Jacobian J and doing dq = inv(J)*delta(sTt, pose). - for pose in poses: - sTt = arm.fk(q) - error = delta(sTt, pose) - J = arm.jacobian(q) - q += np.dot(np.linalg.inv(J), error) - arm.plot(fignum, q) - plt.pause(0.01) - - plt.pause(10) - - -if __name__ == "__main__": - run_example() - unittest.main() diff --git a/python/gtsam/examples/PlanarSLAMExample.py b/python/gtsam/examples/PlanarSLAMExample.py deleted file mode 100644 index 5ffdf048d..000000000 --- a/python/gtsam/examples/PlanarSLAMExample.py +++ /dev/null @@ -1,81 +0,0 @@ -""" -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 - -Simple robotics example using odometry measurements and bearing-range (laser) measurements -Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) -""" -# pylint: disable=invalid-name, E1101 - -from __future__ import print_function - -import numpy as np - -import gtsam -from gtsam.symbol_shorthand import X, L - -# Create noise models -PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) -ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) -MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.2])) - -# Create an empty nonlinear factor graph -graph = gtsam.NonlinearFactorGraph() - -# Create the keys corresponding to unknown variables in the factor graph -X1 = X(1) -X2 = X(2) -X3 = X(3) -L1 = L(4) -L2 = L(5) - -# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model -graph.add(gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE)) - -# Add odometry factors between X1,X2 and X2,X3, respectively -graph.add(gtsam.BetweenFactorPose2( - X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE)) -graph.add(gtsam.BetweenFactorPose2( - X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE)) - -# Add Range-Bearing measurements to two different landmarks L1 and L2 -graph.add(gtsam.BearingRangeFactor2D( - X1, L1, gtsam.Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE)) -graph.add(gtsam.BearingRangeFactor2D( - X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE)) -graph.add(gtsam.BearingRangeFactor2D( - X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE)) - -# Print graph -print("Factor Graph:\n{}".format(graph)) - -# Create (deliberately inaccurate) initial estimate -initial_estimate = gtsam.Values() -initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15)) -initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20)) -initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10)) -initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10)) -initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80)) - -# Print -print("Initial Estimate:\n{}".format(initial_estimate)) - -# Optimize using Levenberg-Marquardt optimization. The optimizer -# accepts an optional set of configuration parameters, controlling -# things like convergence criteria, the type of linear system solver -# to use, and the amount of information displayed during optimization. -# Here we will use the default set of parameters. See the -# documentation for the full set of parameters. -params = gtsam.LevenbergMarquardtParams() -optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params) -result = optimizer.optimize() -print("\nFinal Result:\n{}".format(result)) - -# Calculate and print marginal covariances for all variables -marginals = gtsam.Marginals(graph, result) -for (key, str) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"), (L2, "L2")]: - print("{} covariance:\n{}\n".format(str, marginals.marginalCovariance(key))) diff --git a/python/gtsam/examples/Pose2SLAMExample.py b/python/gtsam/examples/Pose2SLAMExample.py deleted file mode 100644 index 2569f0953..000000000 --- a/python/gtsam/examples/Pose2SLAMExample.py +++ /dev/null @@ -1,96 +0,0 @@ -""" -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 - -Simple robotics example using odometry measurements and bearing-range (laser) measurements -Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) -""" -# pylint: disable=invalid-name, E1101 - -from __future__ import print_function - -import math - -import numpy as np - -import gtsam - -import matplotlib.pyplot as plt -import gtsam.utils.plot as gtsam_plot - - -def vector3(x, y, z): - """Create 3d double numpy array.""" - return np.array([x, y, z], dtype=np.float) - -# Create noise models -PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.3, 0.3, 0.1)) -ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.2, 0.2, 0.1)) - -# 1. Create a factor graph container and add factors to it -graph = gtsam.NonlinearFactorGraph() - -# 2a. Add a prior on the first pose, setting it to the origin -# A prior factor consists of a mean and a noise ODOMETRY_NOISE (covariance matrix) -graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE)) - -# 2b. Add odometry factors -# Create odometry (Between) factors between consecutive poses -graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE)) -graph.add(gtsam.BetweenFactorPose2( - 2, 3, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) -graph.add(gtsam.BetweenFactorPose2( - 3, 4, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) -graph.add(gtsam.BetweenFactorPose2( - 4, 5, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) - -# 2c. Add the loop closure constraint -# This factor encodes the fact that we have returned to the same pose. In real -# systems, these constraints may be identified in many ways, such as appearance-based -# techniques with camera images. We will use another Between Factor to enforce this constraint: -graph.add(gtsam.BetweenFactorPose2( - 5, 2, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) -print("\nFactor Graph:\n{}".format(graph)) # print - -# 3. Create the data structure to hold the initial_estimate estimate to the -# solution. For illustrative purposes, these have been deliberately set to incorrect values -initial_estimate = gtsam.Values() -initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) -initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) -initial_estimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2)) -initial_estimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi)) -initial_estimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2)) -print("\nInitial Estimate:\n{}".format(initial_estimate)) # print - -# 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer -# The optimizer accepts an optional set of configuration parameters, -# controlling things like convergence criteria, the type of linear -# system solver to use, and the amount of information displayed during -# optimization. We will set a few parameters as a demonstration. -parameters = gtsam.GaussNewtonParams() - -# Stop iterating once the change in error between steps is less than this value -parameters.setRelativeErrorTol(1e-5) -# Do not perform more than N iteration steps -parameters.setMaxIterations(100) -# Create the optimizer ... -optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, parameters) -# ... and optimize -result = optimizer.optimize() -print("Final Result:\n{}".format(result)) - -# 5. Calculate and print marginal covariances for all variables -marginals = gtsam.Marginals(graph, result) -for i in range(1, 6): - print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i))) - -fig = plt.figure(0) -for i in range(1, 6): - gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i)) - -plt.axis('equal') -plt.show() diff --git a/python/gtsam/examples/Pose2SLAMExample_g2o.py b/python/gtsam/examples/Pose2SLAMExample_g2o.py deleted file mode 100644 index b2ba9c5bc..000000000 --- a/python/gtsam/examples/Pose2SLAMExample_g2o.py +++ /dev/null @@ -1,88 +0,0 @@ -""" -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 - -A 2D Pose SLAM example that reads input from g2o, converts it to a factor graph -and does the optimization. Output is written on a file, in g2o format -""" -# pylint: disable=invalid-name, E1101 - -from __future__ import print_function -import argparse -import math -import numpy as np -import matplotlib.pyplot as plt - -import gtsam -from gtsam.utils import plot - - -def vector3(x, y, z): - """Create 3d double numpy array.""" - return np.array([x, y, z], dtype=np.float) - - -parser = argparse.ArgumentParser( - description="A 2D Pose SLAM example that reads input from g2o, " - "converts it to a factor graph and does the optimization. " - "Output is written on a file, in g2o format") -parser.add_argument('-i', '--input', help='input file g2o format') -parser.add_argument('-o', '--output', - help="the path to the output file with optimized graph") -parser.add_argument('-m', '--maxiter', type=int, - help="maximum number of iterations for optimizer") -parser.add_argument('-k', '--kernel', choices=['none', 'huber', 'tukey'], - default="none", help="Type of kernel used") -parser.add_argument("-p", "--plot", action="store_true", - help="Flag to plot results") -args = parser.parse_args() - -g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None\ - else args.input - -maxIterations = 100 if args.maxiter is None else args.maxiter - -is3D = False - -graph, initial = gtsam.readG2o(g2oFile, is3D) - -assert args.kernel == "none", "Supplied kernel type is not yet implemented" - -# Add prior on the pose having index (key) = 0 -priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8)) -graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel)) - -params = gtsam.GaussNewtonParams() -params.setVerbosity("Termination") -params.setMaxIterations(maxIterations) -# parameters.setRelativeErrorTol(1e-5) -# Create the optimizer ... -optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) -# ... and optimize -result = optimizer.optimize() - -print("Optimization complete") -print("initial error = ", graph.error(initial)) -print("final error = ", graph.error(result)) - - -if args.output is None: - print("\nFactor Graph:\n{}".format(graph)) - print("\nInitial Estimate:\n{}".format(initial)) - print("Final Result:\n{}".format(result)) -else: - outputFile = args.output - print("Writing results to file: ", outputFile) - graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D) - gtsam.writeG2o(graphNoKernel, result, outputFile) - print ("Done!") - -if args.plot: - resultPoses = gtsam.utilities.extractPose2(result) - for i in range(resultPoses.shape[0]): - plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :])) - plt.show() diff --git a/python/gtsam/examples/Pose3SLAMExample_g2o.py b/python/gtsam/examples/Pose3SLAMExample_g2o.py deleted file mode 100644 index 82b3bda98..000000000 --- a/python/gtsam/examples/Pose3SLAMExample_g2o.py +++ /dev/null @@ -1,71 +0,0 @@ -""" - * @file Pose3SLAMExample_initializePose3.cpp - * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the - * Pose3 using InitializePose3 - * @date Jan 17, 2019 - * @author Vikrant Shah based on CPP example by Luca Carlone -""" -# pylint: disable=invalid-name, E1101 - -from __future__ import print_function -import argparse -import numpy as np -import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D - -import gtsam -from gtsam.utils import plot - - -def vector6(x, y, z, a, b, c): - """Create 6d double numpy array.""" - return np.array([x, y, z, a, b, c], dtype=np.float) - - -parser = argparse.ArgumentParser( - description="A 3D Pose SLAM example that reads input from g2o, and " - "initializes Pose3") -parser.add_argument('-i', '--input', help='input file g2o format') -parser.add_argument('-o', '--output', - help="the path to the output file with optimized graph") -parser.add_argument("-p", "--plot", action="store_true", - help="Flag to plot results") -args = parser.parse_args() - -g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \ - else args.input - -is3D = True -graph, initial = gtsam.readG2o(g2oFile, is3D) - -# Add Prior on the first key -priorModel = gtsam.noiseModel.Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6, - 1e-4, 1e-4, 1e-4)) - -print("Adding prior to g2o file ") -firstKey = initial.keys()[0] -graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) - -params = gtsam.GaussNewtonParams() -params.setVerbosity("Termination") # this will show info about stopping conds -optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) -result = optimizer.optimize() -print("Optimization complete") - -print("initial error = ", graph.error(initial)) -print("final error = ", graph.error(result)) - -if args.output is None: - print("Final Result:\n{}".format(result)) -else: - outputFile = args.output - print("Writing results to file: ", outputFile) - graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D) - gtsam.writeG2o(graphNoKernel, result, outputFile) - print ("Done!") - -if args.plot: - resultPoses = gtsam.utilities.allPose3s(result) - for i in range(resultPoses.size()): - plot.plot_pose3(1, resultPoses.atPose3(i)) - plt.show() diff --git a/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py b/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py deleted file mode 100644 index 2b2c5f991..000000000 --- a/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py +++ /dev/null @@ -1,35 +0,0 @@ -""" -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()[0] -graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel)) - -# Initializing Pose3 - chordal relaxation" -initialization = gtsam.InitializePose3.initialize(graph) - -print(initialization) diff --git a/python/gtsam/examples/PreintegrationExample.py b/python/gtsam/examples/PreintegrationExample.py deleted file mode 100644 index e308bc933..000000000 --- a/python/gtsam/examples/PreintegrationExample.py +++ /dev/null @@ -1,141 +0,0 @@ -""" -GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, -Atlanta, Georgia 30332-0415 -All Rights Reserved - -See LICENSE for the license information - -A script validating the Preintegration of IMU measurements -""" - -import math - -import gtsam -import matplotlib.pyplot as plt -import numpy as np -from gtsam.utils.plot import plot_pose3 -from mpl_toolkits.mplot3d import Axes3D - -IMU_FIG = 1 -POSES_FIG = 2 - - -class PreintegrationExample(object): - - @staticmethod - def defaultParams(g): - """Create default parameters with Z *up* and realistic noise parameters""" - params = gtsam.PreintegrationParams.MakeSharedU(g) - kGyroSigma = math.radians(0.5) / 60 # 0.5 degree ARW - kAccelSigma = 0.1 / 60 # 10 cm VRW - params.setGyroscopeCovariance( - kGyroSigma ** 2 * np.identity(3, np.float)) - params.setAccelerometerCovariance( - kAccelSigma ** 2 * np.identity(3, np.float)) - params.setIntegrationCovariance( - 0.0000001 ** 2 * np.identity(3, np.float)) - return params - - def __init__(self, twist=None, bias=None, dt=1e-2): - """Initialize with given twist, a pair(angularVelocityVector, velocityVector).""" - - # setup interactive plotting - plt.ion() - - # Setup loop as default scenario - if twist is not None: - (W, V) = twist - else: - # default = loop with forward velocity 2m/s, while pitching up - # with angular velocity 30 degree/sec (negative in FLU) - W = np.array([0, -math.radians(30), 0]) - V = np.array([2, 0, 0]) - - self.scenario = gtsam.ConstantTwistScenario(W, V) - self.dt = dt - - self.maxDim = 5 - self.labels = list('xyz') - self.colors = list('rgb') - - # Create runner - self.g = 10 # simple gravity constant - self.params = self.defaultParams(self.g) - - if bias is not None: - self.actualBias = bias - else: - accBias = np.array([0, 0.1, 0]) - gyroBias = np.array([0, 0, 0]) - self.actualBias = gtsam.imuBias.ConstantBias(accBias, gyroBias) - - self.runner = gtsam.ScenarioRunner( - self.scenario, self.params, self.dt, self.actualBias) - - fig, self.axes = plt.subplots(4, 3) - fig.set_tight_layout(True) - - def plotImu(self, t, measuredOmega, measuredAcc): - plt.figure(IMU_FIG) - - # plot angular velocity - omega_b = self.scenario.omega_b(t) - for i, (label, color) in enumerate(zip(self.labels, self.colors)): - ax = self.axes[0][i] - ax.scatter(t, omega_b[i], color='k', marker='.') - ax.scatter(t, measuredOmega[i], color=color, marker='.') - ax.set_xlabel('angular velocity ' + label) - - # plot acceleration in nav - acceleration_n = self.scenario.acceleration_n(t) - for i, (label, color) in enumerate(zip(self.labels, self.colors)): - ax = self.axes[1][i] - ax.scatter(t, acceleration_n[i], color=color, marker='.') - ax.set_xlabel('acceleration in nav ' + label) - - # plot acceleration in body - acceleration_b = self.scenario.acceleration_b(t) - for i, (label, color) in enumerate(zip(self.labels, self.colors)): - ax = self.axes[2][i] - ax.scatter(t, acceleration_b[i], color=color, marker='.') - ax.set_xlabel('acceleration in body ' + label) - - # plot actual specific force, as well as corrupted - actual = self.runner.actualSpecificForce(t) - for i, (label, color) in enumerate(zip(self.labels, self.colors)): - ax = self.axes[3][i] - ax.scatter(t, actual[i], color='k', marker='.') - ax.scatter(t, measuredAcc[i], color=color, marker='.') - ax.set_xlabel('specific force ' + label) - - def plotGroundTruthPose(self, t, scale=0.3, time_interval=0.01): - # plot ground truth pose, as well as prediction from integrated IMU measurements - actualPose = self.scenario.pose(t) - plot_pose3(POSES_FIG, actualPose, 0.3) - t = actualPose.translation() - self.maxDim = max([max(np.abs(t)), self.maxDim]) - ax = plt.gca() - ax.set_xlim3d(-self.maxDim, self.maxDim) - ax.set_ylim3d(-self.maxDim, self.maxDim) - ax.set_zlim3d(-self.maxDim, self.maxDim) - - plt.pause(time_interval) - - def run(self, T=12): - # simulate the loop - for i, t in enumerate(np.arange(0, T, self.dt)): - measuredOmega = self.runner.measuredAngularVelocity(t) - measuredAcc = self.runner.measuredSpecificForce(t) - if i % 25 == 0: - self.plotImu(t, measuredOmega, measuredAcc) - self.plotGroundTruthPose(t) - pim = self.runner.integrate(t, self.actualBias, True) - predictedNavState = self.runner.predict(pim, self.actualBias) - plot_pose3(POSES_FIG, predictedNavState.pose(), 0.1) - - plt.ioff() - plt.show() - - -if __name__ == '__main__': - PreintegrationExample().run() diff --git a/python/gtsam/examples/README.md b/python/gtsam/examples/README.md deleted file mode 100644 index e998e4dcd..000000000 --- a/python/gtsam/examples/README.md +++ /dev/null @@ -1,52 +0,0 @@ -# Porting Progress - -| C++ Example Name | Ported | -|-------------------------------------------------------|--------| -| CameraResectioning | | -| CreateSFMExampleData | | -| DiscreteBayesNet_FG | none of the required discrete functionality is exposed through Python | -| easyPoint2KalmanFilter | ExtendedKalmanFilter not yet exposed through Python | -| elaboratePoint2KalmanFilter | GaussianSequentialSolver not yet exposed through Python | -| ImuFactorExample2 | X | -| ImuFactorsExample | | -| ISAM2Example_SmartFactor | | -| ISAM2_SmartFactorStereo_IMU | | -| LocalizationExample | impossible? | -| METISOrderingExample | | -| OdometryExample | X | -| PlanarSLAMExample | X | -| Pose2SLAMExample | X | -| Pose2SLAMExampleExpressions | | -| Pose2SLAMExample_g2o | X | -| Pose2SLAMExample_graph | | -| Pose2SLAMExample_graphviz | | -| Pose2SLAMExample_lago | lago not yet exposed through Python | -| Pose2SLAMStressTest | | -| Pose2SLAMwSPCG | | -| Pose3SLAMExample_changeKeys | | -| Pose3SLAMExampleExpressions_BearingRangeWithTransform | | -| Pose3SLAMExample_g2o | X | -| Pose3SLAMExample_initializePose3Chordal | | -| Pose3SLAMExample_initializePose3Gradient | | -| RangeISAMExample_plaza2 | | -| SelfCalibrationExample | | -| SFMExample_bal_COLAMD_METIS | | -| SFMExample_bal | | -| SFMExample | X | -| SFMExampleExpressions_bal | | -| SFMExampleExpressions | | -| SFMExample_SmartFactor | | -| SFMExample_SmartFactorPCG | | -| SimpleRotation | X | -| SolverComparer | | -| StereoVOExample | | -| StereoVOExample_large | | -| TimeTBB | | -| UGM_chain | discrete functionality not yet exposed | -| UGM_small | discrete functionality not yet exposed | -| VisualISAM2Example | X | -| VisualISAMExample | X | - -Extra Examples (with no C++ equivalent) -- PlanarManipulatorExample -- SFMData \ No newline at end of file diff --git a/python/gtsam/examples/SFMExample.py b/python/gtsam/examples/SFMExample.py deleted file mode 100644 index f0c4c82ba..000000000 --- a/python/gtsam/examples/SFMExample.py +++ /dev/null @@ -1,122 +0,0 @@ -""" -GTSAM Copyright 2010, 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 - -A structure-from-motion problem on a simulated dataset -""" -from __future__ import print_function - -import gtsam -import matplotlib.pyplot as plt -import numpy as np -from gtsam import symbol_shorthand -L = symbol_shorthand.L -X = symbol_shorthand.X - -from gtsam.examples import SFMdata -from gtsam import (Cal3_S2, DoglegOptimizer, - GenericProjectionFactorCal3_S2, Marginals, - NonlinearFactorGraph, PinholeCameraCal3_S2, Point3, - Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, Values) -from gtsam.utils import plot - - -def main(): - """ - Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). - - Each variable in the system (poses and landmarks) must be identified with a unique key. - We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). - Here we will use Symbols - - In GTSAM, measurement functions are represented as 'factors'. Several common factors - have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. - Here we will use Projection factors to model the camera's landmark observations. - Also, we will initialize the robot at some location using a Prior factor. - - When the factors are created, we will add them to a Factor Graph. As the factors we are using - are nonlinear factors, we will need a Nonlinear Factor Graph. - - Finally, once all of the factors have been added to our factor graph, we will want to - solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. - GTSAM includes several nonlinear optimizers to perform this step. Here we will use a - trust-region method known as Powell's Degleg - - The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the - nonlinear functions around an initial linearization point, then solve the linear system - to update the linearization point. This happens repeatedly until the solver converges - to a consistent set of variable values. This requires us to specify an initial guess - for each variable, held in a Values container. - """ - - # Define the camera calibration parameters - K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) - - # Define the camera observation noise model - measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v - - # Create the set of ground-truth landmarks - points = SFMdata.createPoints() - - # Create the set of ground-truth poses - poses = SFMdata.createPoses(K) - - # Create a factor graph - graph = NonlinearFactorGraph() - - # Add a prior on pose x1. This indirectly specifies where the origin is. - # 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z - pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) - factor = PriorFactorPose3(X(0), poses[0], pose_noise) - graph.push_back(factor) - - # Simulated measurements from each camera pose, adding them to the factor graph - for i, pose in enumerate(poses): - camera = PinholeCameraCal3_S2(pose, K) - for j, point in enumerate(points): - measurement = camera.project(point) - factor = GenericProjectionFactorCal3_S2( - measurement, measurement_noise, X(i), L(j), K) - graph.push_back(factor) - - # Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained - # Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance - # between the first camera and the first landmark. All other landmark positions are interpreted using this scale. - point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) - factor = PriorFactorPoint3(L(0), points[0], point_noise) - graph.push_back(factor) - graph.print_('Factor Graph:\n') - - # Create the data structure to hold the initial estimate to the solution - # Intentionally initialize the variables off from the ground truth - initial_estimate = Values() - for i, pose in enumerate(poses): - transformed_pose = pose.retract(0.1*np.random.randn(6,1)) - initial_estimate.insert(X(i), transformed_pose) - for j, point in enumerate(points): - transformed_point = point + 0.1*np.random.randn(3) - initial_estimate.insert(L(j), transformed_point) - initial_estimate.print_('Initial Estimates:\n') - - # Optimize the graph and print results - params = gtsam.DoglegParams() - params.setVerbosity('TERMINATION') - optimizer = DoglegOptimizer(graph, initial_estimate, params) - print('Optimizing:') - result = optimizer.optimize() - result.print_('Final results:\n') - print('initial error = {}'.format(graph.error(initial_estimate))) - print('final error = {}'.format(graph.error(result))) - - marginals = Marginals(graph, result) - plot.plot_3d_points(1, result, marginals=marginals) - plot.plot_trajectory(1, result, marginals=marginals, scale=8) - plot.set_axes_equal(1) - plt.show() - -if __name__ == '__main__': - main() diff --git a/python/gtsam/examples/SFMdata.py b/python/gtsam/examples/SFMdata.py deleted file mode 100644 index 6ac9c5726..000000000 --- a/python/gtsam/examples/SFMdata.py +++ /dev/null @@ -1,40 +0,0 @@ -""" -A structure-from-motion example with landmarks - - The landmarks form a 10 meter cube - - The robot rotates around the landmarks, always facing towards the cube -""" -# pylint: disable=invalid-name, E1101 - -import numpy as np - -import gtsam - - -def createPoints(): - # Create the set of ground-truth landmarks - points = [gtsam.Point3(10.0, 10.0, 10.0), - gtsam.Point3(-10.0, 10.0, 10.0), - gtsam.Point3(-10.0, -10.0, 10.0), - gtsam.Point3(10.0, -10.0, 10.0), - gtsam.Point3(10.0, 10.0, -10.0), - gtsam.Point3(-10.0, 10.0, -10.0), - gtsam.Point3(-10.0, -10.0, -10.0), - gtsam.Point3(10.0, -10.0, -10.0)] - return points - - -def createPoses(K): - # Create the set of ground-truth poses - radius = 40.0 - height = 10.0 - angles = np.linspace(0, 2*np.pi, 8, endpoint=False) - up = gtsam.Point3(0, 0, 1) - target = gtsam.Point3(0, 0, 0) - poses = [] - for theta in angles: - position = gtsam.Point3(radius*np.cos(theta), - radius*np.sin(theta), - height) - camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K) - poses.append(camera.pose()) - return poses diff --git a/python/gtsam/examples/SimpleRotation.py b/python/gtsam/examples/SimpleRotation.py deleted file mode 100644 index 0fef261f8..000000000 --- a/python/gtsam/examples/SimpleRotation.py +++ /dev/null @@ -1,84 +0,0 @@ -""" -GTSAM Copyright 2010, 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 - -This example will perform a relatively trivial optimization on -a single variable with a single factor. -""" - -import numpy as np -import gtsam -from gtsam.symbol_shorthand import X - -def main(): - """ - Step 1: Create a factor to express a unary constraint - - The "prior" in this case is the measurement from a sensor, - with a model of the noise on the measurement. - - The "Key" created here is a label used to associate parts of the - state (stored in "RotValues") with particular factors. They require - an index to allow for lookup, and should be unique. - - In general, creating a factor requires: - - A key or set of keys labeling the variables that are acted upon - - A measurement value - - A measurement model with the correct dimensionality for the factor - """ - prior = gtsam.Rot2.fromAngle(np.deg2rad(30)) - prior.print_('goal angle') - model = gtsam.noiseModel.Isotropic.Sigma(dim=1, sigma=np.deg2rad(1)) - key = X(1) - factor = gtsam.PriorFactorRot2(key, prior, model) - - """ - Step 2: Create a graph container and add the factor to it - - Before optimizing, all factors need to be added to a Graph container, - which provides the necessary top-level functionality for defining a - system of constraints. - - In this case, there is only one factor, but in a practical scenario, - many more factors would be added. - """ - graph = gtsam.NonlinearFactorGraph() - graph.push_back(factor) - graph.print_('full graph') - - """ - Step 3: Create an initial estimate - - An initial estimate of the solution for the system is necessary to - start optimization. This system state is the "Values" instance, - which is similar in structure to a dictionary, in that it maps - keys (the label created in step 1) to specific values. - - The initial estimate provided to optimization will be used as - a linearization point for optimization, so it is important that - all of the variables in the graph have a corresponding value in - this structure. - """ - initial = gtsam.Values() - initial.insert(key, gtsam.Rot2.fromAngle(np.deg2rad(20))) - initial.print_('initial estimate') - - """ - Step 4: Optimize - - After formulating the problem with a graph of constraints - and an initial estimate, executing optimization is as simple - as calling a general optimization function with the graph and - initial estimate. This will yield a new RotValues structure - with the final state of the optimization. - """ - result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize() - result.print_('final result') - - -if __name__ == '__main__': - main() diff --git a/python/gtsam/examples/VisualISAM2Example.py b/python/gtsam/examples/VisualISAM2Example.py deleted file mode 100644 index bacf510ec..000000000 --- a/python/gtsam/examples/VisualISAM2Example.py +++ /dev/null @@ -1,153 +0,0 @@ -""" -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 - -An example of running visual SLAM using iSAM2. -Author: Duy-Nguyen Ta (C++), Frank Dellaert (Python) -""" -# pylint: disable=invalid-name, E1101 - -from __future__ import print_function - -import gtsam -import gtsam.utils.plot as gtsam_plot -import matplotlib.pyplot as plt -import numpy as np -from gtsam.symbol_shorthand import L, X -from gtsam.examples import SFMdata -from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 - - -def visual_ISAM2_plot(result): - """ - VisualISAMPlot plots current state of ISAM2 object - Author: Ellon Paiva - Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert - """ - - # Declare an id for the figure - fignum = 0 - - fig = plt.figure(fignum) - axes = fig.gca(projection='3d') - plt.cla() - - # Plot points - # Can't use data because current frame might not see all points - # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()) - # gtsam.plot_3d_points(result, [], marginals) - gtsam_plot.plot_3d_points(fignum, result, 'rx') - - # Plot cameras - i = 0 - while result.exists(X(i)): - pose_i = result.atPose3(X(i)) - gtsam_plot.plot_pose3(fignum, pose_i, 10) - i += 1 - - # draw - axes.set_xlim3d(-40, 40) - axes.set_ylim3d(-40, 40) - axes.set_zlim3d(-40, 40) - plt.pause(1) - - -def visual_ISAM2_example(): - plt.ion() - - # Define the camera calibration parameters - K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) - - # Define the camera observation noise model - measurement_noise = gtsam.noiseModel.Isotropic.Sigma( - 2, 1.0) # one pixel in u and v - - # Create the set of ground-truth landmarks - points = SFMdata.createPoints() - - # Create the set of ground-truth poses - poses = SFMdata.createPoses(K) - - # Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps - # to maintain proper linearization and efficient variable ordering, iSAM2 - # performs partial relinearization/reordering at each step. A parameter - # structure is available that allows the user to set various properties, such - # as the relinearization threshold and type of linear solver. For this - # example, we we set the relinearization threshold small so the iSAM2 result - # will approach the batch result. - parameters = gtsam.ISAM2Params() - parameters.setRelinearizeThreshold(0.01) - parameters.setRelinearizeSkip(1) - isam = gtsam.ISAM2(parameters) - - # Create a Factor Graph and Values to hold the new data - graph = gtsam.NonlinearFactorGraph() - initial_estimate = gtsam.Values() - - # Loop over the different poses, adding the observations to iSAM incrementally - for i, pose in enumerate(poses): - - # Add factors for each landmark observation - for j, point in enumerate(points): - camera = gtsam.PinholeCameraCal3_S2(pose, K) - measurement = camera.project(point) - graph.push_back(gtsam.GenericProjectionFactorCal3_S2( - measurement, measurement_noise, X(i), L(j), K)) - - # Add an initial guess for the current pose - # Intentionally initialize the variables off from the ground truth - initial_estimate.insert(X(i), pose.compose(gtsam.Pose3( - gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) - - # If this is the first iteration, add a prior on the first pose to set the - # coordinate frame and a prior on the first landmark to set the scale. - # Also, as iSAM solves incrementally, we must wait until each is observed - # at least twice before adding it to iSAM. - if i == 0: - # Add a prior on pose x0 - pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array( - [0.1, 0.1, 0.1, 0.3, 0.3, 0.3])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], pose_noise)) - - # Add a prior on landmark l0 - point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) - graph.push_back(gtsam.PriorFactorPoint3( - L(0), points[0], point_noise)) # add directly to graph - - # Add initial guesses to all observed landmarks - # Intentionally initialize the variables off from the ground truth - for j, point in enumerate(points): - initial_estimate.insert(L(j), gtsam.Point3( - point[0]-0.25, point[1]+0.20, point[2]+0.15)) - else: - # Update iSAM with the new factors - isam.update(graph, initial_estimate) - # Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver. - # If accuracy is desired at the expense of time, update(*) can be called additional - # times to perform multiple optimizer iterations every step. - isam.update() - current_estimate = isam.calculateEstimate() - print("****************************************************") - print("Frame", i, ":") - for j in range(i + 1): - print(X(j), ":", current_estimate.atPose3(X(j))) - - for j in range(len(points)): - print(L(j), ":", current_estimate.atPoint3(L(j))) - - visual_ISAM2_plot(current_estimate) - - # Clear the factor graph and values for the next iteration - graph.resize(0) - initial_estimate.clear() - - plt.ioff() - plt.show() - - -if __name__ == '__main__': - visual_ISAM2_example() diff --git a/python/gtsam/examples/VisualISAMExample.py b/python/gtsam/examples/VisualISAMExample.py deleted file mode 100644 index f99d3f3e6..000000000 --- a/python/gtsam/examples/VisualISAMExample.py +++ /dev/null @@ -1,105 +0,0 @@ -""" -GTSAM Copyright 2010, 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 - -A visualSLAM example for the structure-from-motion problem on a simulated dataset -This version uses iSAM to solve the problem incrementally -""" - -from __future__ import print_function - -import numpy as np -import gtsam -from gtsam.examples import SFMdata -from gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2, - NonlinearFactorGraph, NonlinearISAM, Pose3, - PriorFactorPoint3, PriorFactorPose3, Rot3, - PinholeCameraCal3_S2, Values, Point3) -from gtsam.symbol_shorthand import X, L - -def main(): - """ - A structure-from-motion example with landmarks - - The landmarks form a 10 meter cube - - The robot rotates around the landmarks, always facing towards the cube - """ - - # Define the camera calibration parameters - K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) - - # Define the camera observation noise model - camera_noise = gtsam.noiseModel.Isotropic.Sigma( - 2, 1.0) # one pixel in u and v - - # Create the set of ground-truth landmarks - points = SFMdata.createPoints() - # Create the set of ground-truth poses - poses = SFMdata.createPoses(K) - - # Create a NonlinearISAM object which will relinearize and reorder the variables - # every "reorderInterval" updates - isam = NonlinearISAM(reorderInterval=3) - - # Create a Factor Graph and Values to hold the new data - graph = NonlinearFactorGraph() - initial_estimate = Values() - - # Loop over the different poses, adding the observations to iSAM incrementally - for i, pose in enumerate(poses): - camera = PinholeCameraCal3_S2(pose, K) - # Add factors for each landmark observation - for j, point in enumerate(points): - measurement = camera.project(point) - factor = GenericProjectionFactorCal3_S2( - measurement, camera_noise, X(i), L(j), K) - graph.push_back(factor) - - # Intentionally initialize the variables off from the ground truth - noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25), - t=Point3(0.05, -0.10, 0.20)) - initial_xi = pose.compose(noise) - - # Add an initial guess for the current pose - initial_estimate.insert(X(i), initial_xi) - - # If this is the first iteration, add a prior on the first pose to set the coordinate frame - # and a prior on the first landmark to set the scale - # Also, as iSAM solves incrementally, we must wait until each is observed at least twice before - # adding it to iSAM. - if i == 0: - # Add a prior on pose x0, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z - pose_noise = gtsam.noiseModel.Diagonal.Sigmas( - np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) - factor = PriorFactorPose3(X(0), poses[0], pose_noise) - graph.push_back(factor) - - # Add a prior on landmark l0 - point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) - factor = PriorFactorPoint3(L(0), points[0], point_noise) - graph.push_back(factor) - - # Add initial guesses to all observed landmarks - noise = np.array([-0.25, 0.20, 0.15]) - for j, point in enumerate(points): - # Intentionally initialize the variables off from the ground truth - initial_lj = points[j] + noise - initial_estimate.insert(L(j), initial_lj) - else: - # Update iSAM with the new factors - isam.update(graph, initial_estimate) - current_estimate = isam.estimate() - print('*' * 50) - print('Frame {}:'.format(i)) - current_estimate.print_('Current estimate: ') - - # Clear the factor graph and values for the next iteration - graph.resize(0) - initial_estimate.clear() - - -if __name__ == '__main__': - main() diff --git a/python/gtsam/examples/__init__.py b/python/gtsam/examples/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/python/gtsam/gtsam.tpl b/python/gtsam/gtsam.tpl deleted file mode 100644 index de3efbee0..000000000 --- a/python/gtsam/gtsam.tpl +++ /dev/null @@ -1,30 +0,0 @@ -{include_boost} - -#include -#include -#include -#include "gtsam/base/serialization.h" -#include "gtsam/nonlinear/utilities.h" // for RedirectCout. - -{includes} -#include - -{boost_class_export} - -{hoder_type} - -#include "python/gtsam/preamble.h" - -using namespace std; - -namespace py = pybind11; - -PYBIND11_MODULE({module_name}, m_) {{ - m_.doc() = "pybind11 wrapper of {module_name}"; - -{wrapped_namespace} - -#include "python/gtsam/specializations.h" - -}} - diff --git a/python/gtsam/imuBias.py b/python/gtsam/imuBias.py deleted file mode 100644 index 1cb367b9f..000000000 --- a/python/gtsam/imuBias.py +++ /dev/null @@ -1 +0,0 @@ -from .gtsam.imuBias import * diff --git a/python/gtsam/noiseModel.py b/python/gtsam/noiseModel.py deleted file mode 100644 index 9b1929a8e..000000000 --- a/python/gtsam/noiseModel.py +++ /dev/null @@ -1 +0,0 @@ -from .gtsam.noiseModel import * \ No newline at end of file diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h deleted file mode 100644 index f8e5804d0..000000000 --- a/python/gtsam/preamble.h +++ /dev/null @@ -1,4 +0,0 @@ -PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector >); -PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector>); diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h deleted file mode 100644 index c1114059b..000000000 --- a/python/gtsam/specializations.h +++ /dev/null @@ -1,4 +0,0 @@ -py::bind_vector >(m_, "KeyVector"); -py::bind_vector > >(m_, "Point2Vector"); -py::bind_vector >(m_, "Pose3Vector"); -py::bind_vector > > >(m_, "BetweenFactorPose3s"); \ No newline at end of file diff --git a/python/gtsam/symbol_shorthand.py b/python/gtsam/symbol_shorthand.py deleted file mode 100644 index 956ed693a..000000000 --- a/python/gtsam/symbol_shorthand.py +++ /dev/null @@ -1 +0,0 @@ -from .gtsam.symbol_shorthand import * \ No newline at end of file diff --git a/python/gtsam/tests/testScenarioRunner.py b/python/gtsam/tests/testScenarioRunner.py deleted file mode 100644 index 2af16a794..000000000 --- a/python/gtsam/tests/testScenarioRunner.py +++ /dev/null @@ -1,47 +0,0 @@ -""" -GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, -Atlanta, Georgia 30332-0415 -All Rights Reserved - -See LICENSE for the license information - -ScenarioRunner unit tests. -Author: Frank Dellaert & Duy Nguyen Ta (Python) -""" -import math -import unittest - -import numpy as np - -import gtsam -from gtsam.utils.test_case import GtsamTestCase - - -class TestScenarioRunner(GtsamTestCase): - def setUp(self): - self.g = 10 # simple gravity constant - - def test_loop_runner(self): - # Forward velocity 2m/s - # Pitch up with angular velocity 6 degree/sec (negative in FLU) - v = 2 - w = math.radians(6) - W = np.array([0, -w, 0]) - V = np.array([v, 0, 0]) - scenario = gtsam.ConstantTwistScenario(W, V) - - dt = 0.1 - params = gtsam.PreintegrationParams.MakeSharedU(self.g) - bias = gtsam.imuBias.ConstantBias() - runner = gtsam.ScenarioRunner( - scenario, params, dt, bias) - - # Test specific force at time 0: a is pointing up - t = 0.0 - a = w * v - np.testing.assert_almost_equal( - np.array([0, 0, a + self.g]), runner.actualSpecificForce(t)) - - -if __name__ == '__main__': - unittest.main() diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py deleted file mode 100644 index fbf5f3565..000000000 --- a/python/gtsam/tests/test_Cal3Unified.py +++ /dev/null @@ -1,38 +0,0 @@ -""" -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 -from gtsam.utils.test_case import GtsamTestCase - - -class TestCal3Unified(GtsamTestCase): - - def test_Cal3Unified(self): - K = gtsam.Cal3Unified() - self.assertEqual(K.fx(), 1.) - 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) - d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10, 1], order='F') - actual = K.retract(d) - self.gtsamAssertEquals(actual, expected) - np.testing.assert_allclose(d, K.localCoordinates(actual)) - - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_FrobeniusFactor.py b/python/gtsam/tests/test_FrobeniusFactor.py deleted file mode 100644 index e808627f5..000000000 --- a/python/gtsam/tests/test_FrobeniusFactor.py +++ /dev/null @@ -1,56 +0,0 @@ -""" -GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, -Atlanta, Georgia 30332-0415 -All Rights Reserved - -See LICENSE for the license information - -FrobeniusFactor unit tests. -Author: Frank Dellaert -""" -# pylint: disable=no-name-in-module, import-error, invalid-name -import unittest - -import numpy as np -from gtsam import (Rot3, SO3, SO4, FrobeniusBetweenFactorSO4, FrobeniusFactorSO4, - ShonanFactor3, SOn) - -id = SO4() -v1 = np.array([0, 0, 0, 0.1, 0, 0]) -Q1 = SO4.Expmap(v1) -v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03]) -Q2 = SO4.Expmap(v2) - - -class TestFrobeniusFactorSO4(unittest.TestCase): - """Test FrobeniusFactor factors.""" - - def test_frobenius_factor(self): - """Test creation of a factor that calculates the Frobenius norm.""" - factor = FrobeniusFactorSO4(1, 2) - actual = factor.evaluateError(Q1, Q2) - expected = (Q2.matrix()-Q1.matrix()).transpose().reshape((16,)) - np.testing.assert_array_equal(actual, expected) - - def test_frobenius_between_factor(self): - """Test creation of a Frobenius BetweenFactor.""" - factor = FrobeniusBetweenFactorSO4(1, 2, Q1.between(Q2)) - actual = factor.evaluateError(Q1, Q2) - expected = np.zeros((16,)) - np.testing.assert_array_almost_equal(actual, expected) - - def test_frobenius_wormhole_factor(self): - """Test creation of a factor that calculates Shonan error.""" - R1 = SO3.Expmap(v1[3:]) - R2 = SO3.Expmap(v2[3:]) - factor = ShonanFactor3(1, 2, Rot3(R1.between(R2).matrix()), p=4) - I4 = SOn(4) - Q1 = I4.retract(v1) - Q2 = I4.retract(v2) - actual = factor.evaluateError(Q1, Q2) - expected = np.zeros((12,)) - np.testing.assert_array_almost_equal(actual, expected, decimal=4) - - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_GaussianFactorGraph.py b/python/gtsam/tests/test_GaussianFactorGraph.py deleted file mode 100644 index a29b0f263..000000000 --- a/python/gtsam/tests/test_GaussianFactorGraph.py +++ /dev/null @@ -1,92 +0,0 @@ -""" -GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, -Atlanta, Georgia 30332-0415 -All Rights Reserved - -See LICENSE for the license information - -Unit tests for Linear Factor Graphs. -Author: Frank Dellaert & Gerry Chen -""" -# pylint: disable=invalid-name, no-name-in-module, no-member - -from __future__ import print_function - -import unittest - -import gtsam -import numpy as np -from gtsam.symbol_shorthand import X -from gtsam.utils.test_case import GtsamTestCase - - -def create_graph(): - """Create a basic linear factor graph for testing""" - graph = gtsam.GaussianFactorGraph() - - x0 = X(0) - x1 = X(1) - x2 = X(2) - - BETWEEN_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1)) - PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1)) - - graph.add(x1, np.eye(1), x0, -np.eye(1), np.ones(1), BETWEEN_NOISE) - graph.add(x2, np.eye(1), x1, -np.eye(1), 2*np.ones(1), BETWEEN_NOISE) - graph.add(x0, np.eye(1), np.zeros(1), PRIOR_NOISE) - - return graph, (x0, x1, x2) - -class TestGaussianFactorGraph(GtsamTestCase): - """Tests for Gaussian Factor Graphs.""" - - def test_fg(self): - """Test solving a linear factor graph""" - graph, X = create_graph() - result = graph.optimize() - - EXPECTEDX = [0, 1, 3] - - # check solutions - self.assertAlmostEqual(EXPECTEDX[0], result.at(X[0]), delta=1e-8) - self.assertAlmostEqual(EXPECTEDX[1], result.at(X[1]), delta=1e-8) - self.assertAlmostEqual(EXPECTEDX[2], result.at(X[2]), delta=1e-8) - - def test_convertNonlinear(self): - """Test converting a linear factor graph to a nonlinear one""" - graph, X = create_graph() - - EXPECTEDM = [1, 2, 3] - - # create nonlinear factor graph for marginalization - nfg = gtsam.LinearContainerFactor.ConvertLinearGraph(graph) - optimizer = gtsam.LevenbergMarquardtOptimizer(nfg, gtsam.Values()) - nlresult = optimizer.optimizeSafely() - - # marginalize - marginals = gtsam.Marginals(nfg, nlresult) - m = [marginals.marginalCovariance(x) for x in X] - - # check linear marginalizations - self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8) - self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8) - self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8) - - def test_linearMarginalization(self): - """Marginalize a linear factor graph""" - graph, X = create_graph() - result = graph.optimize() - - EXPECTEDM = [1, 2, 3] - - # linear factor graph marginalize - marginals = gtsam.Marginals(graph, result) - m = [marginals.marginalCovariance(x) for x in X] - - # check linear marginalizations - self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8) - self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8) - self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8) - -if __name__ == '__main__': - unittest.main() diff --git a/python/gtsam/tests/test_JacobianFactor.py b/python/gtsam/tests/test_JacobianFactor.py deleted file mode 100644 index 6e049ed47..000000000 --- a/python/gtsam/tests/test_JacobianFactor.py +++ /dev/null @@ -1,92 +0,0 @@ -""" -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 numpy as np - -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) - Ax2 = np.array( - [[-5., 0.], - [+0., -5.], - [10., 0.], - [+0., 10.]], order='F') - - # This is good too - Al1 = np.array( - [[5, 0], - [0, 5], - [0, 0], - [0, 0]], dtype=float, order = 'F') - - # Not recommended for performance reasons, but should still work - # as the wrapper should convert it to the correct type and storage order - Ax1 = np.array( - [[0, 0], # f4 - [0, 0], # f4 - [-10, 0], # f2 - [0, -10]]) # f2 - - x2 = 1 - l1 = 2 - x1 = 3 - - # the RHS - b2 = np.array([-1., 1.5, 2., -1.]) - sigmas = np.array([1., 1., 1., 1.]) - model4 = gtsam.noiseModel.Diagonal.Sigmas(sigmas) - combined = gtsam.JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4) - - # eliminate the first variable (x2) in the combined factor, destructive - # ! - ord = gtsam.Ordering() - ord.push_back(x2) - actualCG, lf = combined.eliminate(ord) - - # create expected Conditional Gaussian - R11 = np.array([[11.1803, 0.00], - [0.00, 11.1803]]) - S12 = np.array([[-2.23607, 0.00], - [+0.00, -2.23607]]) - S13 = np.array([[-8.94427, 0.00], - [+0.00, -8.94427]]) - d = np.array([2.23607, -1.56525]) - expectedCG = gtsam.GaussianConditional( - x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel.Unit.Create(2)) - # check if the result matches - self.gtsamAssertEquals(actualCG, expectedCG, 1e-4) - - # the expected linear factor - Bl1 = np.array([[4.47214, 0.00], - [0.00, 4.47214]]) - - Bx1 = np.array( - # x1 - [[-4.47214, 0.00], - [+0.00, -4.47214]]) - - # the RHS - b1 = np.array([0.0, 0.894427]) - - model2 = gtsam.noiseModel.Diagonal.Sigmas(np.array([1., 1.])) - expectedLF = gtsam.JacobianFactor(l1, Bl1, x1, Bx1, b1, model2) - - # check if the result matches the combined (reduced) factor - self.gtsamAssertEquals(lf, expectedLF, 1e-4) - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_KalmanFilter.py b/python/gtsam/tests/test_KalmanFilter.py deleted file mode 100644 index 48a91b96c..000000000 --- a/python/gtsam/tests/test_KalmanFilter.py +++ /dev/null @@ -1,83 +0,0 @@ -""" -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 numpy as np - -import gtsam -from gtsam.utils.test_case import GtsamTestCase - - -class TestKalmanFilter(GtsamTestCase): - - def test_KalmanFilter(self): - F = np.eye(2) - B = np.eye(2) - u = np.array([1.0, 0.0]) - modelQ = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1])) - Q = 0.01 * np.eye(2) - H = np.eye(2) - z1 = np.array([1.0, 0.0]) - z2 = np.array([2.0, 0.0]) - z3 = np.array([3.0, 0.0]) - modelR = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1])) - R = 0.01 * np.eye(2) - - # Create the set of expected output TestValues - expected0 = np.array([0.0, 0.0]) - P00 = 0.01 * np.eye(2) - - expected1 = np.array([1.0, 0.0]) - P01 = P00 + Q - I11 = np.linalg.inv(P01) + np.linalg.inv(R) - - expected2 = np.array([2.0, 0.0]) - P12 = np.linalg.inv(I11) + Q - I22 = np.linalg.inv(P12) + np.linalg.inv(R) - - expected3 = np.array([3.0, 0.0]) - P23 = np.linalg.inv(I22) + Q - I33 = np.linalg.inv(P23) + np.linalg.inv(R) - - # Create an KalmanFilter object - KF = gtsam.KalmanFilter(n=2) - - # Create the Kalman Filter initialization point - x_initial = np.array([0.0, 0.0]) - P_initial = 0.01 * np.eye(2) - - # Create an KF object - state = KF.init(x_initial, P_initial) - self.assertTrue(np.allclose(expected0, state.mean())) - self.assertTrue(np.allclose(P00, state.covariance())) - - # Run iteration 1 - state = KF.predict(state, F, B, u, modelQ) - self.assertTrue(np.allclose(expected1, state.mean())) - self.assertTrue(np.allclose(P01, state.covariance())) - state = KF.update(state, H, z1, modelR) - self.assertTrue(np.allclose(expected1, state.mean())) - self.assertTrue(np.allclose(I11, state.information())) - - # Run iteration 2 - state = KF.predict(state, F, B, u, modelQ) - self.assertTrue(np.allclose(expected2, state.mean())) - state = KF.update(state, H, z2, modelR) - self.assertTrue(np.allclose(expected2, state.mean())) - - # Run iteration 3 - state = KF.predict(state, F, B, u, modelQ) - self.assertTrue(np.allclose(expected3, state.mean())) - state = KF.update(state, H, z3, modelR) - self.assertTrue(np.allclose(expected3, state.mean())) - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_KarcherMeanFactor.py b/python/gtsam/tests/test_KarcherMeanFactor.py deleted file mode 100644 index a315a506c..000000000 --- a/python/gtsam/tests/test_KarcherMeanFactor.py +++ /dev/null @@ -1,80 +0,0 @@ -""" -GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, -Atlanta, Georgia 30332-0415 -All Rights Reserved - -See LICENSE for the license information - -KarcherMeanFactor unit tests. -Author: Frank Dellaert -""" - -# pylint: disable=invalid-name, no-name-in-module, no-member - -import unittest - -import gtsam -import numpy as np -from gtsam.utils.test_case import GtsamTestCase - -KEY = 0 -MODEL = gtsam.noiseModel.Unit.Create(3) - - -def find_Karcher_mean_Rot3(rotations): - """Find the Karcher mean of given values.""" - # Cost function C(R) = \sum PriorFactor(R_i)::error(R) - # No closed form solution. - graph = gtsam.NonlinearFactorGraph() - for R in rotations: - graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL)) - initial = gtsam.Values() - initial.insert(KEY, gtsam.Rot3()) - result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() - return result.atRot3(KEY) - - -# Rot3 version -R = gtsam.Rot3.Expmap(np.array([0.1, 0, 0])) - - -class TestKarcherMean(GtsamTestCase): - - def test_find(self): - # Check that optimizing for Karcher mean (which minimizes Between distance) - # gets correct result. - rotations = {R, R.inverse()} - expected = gtsam.Rot3() - actual = find_Karcher_mean_Rot3(rotations) - self.gtsamAssertEquals(expected, actual) - - def test_factor(self): - """Check that the InnerConstraint factor leaves the mean unchanged.""" - # Make a graph with two variables, one between, and one InnerConstraint - # The optimal result should satisfy the between, while moving the other - # variable to make the mean the same as before. - # Mean of R and R' is identity. Let's make a BetweenFactor making R21 = - # R*R*R, i.e. geodesic length is 3 rather than 2. - graph = gtsam.NonlinearFactorGraph() - R12 = R.compose(R.compose(R)) - graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL)) - keys = gtsam.KeyVector() - keys.append(1) - keys.append(2) - graph.add(gtsam.KarcherMeanFactorRot3(keys)) - - initial = gtsam.Values() - initial.insert(1, R.inverse()) - initial.insert(2, R) - expected = find_Karcher_mean_Rot3([R, R.inverse()]) - - result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() - actual = find_Karcher_mean_Rot3( - [result.atRot3(1), result.atRot3(2)]) - self.gtsamAssertEquals(expected, actual) - self.gtsamAssertEquals( - R12, result.atRot3(1).between(result.atRot3(2))) - - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_LocalizationExample.py b/python/gtsam/tests/test_LocalizationExample.py deleted file mode 100644 index 8ae3583f0..000000000 --- a/python/gtsam/tests/test_LocalizationExample.py +++ /dev/null @@ -1,64 +0,0 @@ -""" -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 numpy as np - -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 - # NonlinearFactorGraph) - graph = gtsam.NonlinearFactorGraph() - - # Add two odometry factors - # create a measurement for both factors (the same in this case) - odometry = gtsam.Pose2(2.0, 0.0, 0.0) - odometryNoise = gtsam.noiseModel.Diagonal.Sigmas( - np.array([0.2, 0.2, 0.1])) # 20cm std on x,y, 0.1 rad on theta - graph.add(gtsam.BetweenFactorPose2(0, 1, odometry, odometryNoise)) - graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise)) - - # Add three "GPS" measurements - # We use Pose2 Priors here with high variance on theta - groundTruth = gtsam.Values() - groundTruth.insert(0, gtsam.Pose2(0.0, 0.0, 0.0)) - groundTruth.insert(1, gtsam.Pose2(2.0, 0.0, 0.0)) - groundTruth.insert(2, gtsam.Pose2(4.0, 0.0, 0.0)) - model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 10.])) - for i in range(3): - graph.add(gtsam.PriorFactorPose2(i, groundTruth.atPose2(i), model)) - - # Initialize to noisy points - initialEstimate = gtsam.Values() - initialEstimate.insert(0, gtsam.Pose2(0.5, 0.0, 0.2)) - initialEstimate.insert(1, gtsam.Pose2(2.3, 0.1, -0.2)) - initialEstimate.insert(2, gtsam.Pose2(4.1, 0.1, 0.1)) - - # Optimize using Levenberg-Marquardt optimization with an ordering from - # colamd - optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) - result = optimizer.optimizeSafely() - - # Plot Covariance Ellipses - marginals = gtsam.Marginals(graph, result) - P = [None] * result.size() - for i in range(0, result.size()): - pose_i = result.atPose2(i) - self.gtsamAssertEquals(pose_i, groundTruth.atPose2(i), 1e-4) - P[i] = marginals.marginalCovariance(i) - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py deleted file mode 100644 index e9234a43b..000000000 --- a/python/gtsam/tests/test_NonlinearOptimizer.py +++ /dev/null @@ -1,83 +0,0 @@ -""" -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, - DummyPreconditionerParameters, GaussNewtonOptimizer, - GaussNewtonParams, LevenbergMarquardtOptimizer, - LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, - PCGSolverParameters, 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)) - - # Levenberg-Marquardt - lmParams = LevenbergMarquardtParams.CeresDefaults() - lmParams.setLinearSolverType("ITERATIVE") - cgParams = PCGSolverParameters() - cgParams.setPreconditionerParams(DummyPreconditionerParameters()) - lmParams.setIterativeParams(cgParams) - actual2 = LevenbergMarquardtOptimizer( - fg, initial_values, lmParams).optimize() - self.assertAlmostEqual(0, fg.error(actual2)) - - # Dogleg - dlParams = DoglegParams() - dlParams.setOrdering(ordering) - actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize() - self.assertAlmostEqual(0, fg.error(actual3)) - - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_OdometryExample.py b/python/gtsam/tests/test_OdometryExample.py deleted file mode 100644 index 72e532f20..000000000 --- a/python/gtsam/tests/test_OdometryExample.py +++ /dev/null @@ -1,59 +0,0 @@ -""" -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 numpy as np - -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 - # NonlinearFactorGraph) - graph = gtsam.NonlinearFactorGraph() - - # Add a Gaussian prior on pose x_1 - priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior mean is at origin - priorNoise = gtsam.noiseModel.Diagonal.Sigmas( - np.array([0.3, 0.3, 0.1])) # 30cm std on x,y, 0.1 rad on theta - # add directly to graph - graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise)) - - # Add two odometry factors - # create a measurement for both factors (the same in this case) - odometry = gtsam.Pose2(2.0, 0.0, 0.0) - odometryNoise = gtsam.noiseModel.Diagonal.Sigmas( - np.array([0.2, 0.2, 0.1])) # 20cm std on x,y, 0.1 rad on theta - graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise)) - graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise)) - - # Initialize to noisy points - initialEstimate = gtsam.Values() - initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) - initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) - initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, 0.1)) - - # Optimize using Levenberg-Marquardt optimization with an ordering from - # colamd - optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) - result = optimizer.optimizeSafely() - marginals = gtsam.Marginals(graph, result) - marginals.marginalCovariance(1) - - # Check first pose equality - pose_1 = result.atPose2(1) - self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4) - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_PlanarSLAMExample.py b/python/gtsam/tests/test_PlanarSLAMExample.py deleted file mode 100644 index 8cb3ad2ac..000000000 --- a/python/gtsam/tests/test_PlanarSLAMExample.py +++ /dev/null @@ -1,78 +0,0 @@ -""" -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 -from math import pi - -import numpy as np - -import gtsam -from gtsam.utils.test_case import GtsamTestCase - - -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) - # - We have full odometry for measurements - # - The robot is on a grid, moving 2 meters each step - - # Create graph container and add factors to it - graph = gtsam.NonlinearFactorGraph() - - # Add prior - # gaussian for prior - priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin - priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) - # add directly to graph - graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise)) - - # Add odometry - # general noisemodel for odometry - odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) - graph.add(gtsam.BetweenFactorPose2( - 1, 2, gtsam.Pose2(2.0, 0.0, 0.0), odometryNoise)) - graph.add(gtsam.BetweenFactorPose2( - 2, 3, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) - graph.add(gtsam.BetweenFactorPose2( - 3, 4, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) - graph.add(gtsam.BetweenFactorPose2( - 4, 5, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) - - # Add pose constraint - model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) - graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2), model)) - - # Initialize to noisy points - initialEstimate = gtsam.Values() - initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) - initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) - initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, pi / 2)) - initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, pi)) - initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -pi / 2)) - - # Optimize using Levenberg-Marquardt optimization with an ordering from - # colamd - optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) - result = optimizer.optimizeSafely() - - # Plot Covariance Ellipses - marginals = gtsam.Marginals(graph, result) - P = marginals.marginalCovariance(1) - - pose_1 = result.atPose2(1) - self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4) - - - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py deleted file mode 100644 index 9652b594a..000000000 --- a/python/gtsam/tests/test_Pose2.py +++ /dev/null @@ -1,32 +0,0 @@ -""" -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(GtsamTestCase): - """Test selected Pose2 methods.""" - - def test_adjoint(self): - """Test adjoint method.""" - xi = np.array([1, 2, 3]) - expected = np.dot(Pose2.adjointMap_(xi), xi) - actual = Pose2.adjoint_(xi, xi) - np.testing.assert_array_equal(actual, expected) - - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_Pose2SLAMExample.py b/python/gtsam/tests/test_Pose2SLAMExample.py deleted file mode 100644 index e47b9fbff..000000000 --- a/python/gtsam/tests/test_Pose2SLAMExample.py +++ /dev/null @@ -1,76 +0,0 @@ -""" -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 -from math import pi - -import numpy as np - -import gtsam -from gtsam.utils.test_case import GtsamTestCase - - -class TestPose2SLAMExample(GtsamTestCase): - - def test_Pose2SLAMExample(self): - # Assumptions - # - All values are axis aligned - # - Robot poses are facing along the X axis (horizontal, to the right in images) - # - We have full odometry for measurements - # - The robot is on a grid, moving 2 meters each step - - # Create graph container and add factors to it - graph = gtsam.NonlinearFactorGraph() - - # Add prior - # gaussian for prior - priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin - priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) - # add directly to graph - graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise)) - - # Add odometry - # general noisemodel for odometry - odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) - graph.add(gtsam.BetweenFactorPose2( - 1, 2, gtsam.Pose2(2.0, 0.0, 0.0), odometryNoise)) - graph.add(gtsam.BetweenFactorPose2( - 2, 3, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) - graph.add(gtsam.BetweenFactorPose2( - 3, 4, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) - graph.add(gtsam.BetweenFactorPose2( - 4, 5, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) - - # Add pose constraint - model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) - graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2), model)) - - # Initialize to noisy points - initialEstimate = gtsam.Values() - initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) - initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) - initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, pi / 2)) - initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, pi)) - initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -pi / 2)) - - # Optimize using Levenberg-Marquardt optimization with an ordering from - # colamd - optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) - result = optimizer.optimizeSafely() - - # Plot Covariance Ellipses - marginals = gtsam.Marginals(graph, result) - P = marginals.marginalCovariance(1) - - pose_1 = result.atPose2(1) - self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4) - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_Pose3.py b/python/gtsam/tests/test_Pose3.py deleted file mode 100644 index 138f1ff13..000000000 --- a/python/gtsam/tests/test_Pose3.py +++ /dev/null @@ -1,70 +0,0 @@ -""" -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 -""" -# pylint: disable=no-name-in-module -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(GtsamTestCase): - """Test selected Pose3 methods.""" - - def test_between(self): - """Test between method.""" - T2 = Pose3(Rot3.Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2)) - T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3)) - expected = T2.inverse().compose(T3) - actual = T2.between(T3) - self.gtsamAssertEquals(actual, expected, 1e-6) - - def test_transform_to(self): - """Test transformTo method.""" - transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0)) - actual = transform.transformTo(Point3(3, 2, 10)) - expected = Point3(2, 1, 10) - self.gtsamAssertEquals(actual, expected, 1e-6) - - def test_range(self): - """Test range method.""" - l1 = Point3(1, 0, 0) - l2 = Point3(1, 1, 0) - x1 = Pose3() - - xl1 = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(1, 0, 0)) - xl2 = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)) - - # establish range is indeed zero - self.assertEqual(1, x1.range(point=l1)) - - # establish range is indeed sqrt2 - self.assertEqual(math.sqrt(2.0), x1.range(point=l2)) - - # establish range is indeed zero - self.assertEqual(1, x1.range(pose=xl1)) - - # establish range is indeed sqrt2 - self.assertEqual(math.sqrt(2.0), x1.range(pose=xl2)) - - def test_adjoint(self): - """Test adjoint method.""" - xi = np.array([1, 2, 3, 4, 5, 6]) - expected = np.dot(Pose3.adjointMap_(xi), xi) - actual = Pose3.adjoint_(xi, xi) - np.testing.assert_array_equal(actual, expected) - - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_Pose3SLAMExample.py b/python/gtsam/tests/test_Pose3SLAMExample.py deleted file mode 100644 index fce171b55..000000000 --- a/python/gtsam/tests/test_Pose3SLAMExample.py +++ /dev/null @@ -1,60 +0,0 @@ -""" -GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, -Atlanta, Georgia 30332-0415 -All Rights Reserved - -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 - hexagon = circlePose3(6, 1.0) - p0 = hexagon.atPose3(0) - p1 = hexagon.atPose3(1) - - # create a Pose graph with one equality constraint and one measurement - fg = gtsam.NonlinearFactorGraph() - fg.add(gtsam.NonlinearEqualityPose3(0, p0)) - delta = p0.between(p1) - covariance = gtsam.noiseModel.Diagonal.Sigmas( - np.array([0.05, 0.05, 0.05, 5. * pi / 180, 5. * pi / 180, 5. * pi / 180])) - fg.add(gtsam.BetweenFactorPose3(0, 1, delta, covariance)) - fg.add(gtsam.BetweenFactorPose3(1, 2, delta, covariance)) - fg.add(gtsam.BetweenFactorPose3(2, 3, delta, covariance)) - fg.add(gtsam.BetweenFactorPose3(3, 4, delta, covariance)) - fg.add(gtsam.BetweenFactorPose3(4, 5, delta, covariance)) - fg.add(gtsam.BetweenFactorPose3(5, 0, delta, covariance)) - - # Create initial config - initial = gtsam.Values() - s = 0.10 - initial.insert(0, p0) - initial.insert(1, hexagon.atPose3(1).retract(s * np.random.randn(6, 1))) - initial.insert(2, hexagon.atPose3(2).retract(s * np.random.randn(6, 1))) - initial.insert(3, hexagon.atPose3(3).retract(s * np.random.randn(6, 1))) - initial.insert(4, hexagon.atPose3(4).retract(s * np.random.randn(6, 1))) - initial.insert(5, hexagon.atPose3(5).retract(s * np.random.randn(6, 1))) - - # optimize - optimizer = gtsam.LevenbergMarquardtOptimizer(fg, initial) - result = optimizer.optimizeSafely() - - pose_1 = result.atPose3(1) - self.gtsamAssertEquals(pose_1, p1, 1e-4) - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_PriorFactor.py b/python/gtsam/tests/test_PriorFactor.py deleted file mode 100644 index 0582cf5d7..000000000 --- a/python/gtsam/tests/test_PriorFactor.py +++ /dev/null @@ -1,61 +0,0 @@ -""" -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 numpy as np - -import gtsam -from gtsam.utils.test_case import GtsamTestCase - - -class TestPriorFactor(GtsamTestCase): - - def test_PriorFactor(self): - values = gtsam.Values() - - key = 5 - priorPose3 = gtsam.Pose3() - model = gtsam.noiseModel.Unit.Create(6) - factor = gtsam.PriorFactorPose3(key, priorPose3, model) - values.insert(key, priorPose3) - self.assertEqual(factor.error(values), 0) - - key = 3 - priorVector = np.array([0., 0., 0.]) - model = gtsam.noiseModel.Unit.Create(3) - factor = gtsam.PriorFactorVector(key, priorVector, model) - values.insert(key, priorVector) - self.assertEqual(factor.error(values), 0) - - def test_AddPrior(self): - """ - Test adding prior factors directly to factor graph via the .addPrior method. - """ - # define factor graph - graph = gtsam.NonlinearFactorGraph() - - # define and add Pose3 prior - key = 5 - priorPose3 = gtsam.Pose3() - model = gtsam.noiseModel.Unit.Create(6) - graph.addPriorPose3(key, priorPose3, model) - self.assertEqual(graph.size(), 1) - - # define and add Vector prior - key = 3 - priorVector = np.array([0., 0., 0.]) - model = gtsam.noiseModel.Unit.Create(3) - graph.addPriorVector(key, priorVector, model) - self.assertEqual(graph.size(), 2) - - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_SFMExample.py b/python/gtsam/tests/test_SFMExample.py deleted file mode 100644 index 47a3cbe3e..000000000 --- a/python/gtsam/tests/test_SFMExample.py +++ /dev/null @@ -1,83 +0,0 @@ -""" -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 numpy as np - -import gtsam -import gtsam.utils.visual_data_generator as generator -from gtsam import symbol -from gtsam.noiseModel import Isotropic, Diagonal -from gtsam.utils.test_case import GtsamTestCase -from gtsam.symbol_shorthand import X, P - -class TestSFMExample(GtsamTestCase): - - def test_SFMExample(self): - options = generator.Options() - options.triangle = False - options.nrCameras = 10 - - [data, truth] = generator.generate_data(options) - - measurementNoiseSigma = 1.0 - pointNoiseSigma = 0.1 - poseNoiseSigmas = np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1]) - - graph = gtsam.NonlinearFactorGraph() - - # Add factors for all measurements - measurementNoise = Isotropic.Sigma(2, measurementNoiseSigma) - for i in range(len(data.Z)): - for k in range(len(data.Z[i])): - j = data.J[i][k] - graph.add(gtsam.GenericProjectionFactorCal3_S2( - data.Z[i][k], measurementNoise, - X(i), P(j), data.K)) - - posePriorNoise = Diagonal.Sigmas(poseNoiseSigmas) - graph.add(gtsam.PriorFactorPose3(X(0), - truth.cameras[0].pose(), posePriorNoise)) - pointPriorNoise = Isotropic.Sigma(3, pointNoiseSigma) - graph.add(gtsam.PriorFactorPoint3(P(0), - truth.points[0], pointPriorNoise)) - - # Initial estimate - initialEstimate = gtsam.Values() - for i in range(len(truth.cameras)): - pose_i = truth.cameras[i].pose() - initialEstimate.insert(X(i), pose_i) - for j in range(len(truth.points)): - point_j = truth.points[j] - initialEstimate.insert(P(j), point_j) - - # Optimization - optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) - for i in range(5): - optimizer.iterate() - result = optimizer.values() - - # Marginalization - marginals = gtsam.Marginals(graph, result) - marginals.marginalCovariance(P(0)) - marginals.marginalCovariance(X(0)) - - # Check optimized results, should be equal to ground truth - for i in range(len(truth.cameras)): - pose_i = result.atPose3(X(i)) - self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) - - for j in range(len(truth.points)): - point_j = result.atPoint3(P(j)) - self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_SO4.py b/python/gtsam/tests/test_SO4.py deleted file mode 100644 index 648bd1710..000000000 --- a/python/gtsam/tests/test_SO4.py +++ /dev/null @@ -1,59 +0,0 @@ -""" -GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, -Atlanta, Georgia 30332-0415 -All Rights Reserved - -See LICENSE for the license information - -SO4 unit tests. -Author: Frank Dellaert -""" -# pylint: disable=no-name-in-module, import-error -import unittest - -import numpy as np -from gtsam import SO4 - -I4 = SO4() -v1 = np.array([0, 0, 0, .1, 0, 0]) -v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03]) -Q1 = SO4.Expmap(v1) -Q2 = SO4.Expmap(v2) - - -class TestSO4(unittest.TestCase): - """Test selected SO4 methods.""" - - def test_constructor(self): - """Construct from matrix.""" - matrix = np.eye(4) - so4 = SO4(matrix) - self.assertIsInstance(so4, SO4) - - def test_retract(self): - """Test retraction to manifold.""" - v = np.zeros((6,), np.float) - actual = I4.retract(v) - self.assertTrue(actual.equals(I4, 1e-9)) - - def test_local(self): - """Check localCoordinates for trivial case.""" - v0 = np.zeros((6,), np.float) - actual = I4.localCoordinates(I4) - np.testing.assert_array_almost_equal(actual, v0) - - def test_compose(self): - """Check compose works in subgroup.""" - expected = SO4.Expmap(2*v1) - actual = Q1.compose(Q1) - self.assertTrue(actual.equals(expected, 1e-9)) - - def test_vec(self): - """Check wrapping of vec.""" - expected = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1]) - actual = I4.vec() - np.testing.assert_array_equal(actual, expected) - - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_SOn.py b/python/gtsam/tests/test_SOn.py deleted file mode 100644 index 7599363e2..000000000 --- a/python/gtsam/tests/test_SOn.py +++ /dev/null @@ -1,59 +0,0 @@ -""" -GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, -Atlanta, Georgia 30332-0415 -All Rights Reserved - -See LICENSE for the license information - -Dynamic SO(n) unit tests. -Author: Frank Dellaert -""" -# pylint: disable=no-name-in-module, import-error -import unittest - -import numpy as np -from gtsam import SOn - -I4 = SOn(4) -v1 = np.array([0, 0, 0, .1, 0, 0]) -v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03]) -Q1 = I4.retract(v1) -Q2 = I4.retract(v2) - - -class TestSO4(unittest.TestCase): - """Test selected SOn methods.""" - - def test_constructor(self): - """Construct from matrix.""" - matrix = np.eye(4) - so4 = SOn.FromMatrix(matrix) - self.assertIsInstance(so4, SOn) - - def test_retract(self): - """Test retraction to manifold.""" - v = np.zeros((6,), np.float) - actual = I4.retract(v) - self.assertTrue(actual.equals(I4, 1e-9)) - - def test_local(self): - """Check localCoordinates for trivial case.""" - v0 = np.zeros((6,), np.float) - actual = I4.localCoordinates(I4) - np.testing.assert_array_almost_equal(actual, v0) - - def test_compose(self): - """Check compose works in subgroup.""" - expected = I4.retract(2*v1) - actual = Q1.compose(Q1) - self.assertTrue(actual.equals(expected, 1e-3)) # not exmap so only approximate - - def test_vec(self): - """Check wrapping of vec.""" - expected = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1]) - actual = I4.vec() - np.testing.assert_array_equal(actual, expected) - - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_Scenario.py b/python/gtsam/tests/test_Scenario.py deleted file mode 100644 index fc5965829..000000000 --- a/python/gtsam/tests/test_Scenario.py +++ /dev/null @@ -1,56 +0,0 @@ -""" -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(GtsamTestCase): - def setUp(self): - pass - - def test_loop(self): - # Forward velocity 2m/s - # Pitch up with angular velocity 6 degree/sec (negative in FLU) - v = 2 - w = math.radians(6) - W = np.array([0, -w, 0]) - V = np.array([v, 0, 0]) - scenario = gtsam.ConstantTwistScenario(W, V) - - T = 30 - np.testing.assert_almost_equal(W, scenario.omega_b(T)) - np.testing.assert_almost_equal(V, scenario.velocity_b(T)) - np.testing.assert_almost_equal( - np.cross(W, V), scenario.acceleration_b(T)) - - # R = v/w, so test if loop crests at 2*R - R = v / w - T30 = scenario.pose(T) - xyz = T30.rotation().xyz() - if xyz[0] < 0: - xyz = -xyz - np.testing.assert_almost_equal( - np.array([math.pi, 0, math.pi]), xyz) - self.gtsamAssertEquals(gtsam.Point3( - 0, 0, 2.0 * R), T30.translation(), 1e-9) - - -if __name__ == '__main__': - unittest.main() diff --git a/python/gtsam/tests/test_SimpleCamera.py b/python/gtsam/tests/test_SimpleCamera.py deleted file mode 100644 index efdfec561..000000000 --- a/python/gtsam/tests/test_SimpleCamera.py +++ /dev/null @@ -1,45 +0,0 @@ -""" -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 unittest - -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(GtsamTestCase): - - def test_constructor(self): - pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5)) - camera = SimpleCamera(pose1, K) - 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 - pose2 = Pose2(0.4,0.3,math.pi/2.0) - camera = SimpleCamera.Level(K, pose2, 0.1) - - # expected - x = Point3(1,0,0) - y = Point3(0,0,-1) - z = Point3(0,1,0) - wRc = Rot3(x,y,z) - expected = Pose3(wRc,Point3(0.4,0.3,0.1)) - self.gtsamAssertEquals(camera.pose(), expected, 1e-9) - - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_StereoVOExample.py b/python/gtsam/tests/test_StereoVOExample.py deleted file mode 100644 index cefc08aab..000000000 --- a/python/gtsam/tests/test_StereoVOExample.py +++ /dev/null @@ -1,82 +0,0 @@ -""" -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 numpy as np - -import gtsam -from gtsam import symbol -from gtsam.utils.test_case import GtsamTestCase - - -class TestStereoVOExample(GtsamTestCase): - - def test_StereoVOExample(self): - ## Assumptions - # - For simplicity this example is in the camera's coordinate frame - # - X: right, Y: down, Z: forward - # - Pose x1 is at the origin, Pose 2 is 1 meter forward (along Z-axis) - # - x1 is fixed with a constraint, x2 is initialized with noisy values - # - No noise on measurements - - ## Create keys for variables - x1 = symbol('x',1) - x2 = symbol('x',2) - l1 = symbol('l',1) - l2 = symbol('l',2) - l3 = symbol('l',3) - - ## Create graph container and add factors to it - graph = gtsam.NonlinearFactorGraph() - - ## add a constraint on the starting pose - first_pose = gtsam.Pose3() - graph.add(gtsam.NonlinearEqualityPose3(x1, first_pose)) - - ## Create realistic calibration and measurement noise model - # format: fx fy skew cx cy baseline - K = gtsam.Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2) - stereo_model = gtsam.noiseModel.Diagonal.Sigmas(np.array([1.0, 1.0, 1.0])) - - ## Add measurements - # pose 1 - graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(520, 480, 440), stereo_model, x1, l1, K)) - graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(120, 80, 440), stereo_model, x1, l2, K)) - graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 280, 140), stereo_model, x1, l3, K)) - - #pose 2 - graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(570, 520, 490), stereo_model, x2, l1, K)) - graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2( 70, 20, 490), stereo_model, x2, l2, K)) - graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 270, 115), stereo_model, x2, l3, K)) - - ## Create initial estimate for camera poses and landmarks - initialEstimate = gtsam.Values() - initialEstimate.insert(x1, first_pose) - # noisy estimate for pose 2 - initialEstimate.insert(x2, gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.1,-.1,1.1))) - expected_l1 = gtsam.Point3( 1, 1, 5) - initialEstimate.insert(l1, expected_l1) - initialEstimate.insert(l2, gtsam.Point3(-1, 1, 5)) - initialEstimate.insert(l3, gtsam.Point3( 0,-.5, 5)) - - ## optimize - optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) - result = optimizer.optimize() - - ## check equality for the first pose and point - pose_x1 = result.atPose3(x1) - self.gtsamAssertEquals(pose_x1, first_pose,1e-4) - - point_l1 = result.atPoint3(l1) - self.gtsamAssertEquals(point_l1, expected_l1,1e-4) - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py deleted file mode 100644 index b43ad9b57..000000000 --- a/python/gtsam/tests/test_Triangulation.py +++ /dev/null @@ -1,80 +0,0 @@ -""" -GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, -Atlanta, Georgia 30332-0415 -All Rights Reserved - -See LICENSE for the license information - -Test Triangulation -Author: Frank Dellaert & Fan Jiang (Python) -""" -import unittest - -import numpy as np - -import gtsam as g -from gtsam.utils.test_case import GtsamTestCase -from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \ - PinholeCameraCal3_S2, Point3, Point2Vector, Pose3Vector, triangulatePoint3 - -class TestVisualISAMExample(GtsamTestCase): - def test_TriangulationExample(self): - # Some common constants - sharedCal = Cal3_S2(1500, 1200, 0, 640, 480) - - # Looking along X-axis, 1 meter above ground plane (x-y) - upright = Rot3.Ypr(-np.pi / 2, 0., -np.pi / 2) - pose1 = Pose3(upright, Point3(0, 0, 1)) - camera1 = PinholeCameraCal3_S2(pose1, sharedCal) - - # create second camera 1 meter to the right of first camera - pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))) - camera2 = PinholeCameraCal3_S2(pose2, sharedCal) - - # landmark ~5 meters infront of camera - landmark = Point3(5, 0.5, 1.2) - - # 1. Project two landmarks into two cameras and triangulate - z1 = camera1.project(landmark) - z2 = camera2.project(landmark) - - # twoPoses - poses = Pose3Vector() - measurements = Point2Vector() - - poses.append(pose1) - poses.append(pose2) - measurements.append(z1) - measurements.append(z2) - - optimize = True - rank_tol = 1e-9 - - triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize) - self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9) - - # 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) - measurements = Point2Vector() - measurements.append(z1 - np.array([0.1, 0.5])) - measurements.append(z2 - np.array([-0.2, 0.3])) - - triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize) - self.gtsamAssertEquals(landmark, triangulated_landmark,1e-2) - # - # # two Poses with Bundler Calibration - # bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480) - # camera1 = PinholeCameraCal3Bundler(pose1, bundlerCal) - # camera2 = PinholeCameraCal3Bundler(pose2, bundlerCal) - # - # z1 = camera1.project(landmark) - # z2 = camera2.project(landmark) - # - # measurements = Point2Vector() - # measurements.append(z1) - # measurements.append(z2) - # - # triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize) - # self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9) - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_Values.py b/python/gtsam/tests/test_Values.py deleted file mode 100644 index dddd11c40..000000000 --- a/python/gtsam/tests/test_Values.py +++ /dev/null @@ -1,85 +0,0 @@ -""" -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 - -import gtsam -from gtsam import (Cal3_S2, Cal3Bundler, Cal3DS2, EssentialMatrix, Point2, - Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values, imuBias) -from gtsam.utils.test_case import GtsamTestCase - - -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(2, Rot2()) - values.insert(3, Pose2()) - values.insert(4, Rot3()) - values.insert(5, Pose3()) - values.insert(6, Cal3_S2()) - values.insert(7, Cal3DS2()) - values.insert(8, Cal3Bundler()) - values.insert(9, E) - values.insert(10, imuBias.ConstantBias()) - - # Special cases for Vectors and Matrices - # Note that gtsam's Eigen Vectors and Matrices requires double-precision - # floating point numbers in column-major (Fortran style) storage order, - # whereas by default, numpy.array is in row-major order and the type is - # in whatever the number input type is, e.g. np.array([1,2,3]) - # will have 'int' type. - # - # 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. - # 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]]) - values.insert(13, mat2) - - 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.atConstantBias( - 10), imuBias.ConstantBias(), tol) - - # special cases for Vector and Matrix: - actualVector = values.atVector(11) - np.testing.assert_allclose(vec, actualVector, tol) - actualMatrix = values.atMatrix(12) - np.testing.assert_allclose(mat, actualMatrix, tol) - actualMatrix2 = values.atMatrix(13) - np.testing.assert_allclose(mat2, actualMatrix2, tol) - - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_VisualISAMExample.py b/python/gtsam/tests/test_VisualISAMExample.py deleted file mode 100644 index 6eb05eeee..000000000 --- a/python/gtsam/tests/test_VisualISAMExample.py +++ /dev/null @@ -1,57 +0,0 @@ -""" -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 - -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(GtsamTestCase): - - def test_VisualISAMExample(self): - # Data Options - options = generator.Options() - options.triangle = False - options.nrCameras = 20 - - # iSAM Options - isamOptions = visual_isam.Options() - isamOptions.hardConstraint = False - isamOptions.pointPriors = False - isamOptions.batchInitialization = True - isamOptions.reorderInterval = 10 - isamOptions.alwaysRelinearize = False - - # Generate data - data, truth = generator.generate_data(options) - - # Initialize iSAM with the first pose and points - isam, result, nextPose = visual_isam.initialize(data, truth, isamOptions) - - # Main loop for iSAM: stepping through all poses - for currentPose in range(nextPose, options.nrCameras): - isam, result = visual_isam.step(data, isam, result, truth, currentPose) - - for i in range(len(truth.cameras)): - pose_i = result.atPose3(symbol('x', i)) - self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) - - for j in range(len(truth.points)): - point_j = result.atPoint3(symbol('l', j)) - self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_dataset.py b/python/gtsam/tests/test_dataset.py deleted file mode 100644 index 87fc2ad54..000000000 --- a/python/gtsam/tests/test_dataset.py +++ /dev/null @@ -1,45 +0,0 @@ -""" -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 -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(len(factors), 6) - self.assertIsInstance(factors[0], BetweenFactorPose3) - - -if __name__ == '__main__': - unittest.main() diff --git a/python/gtsam/tests/test_dsf_map.py b/python/gtsam/tests/test_dsf_map.py deleted file mode 100644 index 73ddcb050..000000000 --- a/python/gtsam/tests/test_dsf_map.py +++ /dev/null @@ -1,40 +0,0 @@ -""" -GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, -Atlanta, Georgia 30332-0415 -All Rights Reserved - -See LICENSE for the license information - -Unit tests for Disjoint Set Forest. -Author: Frank Dellaert & Varun Agrawal -""" -# pylint: disable=invalid-name, no-name-in-module, no-member - -from __future__ import print_function - -import unittest - -import gtsam -from gtsam.utils.test_case import GtsamTestCase - - -class TestDSFMap(GtsamTestCase): - """Tests for DSFMap.""" - - def test_all(self): - """Test everything in DFSMap.""" - def key(index_pair): - return index_pair.i(), index_pair.j() - - dsf = gtsam.DSFMapIndexPair() - pair1 = gtsam.IndexPair(1, 18) - self.assertEqual(key(dsf.find(pair1)), key(pair1)) - pair2 = gtsam.IndexPair(2, 2) - - # testing the merge feature of dsf - dsf.merge(pair1, pair2) - self.assertEqual(key(dsf.find(pair1)), key(dsf.find(pair2))) - - -if __name__ == '__main__': - unittest.main() diff --git a/python/gtsam/tests/test_initialize_pose3.py b/python/gtsam/tests/test_initialize_pose3.py deleted file mode 100644 index 6d7f66653..000000000 --- a/python/gtsam/tests/test_initialize_pose3.py +++ /dev/null @@ -1,88 +0,0 @@ -""" -GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, -Atlanta, Georgia 30332-0415 -All Rights Reserved - -See LICENSE for the license information - -Unit tests for 3D SLAM initialization, using rotation relaxation. -Author: Luca Carlone and Frank Dellaert (Python) -""" -# pylint: disable=invalid-name, E1101, E0611 -import unittest - -import numpy as np - -import gtsam -from gtsam import NonlinearFactorGraph, Point3, Pose3, Rot3, Values -from gtsam.utils.test_case import GtsamTestCase - -x0, x1, x2, x3 = 0, 1, 2, 3 - - -class TestValues(GtsamTestCase): - - def setUp(self): - - model = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) - - # We consider a small graph: - # symbolic FG - # x2 0 1 - # / | \ 1 2 - # / | \ 2 3 - # x3 | x1 2 0 - # \ | / 0 3 - # \ | / - # x0 - # - p0 = Point3(0, 0, 0) - self.R0 = Rot3.Expmap(np.array([0.0, 0.0, 0.0])) - p1 = Point3(1, 2, 0) - self.R1 = Rot3.Expmap(np.array([0.0, 0.0, 1.570796])) - p2 = Point3(0, 2, 0) - self.R2 = Rot3.Expmap(np.array([0.0, 0.0, 3.141593])) - p3 = Point3(-1, 1, 0) - self.R3 = Rot3.Expmap(np.array([0.0, 0.0, 4.712389])) - - pose0 = Pose3(self.R0, p0) - pose1 = Pose3(self.R1, p1) - pose2 = Pose3(self.R2, p2) - pose3 = Pose3(self.R3, p3) - - g = NonlinearFactorGraph() - g.add(gtsam.BetweenFactorPose3(x0, x1, pose0.between(pose1), model)) - g.add(gtsam.BetweenFactorPose3(x1, x2, pose1.between(pose2), model)) - g.add(gtsam.BetweenFactorPose3(x2, x3, pose2.between(pose3), model)) - g.add(gtsam.BetweenFactorPose3(x2, x0, pose2.between(pose0), model)) - g.add(gtsam.BetweenFactorPose3(x0, x3, pose0.between(pose3), model)) - g.add(gtsam.PriorFactorPose3(x0, pose0, model)) - self.graph = g - - def test_buildPose3graph(self): - pose3graph = gtsam.InitializePose3.buildPose3graph(self.graph) - - def test_orientations(self): - pose3Graph = gtsam.InitializePose3.buildPose3graph(self.graph) - initial = gtsam.InitializePose3.computeOrientationsChordal(pose3Graph) - - # comparison is up to M_PI, that's why we add some multiples of 2*M_PI - self.gtsamAssertEquals(initial.atRot3(x0), self.R0, 1e-6) - self.gtsamAssertEquals(initial.atRot3(x1), self.R1, 1e-6) - self.gtsamAssertEquals(initial.atRot3(x2), self.R2, 1e-6) - self.gtsamAssertEquals(initial.atRot3(x3), self.R3, 1e-6) - - def test_initializePoses(self): - g2oFile = gtsam.findExampleDataFile("pose3example-grid") - is3D = True - inputGraph, expectedValues = gtsam.readG2o(g2oFile, is3D) - priorModel = gtsam.noiseModel.Unit.Create(6) - inputGraph.add(gtsam.PriorFactorPose3(0, Pose3(), priorModel)) - - initial = gtsam.InitializePose3.initialize(inputGraph) - # TODO(frank): very loose !! - self.gtsamAssertEquals(initial, expectedValues, 0.1) - - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/tests/test_logging_optimizer.py b/python/gtsam/tests/test_logging_optimizer.py deleted file mode 100644 index 47eb32e7b..000000000 --- a/python/gtsam/tests/test_logging_optimizer.py +++ /dev/null @@ -1,108 +0,0 @@ -""" -Unit tests for optimization that logs to comet.ml. -Author: Jing Wu and Frank Dellaert -""" -# pylint: disable=invalid-name - -import sys -if sys.version_info.major >= 3: - from io import StringIO -else: - from cStringIO import StringIO - -import unittest -from datetime import datetime - -import gtsam -import numpy as np -from gtsam import Rot3 -from gtsam.utils.test_case import GtsamTestCase - -from gtsam.utils.logging_optimizer import gtsam_optimize - -KEY = 0 -MODEL = gtsam.noiseModel.Unit.Create(3) - - -class TestOptimizeComet(GtsamTestCase): - """Check correct logging to comet.ml.""" - - def setUp(self): - """Set up a small Karcher mean optimization example.""" - # Grabbed from KarcherMeanFactor unit tests. - R = Rot3.Expmap(np.array([0.1, 0, 0])) - rotations = {R, R.inverse()} # mean is the identity - self.expected = Rot3() - - graph = gtsam.NonlinearFactorGraph() - for R in rotations: - graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL)) - initial = gtsam.Values() - initial.insert(KEY, R) - self.params = gtsam.GaussNewtonParams() - self.optimizer = gtsam.GaussNewtonOptimizer( - graph, initial, self.params) - - self.lmparams = gtsam.LevenbergMarquardtParams() - self.lmoptimizer = gtsam.LevenbergMarquardtOptimizer( - graph, initial, self.lmparams - ) - - # setup output capture - self.capturedOutput = StringIO() - sys.stdout = self.capturedOutput - - def tearDown(self): - """Reset print capture.""" - sys.stdout = sys.__stdout__ - - def test_simple_printing(self): - """Test with a simple hook.""" - - # Provide a hook that just prints - def hook(_, error): - print(error) - - # Only thing we require from optimizer is an iterate method - gtsam_optimize(self.optimizer, self.params, hook) - - # Check that optimizing yields the identity. - actual = self.optimizer.values() - self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6) - - def test_lm_simple_printing(self): - """Make sure we are properly terminating LM""" - def hook(_, error): - print(error) - - gtsam_optimize(self.lmoptimizer, self.lmparams, hook) - - actual = self.lmoptimizer.values() - self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6) - - @unittest.skip("Not a test we want run every time, as needs comet.ml account") - def test_comet(self): - """Test with a comet hook.""" - from comet_ml import Experiment - comet = Experiment(project_name="Testing", - auto_output_logging="native") - comet.log_dataset_info(name="Karcher", path="shonan") - comet.add_tag("GaussNewton") - comet.log_parameter("method", "GaussNewton") - time = datetime.now() - comet.set_name("GaussNewton-" + str(time.month) + "/" + str(time.day) + " " - + str(time.hour)+":"+str(time.minute)+":"+str(time.second)) - - # I want to do some comet thing here - def hook(optimizer, error): - comet.log_metric("Karcher error", - error, optimizer.iterations()) - - gtsam_optimize(self.optimizer, self.params, hook) - comet.end() - - actual = self.optimizer.values() - self.gtsamAssertEquals(actual.atRot3(KEY), self.expected) - -if __name__ == "__main__": - unittest.main() diff --git a/python/gtsam/utils/__init__.py b/python/gtsam/utils/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/python/gtsam/utils/circlePose3.py b/python/gtsam/utils/circlePose3.py deleted file mode 100644 index e1def9427..000000000 --- a/python/gtsam/utils/circlePose3.py +++ /dev/null @@ -1,35 +0,0 @@ -import gtsam -import math -import numpy as np -from math import pi - - -def circlePose3(numPoses=8, radius=1.0, symbolChar='\0'): - """ - circlePose3 generates a set of poses in a circle. This function - returns those poses inside a gtsam.Values object, with sequential - keys starting from 0. An optional character may be provided, which - will be stored in the msb of each key (i.e. gtsam.Symbol). - - We use aerospace/navlab convention, X forward, Y right, Z down - First pose will be at (R,0,0) - ^y ^ X - | | - z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer) - Vehicle at p0 is looking towards y axis (X-axis points towards world y) - """ - - values = gtsam.Values() - theta = 0.0 - dtheta = 2 * pi / numPoses - gRo = gtsam.Rot3( - np.array([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]], order='F')) - for i in range(numPoses): - key = gtsam.symbol(symbolChar, i) - gti = gtsam.Point3(radius * math.cos(theta), radius * math.sin(theta), 0) - oRi = gtsam.Rot3.Yaw( - -theta) # negative yaw goes counterclockwise, with Z down ! - gTi = gtsam.Pose3(gRo.compose(oRi), gti) - values.insert(key, gTi) - theta = theta + dtheta - return values diff --git a/python/gtsam/utils/logging_optimizer.py b/python/gtsam/utils/logging_optimizer.py deleted file mode 100644 index 3d9175951..000000000 --- a/python/gtsam/utils/logging_optimizer.py +++ /dev/null @@ -1,52 +0,0 @@ -""" -Optimization with logging via a hook. -Author: Jing Wu and Frank Dellaert -""" -# pylint: disable=invalid-name - -from gtsam import NonlinearOptimizer, NonlinearOptimizerParams -import gtsam - - -def optimize(optimizer, check_convergence, hook): - """ Given an optimizer and a convergence check, iterate until convergence. - After each iteration, hook(optimizer, error) is called. - After the function, use values and errors to get the result. - Arguments: - optimizer (T): needs an iterate and an error function. - check_convergence: T * float * float -> bool - hook -- hook function to record the error - """ - # the optimizer is created with default values which incur the error below - current_error = optimizer.error() - hook(optimizer, current_error) - - # Iterative loop - while True: - # Do next iteration - optimizer.iterate() - new_error = optimizer.error() - hook(optimizer, new_error) - if check_convergence(optimizer, current_error, new_error): - return - current_error = new_error - - -def gtsam_optimize(optimizer, - params, - hook): - """ Given an optimizer and params, iterate until convergence. - After each iteration, hook(optimizer) is called. - After the function, use values and errors to get the result. - Arguments: - optimizer {NonlinearOptimizer} -- Nonlinear optimizer - params {NonlinearOptimizarParams} -- Nonlinear optimizer parameters - hook -- hook function to record the error - """ - def check_convergence(optimizer, current_error, new_error): - return (optimizer.iterations() >= params.getMaxIterations()) or ( - gtsam.checkConvergence(params.getRelativeErrorTol(), params.getAbsoluteErrorTol(), params.getErrorTol(), - current_error, new_error)) or ( - isinstance(optimizer, gtsam.LevenbergMarquardtOptimizer) and optimizer.lambda_() > params.getlambdaUpperBound()) - optimize(optimizer, check_convergence, hook) - return optimizer.values() diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py deleted file mode 100644 index 0267da8c3..000000000 --- a/python/gtsam/utils/plot.py +++ /dev/null @@ -1,401 +0,0 @@ -"""Various plotting utlities.""" - -import numpy as np -import matplotlib.pyplot as plt -from matplotlib import patches -from mpl_toolkits.mplot3d import Axes3D - -import gtsam - - -def set_axes_equal(fignum): - """ - Make axes of 3D plot have equal scale so that spheres appear as spheres, - cubes as cubes, etc.. This is one possible solution to Matplotlib's - ax.set_aspect('equal') and ax.axis('equal') not working for 3D. - - Args: - fignum (int): An integer representing the figure number for Matplotlib. - """ - fig = plt.figure(fignum) - ax = fig.gca(projection='3d') - - limits = np.array([ - ax.get_xlim3d(), - ax.get_ylim3d(), - ax.get_zlim3d(), - ]) - - origin = np.mean(limits, axis=1) - radius = 0.5 * np.max(np.abs(limits[:, 1] - limits[:, 0])) - - ax.set_xlim3d([origin[0] - radius, origin[0] + radius]) - ax.set_ylim3d([origin[1] - radius, origin[1] + radius]) - ax.set_zlim3d([origin[2] - radius, origin[2] + radius]) - - -def ellipsoid(xc, yc, zc, rx, ry, rz, n): - """ - Numpy equivalent of Matlab's ellipsoid function. - - Args: - xc (double): Center of ellipsoid in X-axis. - yc (double): Center of ellipsoid in Y-axis. - zc (double): Center of ellipsoid in Z-axis. - rx (double): Radius of ellipsoid in X-axis. - ry (double): Radius of ellipsoid in Y-axis. - rz (double): Radius of ellipsoid in Z-axis. - n (int): The granularity of the ellipsoid plotted. - - Returns: - tuple[numpy.ndarray]: The points in the x, y and z axes to use for the surface plot. - """ - u = np.linspace(0, 2*np.pi, n+1) - v = np.linspace(0, np.pi, n+1) - x = -rx * np.outer(np.cos(u), np.sin(v)).T - y = -ry * np.outer(np.sin(u), np.sin(v)).T - z = -rz * np.outer(np.ones_like(u), np.cos(v)).T - - return x, y, z - - -def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5): - """ - Plots a Gaussian as an uncertainty ellipse - - Based on Maybeck Vol 1, page 366 - k=2.296 corresponds to 1 std, 68.26% of all probability - k=11.82 corresponds to 3 std, 99.74% of all probability - - Args: - axes (matplotlib.axes.Axes): Matplotlib axes. - origin (gtsam.Point3): The origin in the world frame. - P (numpy.ndarray): The marginal covariance matrix of the 3D point which will be represented as an ellipse. - scale (float): Scaling factor of the radii of the covariance ellipse. - n (int): Defines the granularity of the ellipse. Higher values indicate finer ellipses. - alpha (float): Transparency value for the plotted surface in the range [0, 1]. - """ - k = 11.82 - U, S, _ = np.linalg.svd(P) - - radii = k * np.sqrt(S) - radii = radii * scale - rx, ry, rz = radii - - # generate data for "unrotated" ellipsoid - xc, yc, zc = ellipsoid(0, 0, 0, rx, ry, rz, n) - - # rotate data with orientation matrix U and center c - data = np.kron(U[:, 0:1], xc) + np.kron(U[:, 1:2], yc) + \ - np.kron(U[:, 2:3], zc) - n = data.shape[1] - x = data[0:n, :] + origin[0] - y = data[n:2*n, :] + origin[1] - z = data[2*n:, :] + origin[2] - - axes.plot_surface(x, y, z, alpha=alpha, cmap='hot') - - -def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None): - """ - Plot a 2D pose on given axis `axes` with given `axis_length`. - - Args: - axes (matplotlib.axes.Axes): Matplotlib axes. - pose (gtsam.Pose2): The pose to be plotted. - axis_length (float): The length of the camera axes. - covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. - """ - # get rotation and translation (center) - gRp = pose.rotation().matrix() # rotation from pose to global - t = pose.translation() - origin = t - - # draw the camera axes - x_axis = origin + gRp[:, 0] * axis_length - line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0) - axes.plot(line[:, 0], line[:, 1], 'r-') - - y_axis = origin + gRp[:, 1] * axis_length - line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0) - axes.plot(line[:, 0], line[:, 1], 'g-') - - if covariance is not None: - pPp = covariance[0:2, 0:2] - gPp = np.matmul(np.matmul(gRp, pPp), gRp.T) - - w, v = np.linalg.eig(gPp) - - # k = 2.296 - k = 5.0 - - angle = np.arctan2(v[1, 0], v[0, 0]) - e1 = patches.Ellipse(origin, np.sqrt(w[0]*k), np.sqrt(w[1]*k), - np.rad2deg(angle), fill=False) - axes.add_patch(e1) - - -def plot_pose2(fignum, pose, axis_length=0.1, covariance=None, - axis_labels=('X axis', 'Y axis', 'Z axis')): - """ - Plot a 2D pose on given figure with given `axis_length`. - - Args: - fignum (int): Integer representing the figure number to use for plotting. - pose (gtsam.Pose2): The pose to be plotted. - axis_length (float): The length of the camera axes. - covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. - axis_labels (iterable[string]): List of axis labels to set. - """ - # get figure object - fig = plt.figure(fignum) - axes = fig.gca() - plot_pose2_on_axes(axes, pose, axis_length=axis_length, - covariance=covariance) - - axes.set_xlabel(axis_labels[0]) - axes.set_ylabel(axis_labels[1]) - - return fig - - -def plot_point3_on_axes(axes, point, linespec, P=None): - """ - Plot a 3D point on given axis `axes` with given `linespec`. - - Args: - axes (matplotlib.axes.Axes): Matplotlib axes. - point (gtsam.Point3): The point to be plotted. - linespec (string): String representing formatting options for Matplotlib. - P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. - """ - axes.plot([point[0]], [point[1]], [point[2]], linespec) - if P is not None: - plot_covariance_ellipse_3d(axes, point, P) - - -def plot_point3(fignum, point, linespec, P=None, - axis_labels=('X axis', 'Y axis', 'Z axis')): - """ - Plot a 3D point on given figure with given `linespec`. - - Args: - fignum (int): Integer representing the figure number to use for plotting. - point (gtsam.Point3): The point to be plotted. - linespec (string): String representing formatting options for Matplotlib. - P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. - axis_labels (iterable[string]): List of axis labels to set. - - Returns: - fig: The matplotlib figure. - - """ - fig = plt.figure(fignum) - axes = fig.gca(projection='3d') - plot_point3_on_axes(axes, point, linespec, P) - - axes.set_xlabel(axis_labels[0]) - axes.set_ylabel(axis_labels[1]) - axes.set_zlabel(axis_labels[2]) - - return fig - - -def plot_3d_points(fignum, values, linespec="g*", marginals=None, - title="3D Points", axis_labels=('X axis', 'Y axis', 'Z axis')): - """ - Plots the Point3s in `values`, with optional covariances. - Finds all the Point3 objects in the given Values object and plots them. - If a Marginals object is given, this function will also plot marginal - covariance ellipses for each point. - - Args: - fignum (int): Integer representing the figure number to use for plotting. - values (gtsam.Values): Values dictionary consisting of points to be plotted. - linespec (string): String representing formatting options for Matplotlib. - marginals (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. - title (string): The title of the plot. - axis_labels (iterable[string]): List of axis labels to set. - """ - - keys = values.keys() - - # Plot points and covariance matrices - for key in keys: - try: - point = values.atPoint3(key) - if marginals is not None: - covariance = marginals.marginalCovariance(key) - else: - covariance = None - - fig = plot_point3(fignum, point, linespec, covariance, - axis_labels=axis_labels) - - except RuntimeError: - continue - # I guess it's not a Point3 - - fig.suptitle(title) - fig.canvas.set_window_title(title.lower()) - - -def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1): - """ - Plot a 3D pose on given axis `axes` with given `axis_length`. - - Args: - axes (matplotlib.axes.Axes): Matplotlib axes. - point (gtsam.Point3): The point to be plotted. - linespec (string): String representing formatting options for Matplotlib. - P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. - """ - # get rotation and translation (center) - gRp = pose.rotation().matrix() # rotation from pose to global - origin = pose.translation() - - # draw the camera axes - x_axis = origin + gRp[:, 0] * axis_length - line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0) - axes.plot(line[:, 0], line[:, 1], line[:, 2], 'r-') - - y_axis = origin + gRp[:, 1] * axis_length - line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0) - axes.plot(line[:, 0], line[:, 1], line[:, 2], 'g-') - - z_axis = origin + gRp[:, 2] * axis_length - line = np.append(origin[np.newaxis], z_axis[np.newaxis], axis=0) - axes.plot(line[:, 0], line[:, 1], line[:, 2], 'b-') - - # plot the covariance - if P is not None: - # covariance matrix in pose coordinate frame - pPp = P[3:6, 3:6] - # convert the covariance matrix to global coordinate frame - gPp = gRp @ pPp @ gRp.T - plot_covariance_ellipse_3d(axes, origin, gPp) - - -def plot_pose3(fignum, pose, axis_length=0.1, P=None, - axis_labels=('X axis', 'Y axis', 'Z axis')): - """ - Plot a 3D pose on given figure with given `axis_length`. - - Args: - fignum (int): Integer representing the figure number to use for plotting. - pose (gtsam.Pose3): 3D pose to be plotted. - linespec (string): String representing formatting options for Matplotlib. - P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. - axis_labels (iterable[string]): List of axis labels to set. - - Returns: - fig: The matplotlib figure. - """ - # get figure object - fig = plt.figure(fignum) - axes = fig.gca(projection='3d') - plot_pose3_on_axes(axes, pose, P=P, - axis_length=axis_length) - - axes.set_xlabel(axis_labels[0]) - axes.set_ylabel(axis_labels[1]) - axes.set_zlabel(axis_labels[2]) - - return fig - - -def plot_trajectory(fignum, values, scale=1, marginals=None, - title="Plot Trajectory", axis_labels=('X axis', 'Y axis', 'Z axis')): - """ - Plot a complete 3D trajectory using poses in `values`. - - Args: - fignum (int): Integer representing the figure number to use for plotting. - values (gtsam.Values): Values dict containing the poses. - scale (float): Value to scale the poses by. - marginals (gtsam.Marginals): Marginalized probability values of the estimation. - Used to plot uncertainty bounds. - title (string): The title of the plot. - axis_labels (iterable[string]): List of axis labels to set. - """ - pose3Values = gtsam.utilities.allPose3s(values) - keys = gtsam.KeyVector(pose3Values.keys()) - lastKey = None - - for key in keys: - try: - pose = pose3Values.atPose3(key) - except: - print("Warning: no Pose3 at key: {0}".format(key)) - - if lastKey is not None: - try: - lastPose = pose3Values.atPose3(lastKey) - except: - print("Warning: no Pose3 at key: {0}".format(lastKey)) - pass - - if marginals: - covariance = marginals.marginalCovariance(lastKey) - else: - covariance = None - - fig = plot_pose3(fignum, lastPose, P=covariance, - axis_length=scale, axis_labels=axis_labels) - - lastKey = key - - # Draw final pose - if lastKey is not None: - try: - lastPose = pose3Values.atPose3(lastKey) - if marginals: - covariance = marginals.marginalCovariance(lastKey) - else: - covariance = None - - fig = plot_pose3(fignum, lastPose, P=covariance, - axis_length=scale, axis_labels=axis_labels) - - except: - pass - - fig.suptitle(title) - fig.canvas.set_window_title(title.lower()) - - -def plot_incremental_trajectory(fignum, values, start=0, - scale=1, marginals=None, - time_interval=0.0): - """ - Incrementally plot a complete 3D trajectory using poses in `values`. - - Args: - fignum (int): Integer representing the figure number to use for plotting. - values (gtsam.Values): Values dict containing the poses. - start (int): Starting index to start plotting from. - scale (float): Value to scale the poses by. - marginals (gtsam.Marginals): Marginalized probability values of the estimation. - Used to plot uncertainty bounds. - time_interval (float): Time in seconds to pause between each rendering. - Used to create animation effect. - """ - fig = plt.figure(fignum) - axes = fig.gca(projection='3d') - - pose3Values = gtsam.utilities.allPose3s(values) - keys = gtsam.KeyVector(pose3Values.keys()) - - for key in keys[start:]: - if values.exists(key): - pose_i = values.atPose3(key) - plot_pose3(fignum, pose_i, scale) - - # Update the plot space to encompass all plotted points - axes.autoscale() - - # Set the 3 axes equal - set_axes_equal(fignum) - - # Pause for a fixed amount of seconds - plt.pause(time_interval) diff --git a/python/gtsam/utils/test_case.py b/python/gtsam/utils/test_case.py deleted file mode 100644 index 3effd7f65..000000000 --- a/python/gtsam/utils/test_case.py +++ /dev/null @@ -1,31 +0,0 @@ -""" -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 - """ - import numpy - if isinstance(expected, numpy.ndarray): - equal = numpy.allclose(actual, expected, atol=tol) - else: - equal = actual.equals(expected, tol) - if not equal: - raise self.failureException( - "Values are not equal:\n{}!={}".format(actual, expected)) diff --git a/python/gtsam/utils/visual_data_generator.py b/python/gtsam/utils/visual_data_generator.py deleted file mode 100644 index 32ccbc8fa..000000000 --- a/python/gtsam/utils/visual_data_generator.py +++ /dev/null @@ -1,119 +0,0 @@ -from __future__ import print_function - -import numpy as np -import math -from math import pi -import gtsam -from gtsam import Point3, Pose3, PinholeCameraCal3_S2, Cal3_S2 - - -class Options: - """ - Options to generate test scenario - """ - - def __init__(self, triangle=False, nrCameras=3, K=Cal3_S2()): - """ - Options to generate test scenario - @param triangle: generate a triangle scene with 3 points if True, otherwise - a cube with 8 points - @param nrCameras: number of cameras to generate - @param K: camera calibration object - """ - self.triangle = triangle - self.nrCameras = nrCameras - - -class GroundTruth: - """ - Object holding generated ground-truth data - """ - - def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4): - self.K = K - self.cameras = [Pose3()] * nrCameras - self.points = [Point3(0, 0, 0)] * nrPoints - - def print_(self, s=""): - print(s) - print("K = ", self.K) - print("Cameras: ", len(self.cameras)) - for camera in self.cameras: - print("\t", camera) - print("Points: ", len(self.points)) - for point in self.points: - print("\t", point) - pass - - -class Data: - """ - Object holding generated measurement data - """ - - class NoiseModels: - pass - - def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4): - self.K = K - self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras] - self.J = [x[:] for x in [[0] * nrPoints] * nrCameras] - self.odometry = [Pose3()] * nrCameras - - # Set Noise parameters - self.noiseModels = Data.NoiseModels() - self.noiseModels.posePrior = gtsam.noiseModel.Diagonal.Sigmas( - np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1])) - # noiseModels.odometry = gtsam.noiseModel.Diagonal.Sigmas( - # np.array([0.001,0.001,0.001,0.1,0.1,0.1])) - self.noiseModels.odometry = gtsam.noiseModel.Diagonal.Sigmas( - np.array([0.05, 0.05, 0.05, 0.2, 0.2, 0.2])) - self.noiseModels.pointPrior = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) - self.noiseModels.measurement = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) - - -def generate_data(options): - """ Generate ground-truth and measurement data. """ - - K = Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.) - nrPoints = 3 if options.triangle else 8 - - truth = GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints) - data = Data(K, nrCameras=options.nrCameras, nrPoints=nrPoints) - - # Generate simulated data - if options.triangle: # Create a triangle target, just 3 points on a plane - r = 10 - for j in range(len(truth.points)): - theta = j * 2 * pi / nrPoints - truth.points[j] = Point3(r * math.cos(theta), r * math.sin(theta), 0) - else: # 3D landmarks as vertices of a cube - truth.points = [ - Point3(10, 10, 10), Point3(-10, 10, 10), - Point3(-10, -10, 10), Point3(10, -10, 10), - Point3(10, 10, -10), Point3(-10, 10, -10), - Point3(-10, -10, -10), Point3(10, -10, -10) - ] - - # Create camera cameras on a circle around the triangle - height = 10 - r = 40 - for i in range(options.nrCameras): - theta = i * 2 * pi / options.nrCameras - t = Point3(r * math.cos(theta), r * math.sin(theta), height) - truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t, - Point3(0, 0, 0), - Point3(0, 0, 1), - truth.K) - # Create measurements - for j in range(nrPoints): - # All landmarks seen in every frame - data.Z[i][j] = truth.cameras[i].project(truth.points[j]) - data.J[i][j] = j - - # Calculate odometry between cameras - for i in range(1, options.nrCameras): - data.odometry[i] = truth.cameras[i - 1].pose().between( - truth.cameras[i].pose()) - - return data, truth diff --git a/python/gtsam/utils/visual_isam.py b/python/gtsam/utils/visual_isam.py deleted file mode 100644 index a8fed4b23..000000000 --- a/python/gtsam/utils/visual_isam.py +++ /dev/null @@ -1,131 +0,0 @@ -import gtsam -from gtsam import symbol - - -class Options: - """ Options for visual isam example. """ - - def __init__(self): - self.hardConstraint = False - self.pointPriors = False - self.batchInitialization = True - self.reorderInterval = 10 - self.alwaysRelinearize = False - - -def initialize(data, truth, options): - # Initialize iSAM - params = gtsam.ISAM2Params() - if options.alwaysRelinearize: - params.setRelinearizeSkip(1) - isam = gtsam.ISAM2(params=params) - - # Add constraints/priors - # TODO: should not be from ground truth! - newFactors = gtsam.NonlinearFactorGraph() - initialEstimates = gtsam.Values() - for i in range(2): - ii = symbol('x', i) - if i == 0: - if options.hardConstraint: # add hard constraint - newFactors.add( - gtsam.NonlinearEqualityPose3(ii, truth.cameras[0].pose())) - else: - newFactors.add( - gtsam.PriorFactorPose3(ii, truth.cameras[i].pose(), - data.noiseModels.posePrior)) - initialEstimates.insert(ii, truth.cameras[i].pose()) - - nextPoseIndex = 2 - - # Add visual measurement factors from two first poses and initialize - # observed landmarks - for i in range(2): - ii = symbol('x', i) - for k in range(len(data.Z[i])): - j = data.J[i][k] - jj = symbol('l', j) - newFactors.add( - gtsam.GenericProjectionFactorCal3_S2(data.Z[i][ - k], data.noiseModels.measurement, ii, jj, data.K)) - # TODO: initial estimates should not be from ground truth! - if not initialEstimates.exists(jj): - initialEstimates.insert(jj, truth.points[j]) - if options.pointPriors: # add point priors - newFactors.add( - gtsam.PriorFactorPoint3(jj, truth.points[j], - data.noiseModels.pointPrior)) - - # Add odometry between frames 0 and 1 - newFactors.add( - gtsam.BetweenFactorPose3( - symbol('x', 0), - symbol('x', 1), data.odometry[1], data.noiseModels.odometry)) - - # Update ISAM - if options.batchInitialization: # Do a full optimize for first two poses - batchOptimizer = gtsam.LevenbergMarquardtOptimizer(newFactors, - initialEstimates) - fullyOptimized = batchOptimizer.optimize() - isam.update(newFactors, fullyOptimized) - else: - isam.update(newFactors, initialEstimates) - - # figure(1)tic - # t=toc plot(frame_i,t,'r.') tic - result = isam.calculateEstimate() - # t=toc plot(frame_i,t,'g.') - - return isam, result, nextPoseIndex - - -def step(data, isam, result, truth, currPoseIndex): - ''' - Do one step isam update - @param[in] data: measurement data (odometry and visual measurements and their noiseModels) - @param[in/out] isam: current isam object, will be updated - @param[in/out] result: current result object, will be updated - @param[in] truth: ground truth data, used to initialize new variables - @param[currPoseIndex]: index of the current pose - ''' - # iSAM expects us to give it a new set of factors - # along with initial estimates for any new variables introduced. - newFactors = gtsam.NonlinearFactorGraph() - initialEstimates = gtsam.Values() - - # Add odometry - prevPoseIndex = currPoseIndex - 1 - odometry = data.odometry[prevPoseIndex] - newFactors.add( - gtsam.BetweenFactorPose3( - symbol('x', prevPoseIndex), - symbol('x', currPoseIndex), odometry, - data.noiseModels.odometry)) - - # Add visual measurement factors and initializations as necessary - for k in range(len(data.Z[currPoseIndex])): - zij = data.Z[currPoseIndex][k] - j = data.J[currPoseIndex][k] - jj = symbol('l', j) - newFactors.add( - gtsam.GenericProjectionFactorCal3_S2( - zij, data.noiseModels.measurement, - symbol('x', currPoseIndex), jj, data.K)) - # TODO: initialize with something other than truth - if not result.exists(jj) and not initialEstimates.exists(jj): - lmInit = truth.points[j] - initialEstimates.insert(jj, lmInit) - - # Initial estimates for the new pose. - prevPose = result.atPose3(symbol('x', prevPoseIndex)) - initialEstimates.insert( - symbol('x', currPoseIndex), prevPose.compose(odometry)) - - # Update ISAM - # figure(1)tic - isam.update(newFactors, initialEstimates) - # t=toc plot(frame_i,t,'r.') tic - newResult = isam.calculateEstimate() - # t=toc plot(frame_i,t,'g.') - - return isam, newResult diff --git a/python/gtsam_unstable/__init__.py b/python/gtsam_unstable/__init__.py deleted file mode 100644 index 3195e6da4..000000000 --- a/python/gtsam_unstable/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from .gtsam_unstable import * diff --git a/python/gtsam_unstable/examples/FixedLagSmootherExample.py b/python/gtsam_unstable/examples/FixedLagSmootherExample.py deleted file mode 100644 index 7d2cea8ae..000000000 --- a/python/gtsam_unstable/examples/FixedLagSmootherExample.py +++ /dev/null @@ -1,92 +0,0 @@ -""" -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 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((X1, 0.0)) - - delta_time = 0.25 - time = 0.25 - - while time <= 3.0: - previous_key = int(1000 * (time - delta_time)) - current_key = int(1000 * time) - - # assign current key to the current timestamp - new_timestamps.insert((current_key, time)) - - # Add a guess for this pose to the new values - # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s] - current_pose = gtsam.Pose2(time * 2, 0, 0) - new_values.insert(current_key, current_pose) - - # Add odometry factors from two different sources with different error - # stats - odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02) - odometry_noise_1 = gtsam.noiseModel.Diagonal.Sigmas( - np.array([0.1, 0.1, 0.05])) - new_factors.push_back(gtsam.BetweenFactorPose2( - previous_key, current_key, odometry_measurement_1, odometry_noise_1 - )) - - odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01) - odometry_noise_2 = gtsam.noiseModel.Diagonal.Sigmas( - np.array([0.05, 0.05, 0.05])) - new_factors.push_back(gtsam.BetweenFactorPose2( - previous_key, current_key, odometry_measurement_2, odometry_noise_2 - )) - - # Update the smoothers with the new factors. In this case, - # one iteration must pass for Levenberg-Marquardt to accurately - # estimate - if time >= 0.50: - smoother_batch.update(new_factors, new_values, new_timestamps) - print("Timestamp = " + str(time) + ", Key = " + str(current_key)) - print(smoother_batch.calculateEstimatePose2(current_key)) - - new_timestamps.clear() - new_values.clear() - new_factors.resize(0) - - time += delta_time - - -if __name__ == '__main__': - BatchFixedLagSmootherExample() - print("Example complete") diff --git a/python/gtsam_unstable/examples/TimeOfArrivalExample.py b/python/gtsam_unstable/examples/TimeOfArrivalExample.py deleted file mode 100644 index 59f008a05..000000000 --- a/python/gtsam_unstable/examples/TimeOfArrivalExample.py +++ /dev/null @@ -1,128 +0,0 @@ -""" -GTSAM Copyright 2010-2020, 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 - -Track a moving object "Time of Arrival" measurements at 4 microphones. -Author: Frank Dellaert -""" -# pylint: disable=invalid-name, no-name-in-module - -from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams, - NonlinearFactorGraph, Point3, Values, noiseModel) -from gtsam_unstable import Event, TimeOfArrival, TOAFactor - -# units -MS = 1e-3 -CM = 1e-2 - -# Instantiate functor with speed of sound value -TIME_OF_ARRIVAL = TimeOfArrival(330) - - -def define_microphones(): - """Create microphones.""" - height = 0.5 - microphones = [] - microphones.append(Point3(0, 0, height)) - microphones.append(Point3(403 * CM, 0, height)) - microphones.append(Point3(403 * CM, 403 * CM, height)) - microphones.append(Point3(0, 403 * CM, 2 * height)) - return microphones - - -def create_trajectory(n): - """Create ground truth trajectory.""" - trajectory = [] - timeOfEvent = 10 - # simulate emitting a sound every second while moving on straight line - for key in range(n): - trajectory.append( - Event(timeOfEvent, 245 * CM + key * 1.0, 201.5 * CM, (212 - 45) * CM)) - timeOfEvent += 1 - - return trajectory - - -def simulate_one_toa(microphones, event): - """Simulate time-of-arrival measurements for a single event.""" - return [TIME_OF_ARRIVAL.measure(event, microphones[i]) - for i in range(len(microphones))] - - -def simulate_toa(microphones, trajectory): - """Simulate time-of-arrival measurements for an entire trajectory.""" - return [simulate_one_toa(microphones, event) - for event in trajectory] - - -def create_graph(microphones, simulatedTOA): - """Create factor graph.""" - graph = NonlinearFactorGraph() - - # Create a noise model for the TOA error - model = noiseModel.Isotropic.Sigma(1, 0.5 * MS) - - K = len(microphones) - key = 0 - for toa in simulatedTOA: - for i in range(K): - factor = TOAFactor(key, microphones[i], toa[i], model) - graph.push_back(factor) - key += 1 - - return graph - - -def create_initial_estimate(n): - """Create initial estimate for n events.""" - initial = Values() - zero = Event() - for key in range(n): - TOAFactor.InsertEvent(key, zero, initial) - return initial - - -def toa_example(): - """Run example with 4 microphones and 5 events in a straight line.""" - # Create microphones - microphones = define_microphones() - K = len(microphones) - for i in range(K): - print("mic {} = {}".format(i, microphones[i])) - - # Create a ground truth trajectory - n = 5 - groundTruth = create_trajectory(n) - for event in groundTruth: - print(event) - - # Simulate time-of-arrival measurements - simulatedTOA = simulate_toa(microphones, groundTruth) - for key in range(n): - for i in range(K): - print("z_{}{} = {} ms".format(key, i, simulatedTOA[key][i] / MS)) - - # create factor graph - graph = create_graph(microphones, simulatedTOA) - print(graph.at(0)) - - # Create initial estimate - initial_estimate = create_initial_estimate(n) - print(initial_estimate) - - # Optimize using Levenberg-Marquardt optimization. - params = LevenbergMarquardtParams() - params.setAbsoluteErrorTol(1e-10) - params.setVerbosityLM("SUMMARY") - optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params) - result = optimizer.optimize() - print("Final Result:\n", result) - - -if __name__ == '__main__': - toa_example() - print("Example complete") diff --git a/python/gtsam_unstable/examples/__init__.py b/python/gtsam_unstable/examples/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/python/gtsam_unstable/gtsam_unstable.tpl b/python/gtsam_unstable/gtsam_unstable.tpl deleted file mode 100644 index f8d2f231e..000000000 --- a/python/gtsam_unstable/gtsam_unstable.tpl +++ /dev/null @@ -1,32 +0,0 @@ -{include_boost} - -#include -#include -#include -#include "gtsam/base/serialization.h" -#include "gtsam/nonlinear/utilities.h" // for RedirectCout. - -{includes} -#include - -{boost_class_export} - -{hoder_type} - -#include "python/gtsam_unstable/preamble.h" - -using namespace std; - -namespace py = pybind11; - -PYBIND11_MODULE({module_name}, m_) {{ - m_.doc() = "pybind11 wrapper of {module_name}"; - - py::module::import("gtsam"); - -{wrapped_namespace} - -#include "python/gtsam_unstable/specializations.h" - -}} - diff --git a/python/gtsam_unstable/preamble.h b/python/gtsam_unstable/preamble.h deleted file mode 100644 index e69de29bb..000000000 diff --git a/python/gtsam_unstable/specializations.h b/python/gtsam_unstable/specializations.h deleted file mode 100644 index e69de29bb..000000000 diff --git a/python/gtsam_unstable/tests/__init__.py b/python/gtsam_unstable/tests/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/python/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/python/gtsam_unstable/tests/test_FixedLagSmootherExample.py deleted file mode 100644 index c1ccd1ea1..000000000 --- a/python/gtsam_unstable/tests/test_FixedLagSmootherExample.py +++ /dev/null @@ -1,128 +0,0 @@ -""" -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 - -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((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 = int(1000 * (time - delta_time)) - current_key = int(1000 * time) - - # assign current key to the current timestamp - new_timestamps.insert((current_key, time)) - - # Add a guess for this pose to the new values - # Assume that the robot moves at 2 m/s. Position is time[s] * - # 2[m/s] - current_pose = gtsam.Pose2(time * 2, 0, 0) - new_values.insert(current_key, current_pose) - - # Add odometry factors from two different sources with different - # error stats - odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02) - odometry_noise_1 = gtsam.noiseModel.Diagonal.Sigmas( - np.array([0.1, 0.1, 0.05])) - new_factors.push_back( - gtsam.BetweenFactorPose2( - previous_key, - current_key, - odometry_measurement_1, - odometry_noise_1)) - - odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01) - odometry_noise_2 = gtsam.noiseModel.Diagonal.Sigmas( - np.array([0.05, 0.05, 0.05])) - new_factors.push_back( - gtsam.BetweenFactorPose2( - previous_key, - current_key, - odometry_measurement_2, - odometry_noise_2)) - - # Update the smoothers with the new factors. In this case, - # one iteration must pass for Levenberg-Marquardt to accurately - # estimate - if time >= 0.50: - smoother_batch.update(new_factors, new_values, new_timestamps) - - estimate = smoother_batch.calculateEstimatePose2(current_key) - self.assertTrue(estimate.equals(ground_truth[i], 1e-4)) - i += 1 - - new_timestamps.clear() - new_values.clear() - new_factors.resize(0) - - time += delta_time - - -if __name__ == "__main__": - unittest.main() diff --git a/python/requirements.txt b/python/requirements.txt deleted file mode 100644 index 481d27d8e..000000000 --- a/python/requirements.txt +++ /dev/null @@ -1,2 +0,0 @@ -numpy>=1.11.0 -pyparsing>=2.4.2 diff --git a/python/setup.py.in b/python/setup.py.in deleted file mode 100644 index 55431a9ad..000000000 --- a/python/setup.py.in +++ /dev/null @@ -1,49 +0,0 @@ -import os -import sys - -try: - from setuptools import setup, find_packages -except ImportError: - from distutils.core import setup, find_packages - -packages = find_packages(where=".") -print("PACKAGES: ", packages) -package_data = { - '': [ - './*.so', - './*.dll', - ] -} - -# Cleaner to read in the contents rather than copy them over. -readme_contents = open("${PROJECT_SOURCE_DIR}/README.md").read() - -setup( - name='gtsam', - description='Georgia Tech Smoothing And Mapping library', - url='https://gtsam.org/', - version='${GTSAM_VERSION_STRING}', # https://www.python.org/dev/peps/pep-0440/ - author='Frank Dellaert et. al.', - author_email='frank.dellaert@gtsam.org', - 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_data, - test_suite="gtsam.tests", - install_requires=["numpy"], - zip_safe=False, -)