diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index 99fddda68..d026aa123 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -9,33 +9,14 @@ set -x -e # install TBB with _debug.so files function install_tbb() { - TBB_BASEURL=https://github.com/oneapi-src/oneTBB/releases/download - TBB_VERSION=4.4.5 - TBB_DIR=tbb44_20160526oss - TBB_SAVEPATH="/tmp/tbb.tgz" - if [ "$(uname)" == "Linux" ]; then - OS_SHORT="lin" - TBB_LIB_DIR="intel64/gcc4.4" - SUDO="sudo" + sudo apt-get -y install libtbb-dev elif [ "$(uname)" == "Darwin" ]; then - OS_SHORT="osx" - TBB_LIB_DIR="" - SUDO="" + brew install tbb fi - wget "${TBB_BASEURL}/${TBB_VERSION}/${TBB_DIR}_${OS_SHORT}.tgz" -O $TBB_SAVEPATH - tar -C /tmp -xf $TBB_SAVEPATH - - TBBROOT=/tmp/$TBB_DIR - # Copy the needed files to the correct places. - # This works correctly for CI builds, instead of setting path variables. - # This is what Homebrew does to install TBB on Macs - $SUDO cp -R $TBBROOT/lib/$TBB_LIB_DIR/* /usr/local/lib/ - $SUDO cp -R $TBBROOT/include/ /usr/local/include/ - } if [ -z ${PYTHON_VERSION+x} ]; then diff --git a/.github/scripts/unix.sh b/.github/scripts/unix.sh index af9ac8991..557255474 100644 --- a/.github/scripts/unix.sh +++ b/.github/scripts/unix.sh @@ -8,33 +8,14 @@ # install TBB with _debug.so files function install_tbb() { - TBB_BASEURL=https://github.com/oneapi-src/oneTBB/releases/download - TBB_VERSION=4.4.5 - TBB_DIR=tbb44_20160526oss - TBB_SAVEPATH="/tmp/tbb.tgz" - if [ "$(uname)" == "Linux" ]; then - OS_SHORT="lin" - TBB_LIB_DIR="intel64/gcc4.4" - SUDO="sudo" + sudo apt-get -y install libtbb-dev elif [ "$(uname)" == "Darwin" ]; then - OS_SHORT="osx" - TBB_LIB_DIR="" - SUDO="" + brew install tbb fi - wget "${TBB_BASEURL}/${TBB_VERSION}/${TBB_DIR}_${OS_SHORT}.tgz" -O $TBB_SAVEPATH - tar -C /tmp -xf $TBB_SAVEPATH - - TBBROOT=/tmp/$TBB_DIR - # Copy the needed files to the correct places. - # This works correctly for CI builds, instead of setting path variables. - # This is what Homebrew does to install TBB on Macs - $SUDO cp -R $TBBROOT/lib/$TBB_LIB_DIR/* /usr/local/lib/ - $SUDO cp -R $TBBROOT/include/ /usr/local/include/ - } # common tasks before either build or test diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 129b5aaaf..e4937ce06 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -2,6 +2,12 @@ name: Linux CI on: [pull_request] +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: build: name: ${{ matrix.name }} ${{ matrix.build_type }} diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 3fa3c15dd..541701836 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -2,6 +2,12 @@ name: macOS CI on: [pull_request] +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: build: name: ${{ matrix.name }} ${{ matrix.build_type }} diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index f09d589dc..ca4645a77 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -2,6 +2,12 @@ name: Python CI on: [pull_request] +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: build: name: ${{ matrix.name }} ${{ matrix.build_type }} Python ${{ matrix.python_version }} diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index f72dadbae..72466ffd6 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -2,6 +2,12 @@ name: Special Cases CI on: [pull_request] +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: build: name: ${{ matrix.name }} ${{ matrix.build_type }} diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 3d4bf3faf..cbe0c10f1 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -2,6 +2,12 @@ name: Windows CI on: [pull_request] +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: build: name: ${{ matrix.name }} ${{ matrix.build_type }} @@ -21,24 +27,15 @@ jobs: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ - #TODO This build fails, need to understand why. - # windows-2016-cl, windows-2019-cl, ] build_type: [ Debug, - #TODO(Varun) The release build takes over 2.5 hours, need to figure out why. - # Release + Release ] build_unstable: [ON] include: - #TODO This build fails, need to understand why. - # - name: windows-2016-cl - # os: windows-2016 - # compiler: cl - # platform: 32 - - name: windows-2019-cl os: windows-2019 compiler: cl @@ -125,4 +122,3 @@ jobs: # Run GTSAM_UNSTABLE tests #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base_unstable - diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index 4a4f1a36e..13000a5b0 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -19,7 +19,8 @@ option(GTSAM_FORCE_STATIC_LIB "Force gtsam to be a static library, option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON) option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON) -option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) +option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) +option(GTSAM_ENABLE_MEMORY_SANITIZER "Enable/Disable memory sanitizer" OFF) option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF) option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF) @@ -30,6 +31,7 @@ option(GTSAM_ALLOW_DEPRECATED_SINCE_V43 "Allow use of methods/functions depr option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) option(GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR "Use the slower but correct version of BetweenFactor" OFF) +option(GTSAM_SLOW_BUT_CORRECT_EXPMAP "Use slower but correct expmap for Pose2" OFF) if (GTSAM_FORCE_SHARED_LIB) message(STATUS "GTSAM is a shared library due to GTSAM_FORCE_SHARED_LIB") diff --git a/cmake/HandleGlobalBuildFlags.cmake b/cmake/HandleGlobalBuildFlags.cmake index cb48f875b..eba6645d7 100644 --- a/cmake/HandleGlobalBuildFlags.cmake +++ b/cmake/HandleGlobalBuildFlags.cmake @@ -50,3 +50,10 @@ if(GTSAM_ENABLE_CONSISTENCY_CHECKS) # This should be made PUBLIC if GTSAM_EXTRA_CONSISTENCY_CHECKS is someday used in a public .h list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE GTSAM_EXTRA_CONSISTENCY_CHECKS) endif() + +if(GTSAM_ENABLE_MEMORY_SANITIZER) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fsanitize=address -fsanitize=leak -g") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=address -fsanitize=leak -g") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address -fsanitize=leak") + set(CMAKE_MODULE_LINKER_FLAGS "${CMAKE_MODULE_LINKER_FLAGS} -fsanitize=address -fsanitize=leak") +endif() diff --git a/cmake/HandlePrintConfiguration.cmake b/cmake/HandlePrintConfiguration.cmake index b17d522d9..c5c3920cb 100644 --- a/cmake/HandlePrintConfiguration.cmake +++ b/cmake/HandlePrintConfiguration.cmake @@ -87,6 +87,7 @@ print_config("CPack Generator" "${CPACK_GENERATOR}") message(STATUS "GTSAM flags ") print_enabled_config(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ") print_enabled_config(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") +print_enabled_config(${GTSAM_ENABLE_MEMORY_SANITIZER} "Build with Memory Sanitizer ") print_enabled_config(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") print_enabled_config(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V43} "Allow features deprecated in GTSAM 4.3") diff --git a/gtsam/base/base.i b/gtsam/base/base.i index 9b9f351ce..31d09cb3d 100644 --- a/gtsam/base/base.i +++ b/gtsam/base/base.i @@ -41,6 +41,7 @@ class DSFMap { std::map sets(); }; +// Used in Matlab wrapper class IndexPairSet { IndexPairSet(); // common STL methods @@ -54,6 +55,7 @@ class IndexPairSet { bool count(gtsam::IndexPair key) const; // returns true if value exists }; +// Used in Matlab wrapper class IndexPairVector { IndexPairVector(); IndexPairVector(const gtsam::IndexPairVector& other); @@ -70,6 +72,7 @@ class IndexPairVector { gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set); +// Used in Matlab wrapper class IndexPairSetMap { IndexPairSetMap(); // common STL methods diff --git a/gtsam/base/tests/testStdOptionalSerialization.cpp b/gtsam/base/tests/testStdOptionalSerialization.cpp index dd99b0f12..d9bd1da4a 100644 --- a/gtsam/base/tests/testStdOptionalSerialization.cpp +++ b/gtsam/base/tests/testStdOptionalSerialization.cpp @@ -149,6 +149,9 @@ TEST(StdOptionalSerialization, SerializTestOptionalStructPointerPointer) { // Check that it worked EXPECT(opt2.has_value()); EXPECT(**opt2 == TestOptionalStruct(42)); + + delete (*opt); + delete (*opt2); } int main() { diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 5d2f1b49a..70cc5b1dc 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -138,6 +138,8 @@ void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) { /** Traverse a forest depth-first with pre-order and post-order visits. * @param forest The forest of trees to traverse. The method \c forest.roots() should exist * and return a collection of (shared) pointers to \c FOREST::Node. + * @param rootData The data to pass by reference to \c visitorPre when it is called on each + * root node. * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before * visiting its children, and will be passed, by reference, the \c DATA object returned * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object @@ -147,8 +149,8 @@ void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) { * @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting * its children, and will be passed, by reference, the \c DATA object returned by the * call to \c visitorPre (the \c DATA object may be modified by visiting the children). - * @param rootData The data to pass by reference to \c visitorPre when it is called on each - * root node. */ + * @param problemSizeThreshold + */ template void DepthFirstForestParallel(FOREST& forest, DATA& rootData, diff --git a/gtsam/basis/Basis.cpp b/gtsam/basis/Basis.cpp new file mode 100644 index 000000000..3e684b197 --- /dev/null +++ b/gtsam/basis/Basis.cpp @@ -0,0 +1,33 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file Basis.cpp + * @brief Compute an interpolating basis + * @author Varun Agrawal + * @date June 20, 2023 + */ + +#include + +namespace gtsam { + +Matrix kroneckerProductIdentity(size_t M, const Weights& w) { + Matrix result(M, w.cols() * M); + result.setZero(); + + for (int i = 0; i < w.cols(); i++) { + result.block(0, i * M, M, M).diagonal().array() = w(i); + } + return result; +} + +} // namespace gtsam diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index 97704dab4..41cdeeaaa 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -20,7 +20,6 @@ #include #include -#include #include @@ -81,16 +80,7 @@ using Weights = Eigen::Matrix; /* 1xN vector */ * * @ingroup basis */ -template -Matrix kroneckerProductIdentity(const Weights& w) { - Matrix result(M, w.cols() * M); - result.setZero(); - - for (int i = 0; i < w.cols(); i++) { - result.block(0, i * M, M, M).diagonal().array() = w(i); - } - return result; -} +Matrix kroneckerProductIdentity(size_t M, const Weights& w); /** * CRTP Base class for function bases @@ -169,18 +159,18 @@ class Basis { }; /** - * VectorEvaluationFunctor at a given x, applied to ParameterMatrix. + * VectorEvaluationFunctor at a given x, applied to a parameter Matrix. * This functor is used to evaluate a parameterized function at a given scalar * value x. When given a specific M*N parameters, returns an M-vector the M * corresponding functions at x, possibly with Jacobians wrpt the parameters. */ - template class VectorEvaluationFunctor : protected EvaluationFunctor { protected: - using VectorM = Eigen::Matrix; - using Jacobian = Eigen::Matrix; + using Jacobian = Eigen::Matrix; Jacobian H_; + size_t M_; + /** * Calculate the `M*(M*N)` Jacobian of this functor with respect to * the M*N parameter matrix `P`. @@ -190,7 +180,7 @@ class Basis { * i.e., the Kronecker product of weights_ with the MxM identity matrix. */ void calculateJacobian() { - H_ = kroneckerProductIdentity(this->weights_); + H_ = kroneckerProductIdentity(M_, this->weights_); } public: @@ -200,26 +190,27 @@ class Basis { VectorEvaluationFunctor() {} /// Default Constructor - VectorEvaluationFunctor(size_t N, double x) : EvaluationFunctor(N, x) { + VectorEvaluationFunctor(size_t M, size_t N, double x) + : EvaluationFunctor(N, x), M_(M) { calculateJacobian(); } /// Constructor, with interval [a,b] - VectorEvaluationFunctor(size_t N, double x, double a, double b) - : EvaluationFunctor(N, x, a, b) { + VectorEvaluationFunctor(size_t M, size_t N, double x, double a, double b) + : EvaluationFunctor(N, x, a, b), M_(M) { calculateJacobian(); } /// M-dimensional evaluation - VectorM apply(const ParameterMatrix& P, - OptionalJacobian H = {}) const { + Vector apply(const Matrix& P, + OptionalJacobian H = {}) const { if (H) *H = H_; return P.matrix() * this->weights_.transpose(); } /// c++ sugar - VectorM operator()(const ParameterMatrix& P, - OptionalJacobian H = {}) const { + Vector operator()(const Matrix& P, + OptionalJacobian H = {}) const { return apply(P, H); } }; @@ -231,13 +222,14 @@ class Basis { * * This component is specified by the row index i, with 0 class VectorComponentFunctor : public EvaluationFunctor { protected: using Jacobian = Eigen::Matrix; - size_t rowIndex_; Jacobian H_; + size_t M_; + size_t rowIndex_; + /* * Calculate the `1*(M*N)` Jacobian of this functor with respect to * the M*N parameter matrix `P`. @@ -248,9 +240,9 @@ class Basis { * MxM identity matrix. See also VectorEvaluationFunctor. */ void calculateJacobian(size_t N) { - H_.setZero(1, M * N); + H_.setZero(1, M_ * N); for (int j = 0; j < EvaluationFunctor::weights_.size(); j++) - H_(0, rowIndex_ + j * M) = EvaluationFunctor::weights_(j); + H_(0, rowIndex_ + j * M_) = EvaluationFunctor::weights_(j); } public: @@ -258,33 +250,34 @@ class Basis { VectorComponentFunctor() {} /// Construct with row index - VectorComponentFunctor(size_t N, size_t i, double x) - : EvaluationFunctor(N, x), rowIndex_(i) { + VectorComponentFunctor(size_t M, size_t N, size_t i, double x) + : EvaluationFunctor(N, x), M_(M), rowIndex_(i) { calculateJacobian(N); } /// Construct with row index and interval - VectorComponentFunctor(size_t N, size_t i, double x, double a, double b) - : EvaluationFunctor(N, x, a, b), rowIndex_(i) { + VectorComponentFunctor(size_t M, size_t N, size_t i, double x, double a, + double b) + : EvaluationFunctor(N, x, a, b), M_(M), rowIndex_(i) { calculateJacobian(N); } /// Calculate component of component rowIndex_ of P - double apply(const ParameterMatrix& P, + double apply(const Matrix& P, OptionalJacobian H = {}) const { if (H) *H = H_; return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose(); } /// c++ sugar - double operator()(const ParameterMatrix& P, + double operator()(const Matrix& P, OptionalJacobian H = {}) const { return apply(P, H); } }; /** - * Manifold EvaluationFunctor at a given x, applied to ParameterMatrix. + * Manifold EvaluationFunctor at a given x, applied to a parameter Matrix. * This functor is used to evaluate a parameterized function at a given scalar * value x. When given a specific M*N parameters, returns an M-vector the M * corresponding functions at x, possibly with Jacobians wrpt the parameters. @@ -297,25 +290,23 @@ class Basis { * 3D rotation. */ template - class ManifoldEvaluationFunctor - : public VectorEvaluationFunctor::dimension> { + class ManifoldEvaluationFunctor : public VectorEvaluationFunctor { enum { M = traits::dimension }; - using Base = VectorEvaluationFunctor; + using Base = VectorEvaluationFunctor; public: /// For serialization ManifoldEvaluationFunctor() {} /// Default Constructor - ManifoldEvaluationFunctor(size_t N, double x) : Base(N, x) {} + ManifoldEvaluationFunctor(size_t N, double x) : Base(M, N, x) {} /// Constructor, with interval [a,b] ManifoldEvaluationFunctor(size_t N, double x, double a, double b) - : Base(N, x, a, b) {} + : Base(M, N, x, a, b) {} /// Manifold evaluation - T apply(const ParameterMatrix& P, - OptionalJacobian H = {}) const { + T apply(const Matrix& P, OptionalJacobian H = {}) const { // Interpolate the M-dimensional vector to yield a vector in tangent space Eigen::Matrix xi = Base::operator()(P, H); @@ -333,7 +324,7 @@ class Basis { } /// c++ sugar - T operator()(const ParameterMatrix& P, + T operator()(const Matrix& P, OptionalJacobian H = {}) const { return apply(P, H); // might call apply in derived } @@ -389,20 +380,20 @@ class Basis { }; /** - * VectorDerivativeFunctor at a given x, applied to ParameterMatrix. + * VectorDerivativeFunctor at a given x, applied to a parameter Matrix. * * This functor is used to evaluate the derivatives of a parameterized * function at a given scalar value x. When given a specific M*N parameters, * returns an M-vector the M corresponding function derivatives at x, possibly * with Jacobians wrpt the parameters. */ - template class VectorDerivativeFunctor : protected DerivativeFunctorBase { protected: - using VectorM = Eigen::Matrix; - using Jacobian = Eigen::Matrix; + using Jacobian = Eigen::Matrix; Jacobian H_; + size_t M_; + /** * Calculate the `M*(M*N)` Jacobian of this functor with respect to * the M*N parameter matrix `P`. @@ -412,7 +403,7 @@ class Basis { * i.e., the Kronecker product of weights_ with the MxM identity matrix. */ void calculateJacobian() { - H_ = kroneckerProductIdentity(this->weights_); + H_ = kroneckerProductIdentity(M_, this->weights_); } public: @@ -422,25 +413,25 @@ class Basis { VectorDerivativeFunctor() {} /// Default Constructor - VectorDerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) { + VectorDerivativeFunctor(size_t M, size_t N, double x) + : DerivativeFunctorBase(N, x), M_(M) { calculateJacobian(); } /// Constructor, with optional interval [a,b] - VectorDerivativeFunctor(size_t N, double x, double a, double b) - : DerivativeFunctorBase(N, x, a, b) { + VectorDerivativeFunctor(size_t M, size_t N, double x, double a, double b) + : DerivativeFunctorBase(N, x, a, b), M_(M) { calculateJacobian(); } - VectorM apply(const ParameterMatrix& P, - OptionalJacobian H = {}) const { + Vector apply(const Matrix& P, + OptionalJacobian H = {}) const { if (H) *H = H_; return P.matrix() * this->weights_.transpose(); } /// c++ sugar - VectorM operator()( - const ParameterMatrix& P, - OptionalJacobian H = {}) const { + Vector operator()(const Matrix& P, + OptionalJacobian H = {}) const { return apply(P, H); } }; @@ -452,13 +443,14 @@ class Basis { * * This component is specified by the row index i, with 0 class ComponentDerivativeFunctor : protected DerivativeFunctorBase { protected: using Jacobian = Eigen::Matrix; - size_t rowIndex_; Jacobian H_; + size_t M_; + size_t rowIndex_; + /* * Calculate the `1*(M*N)` Jacobian of this functor with respect to * the M*N parameter matrix `P`. @@ -469,9 +461,9 @@ class Basis { * MxM identity matrix. See also VectorDerivativeFunctor. */ void calculateJacobian(size_t N) { - H_.setZero(1, M * N); + H_.setZero(1, M_ * N); for (int j = 0; j < this->weights_.size(); j++) - H_(0, rowIndex_ + j * M) = this->weights_(j); + H_(0, rowIndex_ + j * M_) = this->weights_(j); } public: @@ -479,29 +471,29 @@ class Basis { ComponentDerivativeFunctor() {} /// Construct with row index - ComponentDerivativeFunctor(size_t N, size_t i, double x) - : DerivativeFunctorBase(N, x), rowIndex_(i) { + ComponentDerivativeFunctor(size_t M, size_t N, size_t i, double x) + : DerivativeFunctorBase(N, x), M_(M), rowIndex_(i) { calculateJacobian(N); } /// Construct with row index and interval - ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b) - : DerivativeFunctorBase(N, x, a, b), rowIndex_(i) { + ComponentDerivativeFunctor(size_t M, size_t N, size_t i, double x, double a, + double b) + : DerivativeFunctorBase(N, x, a, b), M_(M), rowIndex_(i) { calculateJacobian(N); } /// Calculate derivative of component rowIndex_ of F - double apply(const ParameterMatrix& P, + double apply(const Matrix& P, OptionalJacobian H = {}) const { if (H) *H = H_; return P.row(rowIndex_) * this->weights_.transpose(); } /// c++ sugar - double operator()(const ParameterMatrix& P, + double operator()(const Matrix& P, OptionalJacobian H = {}) const { return apply(P, H); } }; - }; } // namespace gtsam diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h index 0e42a8866..ad92ad672 100644 --- a/gtsam/basis/BasisFactors.h +++ b/gtsam/basis/BasisFactors.h @@ -75,7 +75,7 @@ class EvaluationFactor : public FunctorizedFactor { }; /** - * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix + * Unary factor for enforcing BASIS polynomial evaluation on a parameter Matrix * of size (M, N) is equal to a vector-valued measurement at the same point, when * using a pseudo-spectral parameterization. @@ -87,15 +87,13 @@ class EvaluationFactor : public FunctorizedFactor { * measurement prediction function. * * @param BASIS: The basis class to use e.g. Chebyshev2 - * @param M: Size of the evaluated state vector. * * @ingroup basis */ -template -class VectorEvaluationFactor - : public FunctorizedFactor> { +template +class VectorEvaluationFactor : public FunctorizedFactor { private: - using Base = FunctorizedFactor>; + using Base = FunctorizedFactor; public: VectorEvaluationFactor() {} @@ -103,42 +101,43 @@ class VectorEvaluationFactor /** * @brief Construct a new VectorEvaluationFactor object. * - * @param key The key to the ParameterMatrix object used to represent the + * @param key The key to the parameter Matrix object used to represent the * polynomial. * @param z The measurement value. * @param model The noise model. + * @param M Size of the evaluated state vector. * @param N The degree of the polynomial. * @param x The point at which to evaluate the basis polynomial. */ VectorEvaluationFactor(Key key, const Vector &z, - const SharedNoiseModel &model, const size_t N, - double x) - : Base(key, z, model, - typename BASIS::template VectorEvaluationFunctor(N, x)) {} + const SharedNoiseModel &model, const size_t M, + const size_t N, double x) + : Base(key, z, model, typename BASIS::VectorEvaluationFunctor(M, N, x)) {} /** * @brief Construct a new VectorEvaluationFactor object. * - * @param key The key to the ParameterMatrix object used to represent the + * @param key The key to the parameter Matrix object used to represent the * polynomial. * @param z The measurement value. * @param model The noise model. + * @param M Size of the evaluated state vector. * @param N The degree of the polynomial. * @param x The point at which to evaluate the basis polynomial. * @param a Lower bound for the polynomial. * @param b Upper bound for the polynomial. */ VectorEvaluationFactor(Key key, const Vector &z, - const SharedNoiseModel &model, const size_t N, - double x, double a, double b) + const SharedNoiseModel &model, const size_t M, + const size_t N, double x, double a, double b) : Base(key, z, model, - typename BASIS::template VectorEvaluationFunctor(N, x, a, b)) {} + typename BASIS::VectorEvaluationFunctor(M, N, x, a, b)) {} virtual ~VectorEvaluationFactor() {} }; /** - * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix + * Unary factor for enforcing BASIS polynomial evaluation on a parameter Matrix * of size (P, N) is equal to specified measurement at the same point, when * using a pseudo-spectral parameterization. * @@ -147,20 +146,18 @@ class VectorEvaluationFactor * indexed by `i`. * * @param BASIS: The basis class to use e.g. Chebyshev2 - * @param P: Size of the fixed-size vector. * * Example: - * VectorComponentFactor controlPrior(key, measured, model, - * N, i, t, a, b); + * VectorComponentFactor controlPrior(key, measured, model, + * N, i, t, a, b); * where N is the degree and i is the component index. * * @ingroup basis */ -template -class VectorComponentFactor - : public FunctorizedFactor> { +template +class VectorComponentFactor : public FunctorizedFactor { private: - using Base = FunctorizedFactor>; + using Base = FunctorizedFactor; public: VectorComponentFactor() {} @@ -168,29 +165,31 @@ class VectorComponentFactor /** * @brief Construct a new VectorComponentFactor object. * - * @param key The key to the ParameterMatrix object used to represent the + * @param key The key to the parameter Matrix object used to represent the * polynomial. * @param z The scalar value at a specified index `i` of the full measurement * vector. * @param model The noise model. + * @param P Size of the fixed-size vector. * @param N The degree of the polynomial. * @param i The index for the evaluated vector to give us the desired scalar * value. * @param x The point at which to evaluate the basis polynomial. */ VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, - const size_t N, size_t i, double x) + const size_t P, const size_t N, size_t i, double x) : Base(key, z, model, - typename BASIS::template VectorComponentFunctor

(N, i, x)) {} + typename BASIS::VectorComponentFunctor(P, N, i, x)) {} /** * @brief Construct a new VectorComponentFactor object. * - * @param key The key to the ParameterMatrix object used to represent the + * @param key The key to the parameter Matrix object used to represent the * polynomial. * @param z The scalar value at a specified index `i` of the full measurement * vector. * @param model The noise model. + * @param P Size of the fixed-size vector. * @param N The degree of the polynomial. * @param i The index for the evaluated vector to give us the desired scalar * value. @@ -199,11 +198,10 @@ class VectorComponentFactor * @param b Upper bound for the polynomial. */ VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, - const size_t N, size_t i, double x, double a, double b) - : Base( - key, z, model, - typename BASIS::template VectorComponentFunctor

(N, i, x, a, b)) { - } + const size_t P, const size_t N, size_t i, double x, + double a, double b) + : Base(key, z, model, + typename BASIS::VectorComponentFunctor(P, N, i, x, a, b)) {} virtual ~VectorComponentFactor() {} }; @@ -226,10 +224,9 @@ class VectorComponentFactor * where `x` is the value (e.g. timestep) at which the rotation was evaluated. */ template -class ManifoldEvaluationFactor - : public FunctorizedFactor::dimension>> { +class ManifoldEvaluationFactor : public FunctorizedFactor { private: - using Base = FunctorizedFactor::dimension>>; + using Base = FunctorizedFactor; public: ManifoldEvaluationFactor() {} @@ -289,7 +286,7 @@ class DerivativeFactor /** * @brief Construct a new DerivativeFactor object. * - * @param key The key to the ParameterMatrix which represents the basis + * @param key The key to the parameter Matrix which represents the basis * polynomial. * @param z The measurement value. * @param model The noise model. @@ -303,7 +300,7 @@ class DerivativeFactor /** * @brief Construct a new DerivativeFactor object. * - * @param key The key to the ParameterMatrix which represents the basis + * @param key The key to the parameter Matrix which represents the basis * polynomial. * @param z The measurement value. * @param model The noise model. @@ -324,14 +321,12 @@ class DerivativeFactor * polynomial at a specified point `x` is equal to the vector value `z`. * * @param BASIS: The basis class to use e.g. Chebyshev2 - * @param M: Size of the evaluated state vector derivative. */ -template -class VectorDerivativeFactor - : public FunctorizedFactor> { +template +class VectorDerivativeFactor : public FunctorizedFactor { private: - using Base = FunctorizedFactor>; - using Func = typename BASIS::template VectorDerivativeFunctor; + using Base = FunctorizedFactor; + using Func = typename BASIS::VectorDerivativeFunctor; public: VectorDerivativeFactor() {} @@ -339,34 +334,36 @@ class VectorDerivativeFactor /** * @brief Construct a new VectorDerivativeFactor object. * - * @param key The key to the ParameterMatrix which represents the basis + * @param key The key to the parameter Matrix which represents the basis * polynomial. * @param z The measurement value. * @param model The noise model. + * @param M Size of the evaluated state vector derivative. * @param N The degree of the polynomial. * @param x The point at which to evaluate the basis polynomial. */ VectorDerivativeFactor(Key key, const Vector &z, - const SharedNoiseModel &model, const size_t N, - double x) - : Base(key, z, model, Func(N, x)) {} + const SharedNoiseModel &model, const size_t M, + const size_t N, double x) + : Base(key, z, model, Func(M, N, x)) {} /** * @brief Construct a new VectorDerivativeFactor object. * - * @param key The key to the ParameterMatrix which represents the basis + * @param key The key to the parameter Matrix which represents the basis * polynomial. * @param z The measurement value. * @param model The noise model. + * @param M Size of the evaluated state vector derivative. * @param N The degree of the polynomial. * @param x The point at which to evaluate the basis polynomial. * @param a Lower bound for the polynomial. * @param b Upper bound for the polynomial. */ VectorDerivativeFactor(Key key, const Vector &z, - const SharedNoiseModel &model, const size_t N, - double x, double a, double b) - : Base(key, z, model, Func(N, x, a, b)) {} + const SharedNoiseModel &model, const size_t M, + const size_t N, double x, double a, double b) + : Base(key, z, model, Func(M, N, x, a, b)) {} virtual ~VectorDerivativeFactor() {} }; @@ -377,14 +374,12 @@ class VectorDerivativeFactor * vector-valued measurement `z`. * * @param BASIS: The basis class to use e.g. Chebyshev2 - * @param P: Size of the control component derivative. */ -template -class ComponentDerivativeFactor - : public FunctorizedFactor> { +template +class ComponentDerivativeFactor : public FunctorizedFactor { private: - using Base = FunctorizedFactor>; - using Func = typename BASIS::template ComponentDerivativeFunctor

; + using Base = FunctorizedFactor; + using Func = typename BASIS::ComponentDerivativeFunctor; public: ComponentDerivativeFactor() {} @@ -392,29 +387,31 @@ class ComponentDerivativeFactor /** * @brief Construct a new ComponentDerivativeFactor object. * - * @param key The key to the ParameterMatrix which represents the basis + * @param key The key to the parameter Matrix which represents the basis * polynomial. * @param z The scalar measurement value at a specific index `i` of the full * measurement vector. * @param model The degree of the polynomial. + * @param P: Size of the control component derivative. * @param N The degree of the polynomial. * @param i The index for the evaluated vector to give us the desired scalar * value. * @param x The point at which to evaluate the basis polynomial. */ ComponentDerivativeFactor(Key key, const double &z, - const SharedNoiseModel &model, const size_t N, - size_t i, double x) - : Base(key, z, model, Func(N, i, x)) {} + const SharedNoiseModel &model, const size_t P, + const size_t N, size_t i, double x) + : Base(key, z, model, Func(P, N, i, x)) {} /** * @brief Construct a new ComponentDerivativeFactor object. * - * @param key The key to the ParameterMatrix which represents the basis + * @param key The key to the parameter Matrix which represents the basis * polynomial. * @param z The scalar measurement value at a specific index `i` of the full * measurement vector. * @param model The degree of the polynomial. + * @param P: Size of the control component derivative. * @param N The degree of the polynomial. * @param i The index for the evaluated vector to give us the desired scalar * value. @@ -423,9 +420,10 @@ class ComponentDerivativeFactor * @param b Upper bound for the polynomial. */ ComponentDerivativeFactor(Key key, const double &z, - const SharedNoiseModel &model, const size_t N, - size_t i, double x, double a, double b) - : Base(key, z, model, Func(N, i, x, a, b)) {} + const SharedNoiseModel &model, const size_t P, + const size_t N, size_t i, double x, double a, + double b) + : Base(key, z, model, Func(P, N, i, x, a, b)) {} virtual ~ComponentDerivativeFactor() {} }; diff --git a/gtsam/basis/ParameterMatrix.h b/gtsam/basis/ParameterMatrix.h deleted file mode 100644 index 81686e046..000000000 --- a/gtsam/basis/ParameterMatrix.h +++ /dev/null @@ -1,215 +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 - - * -------------------------------------------------------------------------- */ - -/** - * @file ParameterMatrix.h - * @brief Define ParameterMatrix class which is used to store values at - * interpolation points. - * @author Varun Agrawal, Frank Dellaert - * @date September 21, 2020 - */ - -#pragma once - -#include -#include -#include - -#include - -namespace gtsam { - -/** - * A matrix abstraction of MxN values at the Basis points. - * This class serves as a wrapper over an Eigen matrix. - * @tparam M: The dimension of the type you wish to evaluate. - * @param N: the number of Basis points (e.g. Chebyshev points of the second - * kind). - */ -template -class ParameterMatrix { - using MatrixType = Eigen::Matrix; - - private: - MatrixType matrix_; - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - enum { dimension = Eigen::Dynamic }; - - /** - * Create ParameterMatrix using the number of basis points. - * @param N: The number of basis points (the columns). - */ - ParameterMatrix(const size_t N) : matrix_(M, N) { matrix_.setZero(); } - - /** - * Create ParameterMatrix from an MxN Eigen Matrix. - * @param matrix: An Eigen matrix used to initialze the ParameterMatrix. - */ - ParameterMatrix(const MatrixType& matrix) : matrix_(matrix) {} - - /// Get the number of rows. - size_t rows() const { return matrix_.rows(); } - - /// Get the number of columns. - size_t cols() const { return matrix_.cols(); } - - /// Get the underlying matrix. - MatrixType matrix() const { return matrix_; } - - /// Return the tranpose of the underlying matrix. - Eigen::Matrix transpose() const { return matrix_.transpose(); } - - /** - * Get the matrix row specified by `index`. - * @param index: The row index to retrieve. - */ - Eigen::Matrix row(size_t index) const { - return matrix_.row(index); - } - - /** - * Set the matrix row specified by `index`. - * @param index: The row index to set. - */ - auto row(size_t index) -> Eigen::Block { - return matrix_.row(index); - } - - /** - * Get the matrix column specified by `index`. - * @param index: The column index to retrieve. - */ - Eigen::Matrix col(size_t index) const { - return matrix_.col(index); - } - - /** - * Set the matrix column specified by `index`. - * @param index: The column index to set. - */ - auto col(size_t index) -> Eigen::Block { - return matrix_.col(index); - } - - /** - * Set all matrix coefficients to zero. - */ - void setZero() { matrix_.setZero(); } - - /** - * Add a ParameterMatrix to another. - * @param other: ParameterMatrix to add. - */ - ParameterMatrix operator+(const ParameterMatrix& other) const { - return ParameterMatrix(matrix_ + other.matrix()); - } - - /** - * Add a MxN-sized vector to the ParameterMatrix. - * @param other: Vector which is reshaped and added. - */ - ParameterMatrix operator+( - const Eigen::Matrix& other) const { - // This form avoids a deep copy and instead typecasts `other`. - Eigen::Map other_(other.data(), M, cols()); - return ParameterMatrix(matrix_ + other_); - } - - /** - * Subtract a ParameterMatrix from another. - * @param other: ParameterMatrix to subtract. - */ - ParameterMatrix operator-(const ParameterMatrix& other) const { - return ParameterMatrix(matrix_ - other.matrix()); - } - - /** - * Subtract a MxN-sized vector from the ParameterMatrix. - * @param other: Vector which is reshaped and subracted. - */ - ParameterMatrix operator-( - const Eigen::Matrix& other) const { - Eigen::Map other_(other.data(), M, cols()); - return ParameterMatrix(matrix_ - other_); - } - - /** - * Multiply ParameterMatrix with an Eigen matrix. - * @param other: Eigen matrix which should be multiplication compatible with - * the ParameterMatrix. - */ - MatrixType operator*(const Eigen::Matrix& other) const { - return matrix_ * other; - } - - /// @name Vector Space requirements - /// @{ - - /** - * Print the ParameterMatrix. - * @param s: The prepend string to add more contextual info. - */ - void print(const std::string& s = "") const { - std::cout << (s == "" ? s : s + " ") << matrix_ << std::endl; - } - - /** - * Check for equality up to absolute tolerance. - * @param other: The ParameterMatrix to check equality with. - * @param tol: The absolute tolerance threshold. - */ - bool equals(const ParameterMatrix& other, double tol = 1e-8) const { - return gtsam::equal_with_abs_tol(matrix_, other.matrix(), tol); - } - - /// Returns dimensionality of the tangent space - inline size_t dim() const { return matrix_.size(); } - - /// Convert to vector form, is done row-wise - inline Vector vector() const { - using RowMajor = Eigen::Matrix; - Vector result(matrix_.size()); - Eigen::Map(&result(0), rows(), cols()) = matrix_; - return result; - } - - /** Identity function to satisfy VectorSpace traits. - * - * NOTE: The size at compile time is unknown so this identity is zero - * length and thus not valid. - */ - inline static ParameterMatrix Identity() { - // throw std::runtime_error( - // "ParameterMatrix::Identity(): Don't use this function"); - return ParameterMatrix(0); - } - - /// @} -}; - -// traits for ParameterMatrix -template -struct traits> - : public internal::VectorSpace> {}; - -/* ************************************************************************* */ -// Stream operator that takes a ParameterMatrix. Used for printing. -template -inline std::ostream& operator<<(std::ostream& os, - const ParameterMatrix& parameterMatrix) { - os << parameterMatrix.matrix(); - return os; -} - -} // namespace gtsam \ No newline at end of file diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i index 0cd87ba65..f8cea70f8 100644 --- a/gtsam/basis/basis.i +++ b/gtsam/basis/basis.i @@ -46,18 +46,6 @@ class Chebyshev2 { static Matrix DifferentiationMatrix(size_t N, double a, double b); }; -#include - -template -class ParameterMatrix { - ParameterMatrix(const size_t N); - ParameterMatrix(const Matrix& matrix); - - Matrix matrix() const; - - void print(const string& s = "") const; -}; - #include template +template virtual class VectorEvaluationFactor : gtsam::NoiseModelFactor { VectorEvaluationFactor(); VectorEvaluationFactor(gtsam::Key key, const Vector& z, - const gtsam::noiseModel::Base* model, const size_t N, - double x); + const gtsam::noiseModel::Base* model, const size_t M, + const size_t N, double x); VectorEvaluationFactor(gtsam::Key key, const Vector& z, - const gtsam::noiseModel::Base* model, const size_t N, - double x, double a, double b); + const gtsam::noiseModel::Base* model, const size_t M, + const size_t N, double x, double a, double b); }; -// TODO(Varun) Better way to support arbitrary dimensions? -// Especially if users mainly do `pip install gtsam` for the Python wrapper. -typedef gtsam::VectorEvaluationFactor - VectorEvaluationFactorChebyshev2D3; -typedef gtsam::VectorEvaluationFactor - VectorEvaluationFactorChebyshev2D4; -typedef gtsam::VectorEvaluationFactor - VectorEvaluationFactorChebyshev2D12; - -template +template virtual class VectorComponentFactor : gtsam::NoiseModelFactor { VectorComponentFactor(); VectorComponentFactor(gtsam::Key key, const double z, - const gtsam::noiseModel::Base* model, const size_t N, - size_t i, double x); + const gtsam::noiseModel::Base* model, const size_t M, + const size_t N, size_t i, double x); VectorComponentFactor(gtsam::Key key, const double z, - const gtsam::noiseModel::Base* model, const size_t N, - size_t i, double x, double a, double b); + const gtsam::noiseModel::Base* model, const size_t M, + const size_t N, size_t i, double x, double a, double b); }; -typedef gtsam::VectorComponentFactor - VectorComponentFactorChebyshev2D3; -typedef gtsam::VectorComponentFactor - VectorComponentFactorChebyshev2D4; -typedef gtsam::VectorComponentFactor - VectorComponentFactorChebyshev2D12; +#include +#include -template +template virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor { ManifoldEvaluationFactor(); ManifoldEvaluationFactor(gtsam::Key key, const T& z, @@ -121,8 +100,42 @@ virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor { double x, double a, double b); }; -// TODO(gerry): Add `DerivativeFactor`, `VectorDerivativeFactor`, and -// `ComponentDerivativeFactor` +template +virtual class DerivativeFactor : gtsam::NoiseModelFactor { + DerivativeFactor(); + DerivativeFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + DerivativeFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + +template +virtual class VectorDerivativeFactor : gtsam::NoiseModelFactor { + VectorDerivativeFactor(); + VectorDerivativeFactor(gtsam::Key key, const Vector& z, + const gtsam::noiseModel::Base* model, const size_t M, + const size_t N, double x); + VectorDerivativeFactor(gtsam::Key key, const Vector& z, + const gtsam::noiseModel::Base* model, const size_t M, + const size_t N, double x, double a, double b); +}; + +template +virtual class ComponentDerivativeFactor : gtsam::NoiseModelFactor { + ComponentDerivativeFactor(); + ComponentDerivativeFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, + const size_t P, const size_t N, size_t i, double x); + ComponentDerivativeFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, + const size_t P, const size_t N, size_t i, double x, + double a, double b); +}; #include template +#include +#include +#include #include #include #include #include +#include #include #include #include -#include -#include -#include -#include -#include - -using gtsam::noiseModel::Isotropic; -using gtsam::Pose2; -using gtsam::Vector; -using gtsam::Values; using gtsam::Chebyshev2; -using gtsam::ParameterMatrix; -using gtsam::LevenbergMarquardtParams; using gtsam::LevenbergMarquardtOptimizer; +using gtsam::LevenbergMarquardtParams; using gtsam::NonlinearFactorGraph; using gtsam::NonlinearOptimizerParams; +using gtsam::Pose2; +using gtsam::Values; +using gtsam::Vector; +using gtsam::noiseModel::Isotropic; constexpr size_t N = 2; @@ -81,15 +79,15 @@ TEST(BasisFactors, VectorEvaluationFactor) { const Vector measured = Vector::Zero(M); auto model = Isotropic::Sigma(M, 1.0); - VectorEvaluationFactor factor(key, measured, model, N, 0); + VectorEvaluationFactor factor(key, measured, model, M, N, 0); NonlinearFactorGraph graph; graph.add(factor); - ParameterMatrix stateMatrix(N); + gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N); Values initial; - initial.insert>(key, stateMatrix); + initial.insert(key, stateMatrix); LevenbergMarquardtParams parameters; parameters.setMaxIterations(20); @@ -107,7 +105,7 @@ TEST(BasisFactors, Print) { const Vector measured = Vector::Ones(M) * 42; auto model = Isotropic::Sigma(M, 1.0); - VectorEvaluationFactor factor(key, measured, model, N, 0); + VectorEvaluationFactor factor(key, measured, model, M, N, 0); std::string expected = " keys = { X0 }\n" @@ -128,16 +126,16 @@ TEST(BasisFactors, VectorComponentFactor) { const size_t i = 2; const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0; auto model = Isotropic::Sigma(1, 1.0); - VectorComponentFactor factor(key, measured, model, N, i, - t, a, b); + VectorComponentFactor factor(key, measured, model, P, N, i, t, a, + b); NonlinearFactorGraph graph; graph.add(factor); - ParameterMatrix

stateMatrix(N); + gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(P, N); Values initial; - initial.insert>(key, stateMatrix); + initial.insert(key, stateMatrix); LevenbergMarquardtParams parameters; parameters.setMaxIterations(20); @@ -153,16 +151,16 @@ TEST(BasisFactors, ManifoldEvaluationFactor) { const Pose2 measured; const double t = 3.0, a = 2.0, b = 4.0; auto model = Isotropic::Sigma(3, 1.0); - ManifoldEvaluationFactor factor(key, measured, model, N, - t, a, b); + ManifoldEvaluationFactor factor(key, measured, model, N, t, + a, b); NonlinearFactorGraph graph; graph.add(factor); - ParameterMatrix<3> stateMatrix(N); + gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(3, N); Values initial; - initial.insert>(key, stateMatrix); + initial.insert(key, stateMatrix); LevenbergMarquardtParams parameters; parameters.setMaxIterations(20); @@ -170,6 +168,8 @@ TEST(BasisFactors, ManifoldEvaluationFactor) { LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); + // Check Jacobians + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, initial, 1e-7, 1e-5); } //****************************************************************************** @@ -179,15 +179,15 @@ TEST(BasisFactors, VecDerivativePrior) { const Vector measured = Vector::Zero(M); auto model = Isotropic::Sigma(M, 1.0); - VectorDerivativeFactor vecDPrior(key, measured, model, N, 0); + VectorDerivativeFactor vecDPrior(key, measured, model, M, N, 0); NonlinearFactorGraph graph; graph.add(vecDPrior); - ParameterMatrix stateMatrix(N); + gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N); Values initial; - initial.insert>(key, stateMatrix); + initial.insert(key, stateMatrix); LevenbergMarquardtParams parameters; parameters.setMaxIterations(20); @@ -204,15 +204,15 @@ TEST(BasisFactors, ComponentDerivativeFactor) { double measured = 0; auto model = Isotropic::Sigma(1, 1.0); - ComponentDerivativeFactor controlDPrior(key, measured, model, - N, 0, 0); + ComponentDerivativeFactor controlDPrior(key, measured, model, M, + N, 0, 0); NonlinearFactorGraph graph; graph.add(controlDPrior); Values initial; - ParameterMatrix stateMatrix(N); - initial.insert>(key, stateMatrix); + gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N); + initial.insert(key, stateMatrix); LevenbergMarquardtParams parameters; parameters.setMaxIterations(20); diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index 56a79a38a..780714936 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -17,14 +17,15 @@ * methods */ -#include +#include +#include #include #include #include +#include #include -#include -#include +#include #include using namespace std; @@ -107,7 +108,7 @@ TEST(Chebyshev2, InterpolateVector) { double t = 30, a = 0, b = 100; const size_t N = 3; // Create 2x3 matrix with Vectors at Chebyshev points - ParameterMatrix<2> X(N); + Matrix X = Matrix::Zero(2, N); X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp // Check value @@ -115,14 +116,15 @@ TEST(Chebyshev2, InterpolateVector) { expected << t, 0; Eigen::Matrix actualH(2, 2 * N); - Chebyshev2::VectorEvaluationFunctor<2> fx(N, t, a, b); + Chebyshev2::VectorEvaluationFunctor fx(2, N, t, a, b); EXPECT(assert_equal(expected, fx(X, actualH), 1e-9)); // Check derivative - std::function)> f = std::bind( - &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, std::placeholders::_1, nullptr); + std::function f = + std::bind(&Chebyshev2::VectorEvaluationFunctor::operator(), fx, + std::placeholders::_1, nullptr); Matrix numericalH = - numericalDerivative11, 2 * N>(f, X); + numericalDerivative11(f, X); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } @@ -131,25 +133,66 @@ TEST(Chebyshev2, InterpolateVector) { TEST(Chebyshev2, InterpolatePose2) { double t = 30, a = 0, b = 100; - ParameterMatrix<3> X(N); + Matrix X(3, N); X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp X.row(1) = Vector::Zero(N); X.row(2) = 0.1 * Vector::Ones(N); Vector xi(3); xi << t, 0, 0.1; + Eigen::Matrix actualH(3, 3 * N); + Chebyshev2::ManifoldEvaluationFunctor fx(N, t, a, b); // We use xi as canonical coordinates via exponential map Pose2 expected = Pose2::ChartAtOrigin::Retract(xi); - EXPECT(assert_equal(expected, fx(X))); + EXPECT(assert_equal(expected, fx(X, actualH))); + + // Check derivative + std::function f = + std::bind(&Chebyshev2::ManifoldEvaluationFunctor::operator(), fx, + std::placeholders::_1, nullptr); + Matrix numericalH = + numericalDerivative11(f, X); + EXPECT(assert_equal(numericalH, actualH, 1e-9)); } +#ifdef GTSAM_POSE3_EXPMAP +//****************************************************************************** +// Interpolating poses using the exponential map +TEST(Chebyshev2, InterpolatePose3) { + double a = 10, b = 100; + double t = Chebyshev2::Points(N, a, b)(11); + + Rot3 R = Rot3::Ypr(-2.21366492e-05, -9.35353636e-03, -5.90463598e-04); + Pose3 pose(R, Point3(1, 2, 3)); + + Vector6 xi = Pose3::ChartAtOrigin::Local(pose); + Eigen::Matrix actualH(6, 6 * N); + + Matrix X = Matrix::Zero(6, N); + X.col(11) = xi; + + Chebyshev2::ManifoldEvaluationFunctor fx(N, t, a, b); + // We use xi as canonical coordinates via exponential map + Pose3 expected = Pose3::ChartAtOrigin::Retract(xi); + EXPECT(assert_equal(expected, fx(X, actualH))); + + // Check derivative + std::function f = + std::bind(&Chebyshev2::ManifoldEvaluationFunctor::operator(), fx, + std::placeholders::_1, nullptr); + Matrix numericalH = + numericalDerivative11(f, X); + EXPECT(assert_equal(numericalH, actualH, 1e-8)); +} +#endif + //****************************************************************************** TEST(Chebyshev2, Decomposition) { // Create example sequence Sequence sequence; for (size_t i = 0; i < 16; i++) { - double x = (1.0/ 16)*i - 0.99, y = x; + double x = (1.0 / 16) * i - 0.99, y = x; sequence[x] = y; } @@ -370,14 +413,14 @@ TEST(Chebyshev2, Derivative6_03) { TEST(Chebyshev2, VectorDerivativeFunctor) { const size_t N = 3, M = 2; const double x = 0.2; - using VecD = Chebyshev2::VectorDerivativeFunctor; - VecD fx(N, x, 0, 3); - ParameterMatrix X(N); + using VecD = Chebyshev2::VectorDerivativeFunctor; + VecD fx(M, N, x, 0, 3); + Matrix X = Matrix::Zero(M, N); Matrix actualH(M, M * N); EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8)); // Test Jacobian - Matrix expectedH = numericalDerivative11, M * N>( + Matrix expectedH = numericalDerivative11( std::bind(&VecD::operator(), fx, std::placeholders::_1, nullptr), X); EXPECT(assert_equal(expectedH, actualH, 1e-7)); } @@ -386,30 +429,29 @@ TEST(Chebyshev2, VectorDerivativeFunctor) { // Test VectorDerivativeFunctor with polynomial function TEST(Chebyshev2, VectorDerivativeFunctor2) { const size_t N = 64, M = 1, T = 15; - using VecD = Chebyshev2::VectorDerivativeFunctor; + using VecD = Chebyshev2::VectorDerivativeFunctor; const Vector points = Chebyshev2::Points(N, 0, T); - // Assign the parameter matrix - Vector values(N); + // Assign the parameter matrix 1xN + Matrix X(1, N); for (size_t i = 0; i < N; ++i) { - values(i) = f(points(i)); + X(i) = f(points(i)); } - ParameterMatrix X(values); // Evaluate the derivative at the chebyshev points using // VectorDerivativeFunctor. for (size_t i = 0; i < N; ++i) { - VecD d(N, points(i), 0, T); + VecD d(M, N, points(i), 0, T); Vector1 Dx = d(X); EXPECT_DOUBLES_EQUAL(fprime(points(i)), Dx(0), 1e-6); } // Test Jacobian at the first chebyshev point. Matrix actualH(M, M * N); - VecD vecd(N, points(0), 0, T); + VecD vecd(M, N, points(0), 0, T); vecd(X, actualH); - Matrix expectedH = numericalDerivative11, M * N>( + Matrix expectedH = numericalDerivative11( std::bind(&VecD::operator(), vecd, std::placeholders::_1, nullptr), X); EXPECT(assert_equal(expectedH, actualH, 1e-6)); } @@ -419,14 +461,14 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) { TEST(Chebyshev2, ComponentDerivativeFunctor) { const size_t N = 6, M = 2; const double x = 0.2; - using CompFunc = Chebyshev2::ComponentDerivativeFunctor; + using CompFunc = Chebyshev2::ComponentDerivativeFunctor; size_t row = 1; - CompFunc fx(N, row, x, 0, 3); - ParameterMatrix X(N); + CompFunc fx(M, N, row, x, 0, 3); + Matrix X = Matrix::Zero(M, N); Matrix actualH(1, M * N); EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8); - Matrix expectedH = numericalDerivative11, M * N>( + Matrix expectedH = numericalDerivative11( std::bind(&CompFunc::operator(), fx, std::placeholders::_1, nullptr), X); EXPECT(assert_equal(expectedH, actualH, 1e-7)); } diff --git a/gtsam/basis/tests/testFourier.cpp b/gtsam/basis/tests/testFourier.cpp index 3a147a9f6..2995e8c45 100644 --- a/gtsam/basis/tests/testFourier.cpp +++ b/gtsam/basis/tests/testFourier.cpp @@ -180,17 +180,16 @@ TEST(Basis, Derivative7) { //****************************************************************************** TEST(Basis, VecDerivativeFunctor) { - using DotShape = typename FourierBasis::VectorDerivativeFunctor<2>; + using DotShape = typename FourierBasis::VectorDerivativeFunctor; const size_t N = 3; // MATLAB example, Dec 27 2019, commit 014eded5 double h = 2 * M_PI / 16; Vector2 dotShape(0.5556, -0.8315); // at h/2 - DotShape dotShapeFunction(N, h / 2); - Matrix23 theta_mat = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071) - .finished() - .transpose(); - ParameterMatrix<2> theta(theta_mat); + DotShape dotShapeFunction(2, N, h / 2); + Matrix theta = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071) + .finished() + .transpose(); EXPECT(assert_equal(Vector(dotShape), dotShapeFunction(theta), 1e-4)); } diff --git a/gtsam/basis/tests/testParameterMatrix.cpp b/gtsam/basis/tests/testParameterMatrix.cpp deleted file mode 100644 index ae2e8e111..000000000 --- a/gtsam/basis/tests/testParameterMatrix.cpp +++ /dev/null @@ -1,145 +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 - - * -------------------------------------------------------------------------- */ - -/** - * @file testParameterMatrix.cpp - * @date Sep 22, 2020 - * @author Varun Agrawal, Frank Dellaert - * @brief Unit tests for ParameterMatrix.h - */ - -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -using Parameters = Chebyshev2::Parameters; - -const size_t M = 2, N = 5; - -//****************************************************************************** -TEST(ParameterMatrix, Constructor) { - ParameterMatrix<2> actual1(3); - ParameterMatrix<2> expected1(Matrix::Zero(2, 3)); - EXPECT(assert_equal(expected1, actual1)); - - ParameterMatrix<2> actual2(Matrix::Ones(2, 3)); - ParameterMatrix<2> expected2(Matrix::Ones(2, 3)); - EXPECT(assert_equal(expected2, actual2)); - EXPECT(assert_equal(expected2.matrix(), actual2.matrix())); -} - -//****************************************************************************** -TEST(ParameterMatrix, Dimensions) { - ParameterMatrix params(N); - EXPECT_LONGS_EQUAL(params.rows(), M); - EXPECT_LONGS_EQUAL(params.cols(), N); - EXPECT_LONGS_EQUAL(params.dim(), M * N); -} - -//****************************************************************************** -TEST(ParameterMatrix, Getters) { - ParameterMatrix params(N); - - Matrix expectedMatrix = Matrix::Zero(2, 5); - EXPECT(assert_equal(expectedMatrix, params.matrix())); - - Matrix expectedMatrixTranspose = Matrix::Zero(5, 2); - EXPECT(assert_equal(expectedMatrixTranspose, params.transpose())); - - ParameterMatrix p2(Matrix::Ones(M, N)); - Vector expectedRowVector = Vector::Ones(N); - for (size_t r = 0; r < M; ++r) { - EXPECT(assert_equal(p2.row(r), expectedRowVector)); - } - - ParameterMatrix p3(2 * Matrix::Ones(M, N)); - Vector expectedColVector = 2 * Vector::Ones(M); - for (size_t c = 0; c < M; ++c) { - EXPECT(assert_equal(p3.col(c), expectedColVector)); - } -} - -//****************************************************************************** -TEST(ParameterMatrix, Setters) { - ParameterMatrix params(Matrix::Zero(M, N)); - Matrix expected = Matrix::Zero(M, N); - - // row - params.row(0) = Vector::Ones(N); - expected.row(0) = Vector::Ones(N); - EXPECT(assert_equal(expected, params.matrix())); - - // col - params.col(2) = Vector::Ones(M); - expected.col(2) = Vector::Ones(M); - - EXPECT(assert_equal(expected, params.matrix())); - - // setZero - params.setZero(); - expected.setZero(); - EXPECT(assert_equal(expected, params.matrix())); -} - -//****************************************************************************** -TEST(ParameterMatrix, Addition) { - ParameterMatrix params(Matrix::Ones(M, N)); - ParameterMatrix expected(2 * Matrix::Ones(M, N)); - - // Add vector - EXPECT(assert_equal(expected, params + Vector::Ones(M * N))); - // Add another ParameterMatrix - ParameterMatrix actual = params + ParameterMatrix(Matrix::Ones(M, N)); - EXPECT(assert_equal(expected, actual)); -} - -//****************************************************************************** -TEST(ParameterMatrix, Subtraction) { - ParameterMatrix params(2 * Matrix::Ones(M, N)); - ParameterMatrix expected(Matrix::Ones(M, N)); - - // Subtract vector - EXPECT(assert_equal(expected, params - Vector::Ones(M * N))); - // Subtract another ParameterMatrix - ParameterMatrix actual = params - ParameterMatrix(Matrix::Ones(M, N)); - EXPECT(assert_equal(expected, actual)); -} - -//****************************************************************************** -TEST(ParameterMatrix, Multiplication) { - ParameterMatrix params(Matrix::Ones(M, N)); - Matrix multiplier = 2 * Matrix::Ones(N, 2); - Matrix expected = 2 * N * Matrix::Ones(M, 2); - EXPECT(assert_equal(expected, params * multiplier)); -} - -//****************************************************************************** -TEST(ParameterMatrix, VectorSpace) { - ParameterMatrix params(Matrix::Ones(M, N)); - // vector - EXPECT(assert_equal(Vector::Ones(M * N), params.vector())); - // identity - EXPECT(assert_equal(ParameterMatrix::Identity(), - ParameterMatrix(Matrix::Zero(M, 0)))); -} - -//****************************************************************************** -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -//****************************************************************************** diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 7f8936d1e..ec06d90c9 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -83,3 +83,5 @@ // Toggle switch for BetweenFactor jacobian computation #cmakedefine GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR + +#cmakedefine GTSAM_SLOW_BUT_CORRECT_EXPMAP diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp new file mode 100644 index 000000000..5fe3cd9d1 --- /dev/null +++ b/gtsam/discrete/TableFactor.cpp @@ -0,0 +1,553 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file TableFactor.cpp + * @brief discrete factor + * @date May 4, 2023 + * @author Yoonwoo Kim + */ + +#include +#include +#include +#include + +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************ */ +TableFactor::TableFactor() {} + +/* ************************************************************************ */ +TableFactor::TableFactor(const DiscreteKeys& dkeys, + const TableFactor& potentials) + : DiscreteFactor(dkeys.indices()), + cardinalities_(potentials.cardinalities_) { + sparse_table_ = potentials.sparse_table_; + denominators_ = potentials.denominators_; + sorted_dkeys_ = discreteKeys(); + sort(sorted_dkeys_.begin(), sorted_dkeys_.end()); +} + +/* ************************************************************************ */ +TableFactor::TableFactor(const DiscreteKeys& dkeys, + const Eigen::SparseVector& table) + : DiscreteFactor(dkeys.indices()), sparse_table_(table.size()) { + sparse_table_ = table; + double denom = table.size(); + for (const DiscreteKey& dkey : dkeys) { + cardinalities_.insert(dkey); + denom /= dkey.second; + denominators_.insert(std::pair(dkey.first, denom)); + } + sorted_dkeys_ = discreteKeys(); + sort(sorted_dkeys_.begin(), sorted_dkeys_.end()); +} + +/* ************************************************************************ */ +Eigen::SparseVector TableFactor::Convert( + const std::vector& table) { + Eigen::SparseVector sparse_table(table.size()); + // Count number of nonzero elements in table and reserving the space. + const uint64_t nnz = std::count_if(table.begin(), table.end(), + [](uint64_t i) { return i != 0; }); + sparse_table.reserve(nnz); + for (uint64_t i = 0; i < table.size(); i++) { + if (table[i] != 0) sparse_table.insert(i) = table[i]; + } + sparse_table.pruned(); + sparse_table.data().squeeze(); + return sparse_table; +} + +/* ************************************************************************ */ +Eigen::SparseVector TableFactor::Convert(const std::string& table) { + // Convert string to doubles. + std::vector ys; + std::istringstream iss(table); + std::copy(std::istream_iterator(iss), std::istream_iterator(), + std::back_inserter(ys)); + return Convert(ys); +} + +/* ************************************************************************ */ +bool TableFactor::equals(const DiscreteFactor& other, double tol) const { + if (!dynamic_cast(&other)) { + return false; + } else { + const auto& f(static_cast(other)); + return sparse_table_.isApprox(f.sparse_table_, tol); + } +} + +/* ************************************************************************ */ +double TableFactor::operator()(const DiscreteValues& values) const { + // a b c d => D * (C * (B * (a) + b) + c) + d + uint64_t idx = 0, card = 1; + for (auto it = sorted_dkeys_.rbegin(); it != sorted_dkeys_.rend(); ++it) { + if (values.find(it->first) != values.end()) { + idx += card * values.at(it->first); + } + card *= it->second; + } + return sparse_table_.coeff(idx); +} + +/* ************************************************************************ */ +double TableFactor::findValue(const DiscreteValues& values) const { + // a b c d => D * (C * (B * (a) + b) + c) + d + uint64_t idx = 0, card = 1; + for (auto it = keys_.rbegin(); it != keys_.rend(); ++it) { + if (values.find(*it) != values.end()) { + idx += card * values.at(*it); + } + card *= cardinality(*it); + } + return sparse_table_.coeff(idx); +} + +/* ************************************************************************ */ +double TableFactor::error(const DiscreteValues& values) const { + return -log(evaluate(values)); +} + +/* ************************************************************************ */ +double TableFactor::error(const HybridValues& values) const { + return error(values.discrete()); +} + +/* ************************************************************************ */ +DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const { + return toDecisionTreeFactor() * f; +} + +/* ************************************************************************ */ +DecisionTreeFactor TableFactor::toDecisionTreeFactor() const { + DiscreteKeys dkeys = discreteKeys(); + std::vector table; + for (auto i = 0; i < sparse_table_.size(); i++) { + table.push_back(sparse_table_.coeff(i)); + } + DecisionTreeFactor f(dkeys, table); + return f; +} + +/* ************************************************************************ */ +TableFactor TableFactor::choose(const DiscreteValues parent_assign, + DiscreteKeys parent_keys) const { + if (parent_keys.empty()) return *this; + + // Unique representation of parent values. + uint64_t unique = 0; + uint64_t card = 1; + for (auto it = keys_.rbegin(); it != keys_.rend(); ++it) { + if (parent_assign.find(*it) != parent_assign.end()) { + unique += parent_assign.at(*it) * card; + card *= cardinality(*it); + } + } + + // Find child DiscreteKeys + DiscreteKeys child_dkeys; + std::sort(parent_keys.begin(), parent_keys.end()); + std::set_difference(sorted_dkeys_.begin(), sorted_dkeys_.end(), + parent_keys.begin(), parent_keys.end(), + std::back_inserter(child_dkeys)); + + // Create child sparse table to populate. + uint64_t child_card = 1; + for (const DiscreteKey& child_dkey : child_dkeys) + child_card *= child_dkey.second; + Eigen::SparseVector child_sparse_table_(child_card); + child_sparse_table_.reserve(child_card); + + // Populate child sparse table. + for (SparseIt it(sparse_table_); it; ++it) { + // Create unique representation of parent keys + uint64_t parent_unique = uniqueRep(parent_keys, it.index()); + // Populate the table + if (parent_unique == unique) { + uint64_t idx = uniqueRep(child_dkeys, it.index()); + child_sparse_table_.insert(idx) = it.value(); + } + } + + child_sparse_table_.pruned(); + child_sparse_table_.data().squeeze(); + return TableFactor(child_dkeys, child_sparse_table_); +} + +/* ************************************************************************ */ +double TableFactor::safe_div(const double& a, const double& b) { + // The use for safe_div is when we divide the product factor by the sum + // factor. If the product or sum is zero, we accord zero probability to the + // event. + return (a == 0 || b == 0) ? 0 : (a / b); +} + +/* ************************************************************************ */ +void TableFactor::print(const string& s, const KeyFormatter& formatter) const { + cout << s; + cout << " f["; + for (auto&& key : keys()) + cout << " (" << formatter(key) << "," << cardinality(key) << "),"; + cout << " ]" << endl; + for (SparseIt it(sparse_table_); it; ++it) { + DiscreteValues assignment = findAssignments(it.index()); + for (auto&& kv : assignment) { + cout << "(" << formatter(kv.first) << ", " << kv.second << ")"; + } + cout << " | " << it.value() << " | " << it.index() << endl; + } + cout << "number of nnzs: " << sparse_table_.nonZeros() << endl; +} + +/* ************************************************************************ */ +TableFactor TableFactor::apply(const TableFactor& f, Binary op) const { + if (keys_.empty() && sparse_table_.nonZeros() == 0) + return f; + else if (f.keys_.empty() && f.sparse_table_.nonZeros() == 0) + return *this; + // 1. Identify keys for contract and free modes. + DiscreteKeys contract_dkeys = contractDkeys(f); + DiscreteKeys f_free_dkeys = f.freeDkeys(*this); + DiscreteKeys union_dkeys = unionDkeys(f); + // 2. Create hash table for input factor f + unordered_map map_f = + f.createMap(contract_dkeys, f_free_dkeys); + // 3. Initialize multiplied factor. + uint64_t card = 1; + for (auto u_dkey : union_dkeys) card *= u_dkey.second; + Eigen::SparseVector mult_sparse_table(card); + mult_sparse_table.reserve(card); + // 3. Multiply. + for (SparseIt it(sparse_table_); it; ++it) { + uint64_t contract_unique = uniqueRep(contract_dkeys, it.index()); + if (map_f.find(contract_unique) == map_f.end()) continue; + for (auto assignVal : map_f[contract_unique]) { + uint64_t union_idx = unionRep(union_dkeys, assignVal.first, it.index()); + mult_sparse_table.insert(union_idx) = op(it.value(), assignVal.second); + } + } + // 4. Free unused memory. + mult_sparse_table.pruned(); + mult_sparse_table.data().squeeze(); + // 5. Create union keys and return. + return TableFactor(union_dkeys, mult_sparse_table); +} + +/* ************************************************************************ */ +DiscreteKeys TableFactor::contractDkeys(const TableFactor& f) const { + // Find contract modes. + DiscreteKeys contract; + set_intersection(sorted_dkeys_.begin(), sorted_dkeys_.end(), + f.sorted_dkeys_.begin(), f.sorted_dkeys_.end(), + back_inserter(contract)); + return contract; +} + +/* ************************************************************************ */ +DiscreteKeys TableFactor::freeDkeys(const TableFactor& f) const { + // Find free modes. + DiscreteKeys free; + set_difference(sorted_dkeys_.begin(), sorted_dkeys_.end(), + f.sorted_dkeys_.begin(), f.sorted_dkeys_.end(), + back_inserter(free)); + return free; +} + +/* ************************************************************************ */ +DiscreteKeys TableFactor::unionDkeys(const TableFactor& f) const { + // Find union modes. + DiscreteKeys union_dkeys; + set_union(sorted_dkeys_.begin(), sorted_dkeys_.end(), f.sorted_dkeys_.begin(), + f.sorted_dkeys_.end(), back_inserter(union_dkeys)); + return union_dkeys; +} + +/* ************************************************************************ */ +uint64_t TableFactor::unionRep(const DiscreteKeys& union_keys, + const DiscreteValues& f_free, + const uint64_t idx) const { + uint64_t union_idx = 0, card = 1; + for (auto it = union_keys.rbegin(); it != union_keys.rend(); it++) { + if (f_free.find(it->first) == f_free.end()) { + union_idx += keyValueForIndex(it->first, idx) * card; + } else { + union_idx += f_free.at(it->first) * card; + } + card *= it->second; + } + return union_idx; +} + +/* ************************************************************************ */ +unordered_map TableFactor::createMap( + const DiscreteKeys& contract, const DiscreteKeys& free) const { + // 1. Initialize map. + unordered_map map_f; + // 2. Iterate over nonzero elements. + for (SparseIt it(sparse_table_); it; ++it) { + // 3. Create unique representation of contract modes. + uint64_t unique_rep = uniqueRep(contract, it.index()); + // 4. Create assignment for free modes. + DiscreteValues free_assignments; + for (auto& key : free) + free_assignments[key.first] = keyValueForIndex(key.first, it.index()); + // 5. Populate map. + if (map_f.find(unique_rep) == map_f.end()) { + map_f[unique_rep] = {make_pair(free_assignments, it.value())}; + } else { + map_f[unique_rep].push_back(make_pair(free_assignments, it.value())); + } + } + return map_f; +} + +/* ************************************************************************ */ +uint64_t TableFactor::uniqueRep(const DiscreteKeys& dkeys, + const uint64_t idx) const { + if (dkeys.empty()) return 0; + uint64_t unique_rep = 0, card = 1; + for (auto it = dkeys.rbegin(); it != dkeys.rend(); it++) { + unique_rep += keyValueForIndex(it->first, idx) * card; + card *= it->second; + } + return unique_rep; +} + +/* ************************************************************************ */ +uint64_t TableFactor::uniqueRep(const DiscreteValues& assignments) const { + if (assignments.empty()) return 0; + uint64_t unique_rep = 0, card = 1; + for (auto it = assignments.rbegin(); it != assignments.rend(); it++) { + unique_rep += it->second * card; + card *= cardinalities_.at(it->first); + } + return unique_rep; +} + +/* ************************************************************************ */ +DiscreteValues TableFactor::findAssignments(const uint64_t idx) const { + DiscreteValues assignment; + for (Key key : keys_) { + assignment[key] = keyValueForIndex(key, idx); + } + return assignment; +} + +/* ************************************************************************ */ +TableFactor::shared_ptr TableFactor::combine(size_t nrFrontals, + Binary op) const { + if (nrFrontals > size()) { + throw invalid_argument( + "TableFactor::combine: invalid number of frontal " + "keys " + + to_string(nrFrontals) + ", nr.keys=" + std::to_string(size())); + } + // Find remaining keys. + DiscreteKeys remain_dkeys; + uint64_t card = 1; + for (auto i = nrFrontals; i < keys_.size(); i++) { + remain_dkeys.push_back(discreteKey(i)); + card *= cardinality(keys_[i]); + } + // Create combined table. + Eigen::SparseVector combined_table(card); + combined_table.reserve(sparse_table_.nonZeros()); + // Populate combined table. + for (SparseIt it(sparse_table_); it; ++it) { + uint64_t idx = uniqueRep(remain_dkeys, it.index()); + double new_val = op(combined_table.coeff(idx), it.value()); + combined_table.coeffRef(idx) = new_val; + } + // Free unused memory. + combined_table.pruned(); + combined_table.data().squeeze(); + return std::make_shared(remain_dkeys, combined_table); +} + +/* ************************************************************************ */ +TableFactor::shared_ptr TableFactor::combine(const Ordering& frontalKeys, + Binary op) const { + if (frontalKeys.size() > size()) { + throw invalid_argument( + "TableFactor::combine: invalid number of frontal " + "keys " + + std::to_string(frontalKeys.size()) + + ", nr.keys=" + std::to_string(size())); + } + // Find remaining keys. + DiscreteKeys remain_dkeys; + uint64_t card = 1; + for (Key key : keys_) { + if (std::find(frontalKeys.begin(), frontalKeys.end(), key) == + frontalKeys.end()) { + remain_dkeys.emplace_back(key, cardinality(key)); + card *= cardinality(key); + } + } + // Create combined table. + Eigen::SparseVector combined_table(card); + combined_table.reserve(sparse_table_.nonZeros()); + // Populate combined table. + for (SparseIt it(sparse_table_); it; ++it) { + uint64_t idx = uniqueRep(remain_dkeys, it.index()); + double new_val = op(combined_table.coeff(idx), it.value()); + combined_table.coeffRef(idx) = new_val; + } + // Free unused memory. + combined_table.pruned(); + combined_table.data().squeeze(); + return std::make_shared(remain_dkeys, combined_table); +} + +/* ************************************************************************ */ +size_t TableFactor::keyValueForIndex(Key target_key, uint64_t index) const { + // http://phrogz.net/lazy-cartesian-product + return (index / denominators_.at(target_key)) % cardinality(target_key); +} + +/* ************************************************************************ */ +std::vector> TableFactor::enumerate() const { + // Get all possible assignments + std::vector> pairs = discreteKeys(); + // Reverse to make cartesian product output a more natural ordering. + std::vector> rpairs(pairs.rbegin(), pairs.rend()); + const auto assignments = DiscreteValues::CartesianProduct(rpairs); + // Construct unordered_map with values + std::vector> result; + for (const auto& assignment : assignments) { + result.emplace_back(assignment, operator()(assignment)); + } + return result; +} + +/* ************************************************************************ */ +DiscreteKeys TableFactor::discreteKeys() const { + DiscreteKeys result; + for (auto&& key : keys()) { + DiscreteKey dkey(key, cardinality(key)); + if (std::find(result.begin(), result.end(), dkey) == result.end()) { + result.push_back(dkey); + } + } + return result; +} + +// Print out header. +/* ************************************************************************ */ +string TableFactor::markdown(const KeyFormatter& keyFormatter, + const Names& names) const { + stringstream ss; + + // Print out header. + ss << "|"; + for (auto& key : keys()) { + ss << keyFormatter(key) << "|"; + } + ss << "value|\n"; + + // Print out separator with alignment hints. + ss << "|"; + for (size_t j = 0; j < size(); j++) ss << ":-:|"; + ss << ":-:|\n"; + + // Print out all rows. + for (SparseIt it(sparse_table_); it; ++it) { + DiscreteValues assignment = findAssignments(it.index()); + ss << "|"; + for (auto& key : keys()) { + size_t index = assignment.at(key); + ss << DiscreteValues::Translate(names, key, index) << "|"; + } + ss << it.value() << "|\n"; + } + return ss.str(); +} + +/* ************************************************************************ */ +string TableFactor::html(const KeyFormatter& keyFormatter, + const Names& names) const { + stringstream ss; + + // Print out preamble. + ss << "

\n\n \n"; + + // Print out header row. + ss << " "; + for (auto& key : keys()) { + ss << ""; + } + ss << "\n"; + + // Finish header and start body. + ss << " \n \n"; + + // Print out all rows. + for (SparseIt it(sparse_table_); it; ++it) { + DiscreteValues assignment = findAssignments(it.index()); + ss << " "; + for (auto& key : keys()) { + size_t index = assignment.at(key); + ss << ""; + } + ss << ""; // value + ss << "\n"; + } + ss << " \n
" << keyFormatter(key) << "value
" << DiscreteValues::Translate(names, key, index) << "" << it.value() << "
\n
"; + return ss.str(); +} + +/* ************************************************************************ */ +TableFactor TableFactor::prune(size_t maxNrAssignments) const { + const size_t N = maxNrAssignments; + + // Get the probabilities in the TableFactor so we can threshold. + vector> probabilities; + + // Store non-zero probabilities along with their indices in a vector. + for (SparseIt it(sparse_table_); it; ++it) { + probabilities.emplace_back(it.index(), it.value()); + } + + // The number of probabilities can be lower than max_leaves. + if (probabilities.size() <= N) return *this; + + // Sort the vector in descending order based on the element values. + sort(probabilities.begin(), probabilities.end(), + [](const std::pair& a, + const std::pair& b) { + return a.second > b.second; + }); + + // Keep the largest N probabilities in the vector. + if (probabilities.size() > N) probabilities.resize(N); + + // Create pruned sparse vector. + Eigen::SparseVector pruned_vec(sparse_table_.size()); + pruned_vec.reserve(probabilities.size()); + + // Populate pruned sparse vector. + for (const auto& prob : probabilities) { + pruned_vec.insert(prob.first) = prob.second; + } + + // Create pruned decision tree factor and return. + return TableFactor(this->discreteKeys(), pruned_vec); +} + +/* ************************************************************************ */ +} // namespace gtsam diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h new file mode 100644 index 000000000..1462180e0 --- /dev/null +++ b/gtsam/discrete/TableFactor.h @@ -0,0 +1,340 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file TableFactor.h + * @date May 4, 2023 + * @author Yoonwoo Kim + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +class HybridValues; + +/** + * A discrete probabilistic factor optimized for sparsity. + * Uses sparse_table_ to store only the nonzero probabilities. + * Computes the assigned value for the key using the ordering which the + * nonzero probabilties are stored in. (lazy cartesian product) + * + * @ingroup discrete + */ +class GTSAM_EXPORT TableFactor : public DiscreteFactor { + protected: + /// Map of Keys and their cardinalities. + std::map cardinalities_; + /// SparseVector of nonzero probabilities. + Eigen::SparseVector sparse_table_; + + private: + /// Map of Keys and their denominators used in keyValueForIndex. + std::map denominators_; + /// Sorted DiscreteKeys to use internally. + DiscreteKeys sorted_dkeys_; + + /** + * @brief Uses lazy cartesian product to find nth entry in the cartesian + * product of arrays in O(1) + * Example) + * v0 | v1 | val + * 0 | 0 | 10 + * 0 | 1 | 21 + * 1 | 0 | 32 + * 1 | 1 | 43 + * keyValueForIndex(v1, 2) = 0 + * @param target_key nth entry's key to find out its assigned value + * @param index nth entry in the sparse vector + * @return TableFactor + */ + size_t keyValueForIndex(Key target_key, uint64_t index) const; + + /** + * @brief Return ith key in keys_ as a DiscreteKey + * @param i ith key in keys_ + * @return DiscreteKey + * */ + DiscreteKey discreteKey(size_t i) const { + return DiscreteKey(keys_[i], cardinalities_.at(keys_[i])); + } + + /// Convert probability table given as doubles to SparseVector. + /// Example) {0, 1, 1, 0, 0, 1, 0} -> values: {1, 1, 1}, indices: {1, 2, 5} + static Eigen::SparseVector Convert(const std::vector& table); + + /// Convert probability table given as string to SparseVector. + static Eigen::SparseVector Convert(const std::string& table); + + public: + // typedefs needed to play nice with gtsam + typedef TableFactor This; + typedef DiscreteFactor Base; ///< Typedef to base class + typedef std::shared_ptr shared_ptr; + typedef Eigen::SparseVector::InnerIterator SparseIt; + typedef std::vector> AssignValList; + using Binary = std::function; + + public: + /** The Real ring with addition and multiplication */ + struct Ring { + static inline double zero() { return 0.0; } + static inline double one() { return 1.0; } + static inline double add(const double& a, const double& b) { return a + b; } + static inline double max(const double& a, const double& b) { + return std::max(a, b); + } + static inline double mul(const double& a, const double& b) { return a * b; } + static inline double div(const double& a, const double& b) { + return (a == 0 || b == 0) ? 0 : (a / b); + } + static inline double id(const double& x) { return x; } + }; + + /// @name Standard Constructors + /// @{ + + /** Default constructor for I/O */ + TableFactor(); + + /** Constructor from DiscreteKeys and TableFactor */ + TableFactor(const DiscreteKeys& keys, const TableFactor& potentials); + + /** Constructor from sparse_table */ + TableFactor(const DiscreteKeys& keys, + const Eigen::SparseVector& table); + + /** Constructor from doubles */ + TableFactor(const DiscreteKeys& keys, const std::vector& table) + : TableFactor(keys, Convert(table)) {} + + /** Constructor from string */ + TableFactor(const DiscreteKeys& keys, const std::string& table) + : TableFactor(keys, Convert(table)) {} + + /// Single-key specialization + template + TableFactor(const DiscreteKey& key, SOURCE table) + : TableFactor(DiscreteKeys{key}, table) {} + + /// Single-key specialization, with vector of doubles. + TableFactor(const DiscreteKey& key, const std::vector& row) + : TableFactor(DiscreteKeys{key}, row) {} + + /// @} + /// @name Testable + /// @{ + + /// equality + bool equals(const DiscreteFactor& other, double tol = 1e-9) const override; + + // print + void print( + const std::string& s = "TableFactor:\n", + const KeyFormatter& formatter = DefaultKeyFormatter) const override; + + // /// @} + // /// @name Standard Interface + // /// @{ + + /// Calculate probability for given values `x`, + /// is just look up in TableFactor. + double evaluate(const DiscreteValues& values) const { + return operator()(values); + } + + /// Evaluate probability distribution, sugar. + double operator()(const DiscreteValues& values) const override; + + /// Calculate error for DiscreteValues `x`, is -log(probability). + double error(const DiscreteValues& values) const; + + /// multiply two TableFactors + TableFactor operator*(const TableFactor& f) const { + return apply(f, Ring::mul); + }; + + /// multiple with DecisionTreeFactor + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; + + static double safe_div(const double& a, const double& b); + + size_t cardinality(Key j) const { return cardinalities_.at(j); } + + /// divide by factor f (safely) + TableFactor operator/(const TableFactor& f) const { + return apply(f, safe_div); + } + + /// Convert into a decisiontree + DecisionTreeFactor toDecisionTreeFactor() const override; + + /// Create a TableFactor that is a subset of this TableFactor + TableFactor choose(const DiscreteValues assignments, + DiscreteKeys parent_keys) const; + + /// Create new factor by summing all values with the same separator values + shared_ptr sum(size_t nrFrontals) const { + return combine(nrFrontals, Ring::add); + } + + /// Create new factor by summing all values with the same separator values + shared_ptr sum(const Ordering& keys) const { + return combine(keys, Ring::add); + } + + /// Create new factor by maximizing over all values with the same separator. + shared_ptr max(size_t nrFrontals) const { + return combine(nrFrontals, Ring::max); + } + + /// Create new factor by maximizing over all values with the same separator. + shared_ptr max(const Ordering& keys) const { + return combine(keys, Ring::max); + } + + /// @} + /// @name Advanced Interface + /// @{ + + /** + * Apply binary operator (*this) "op" f + * @param f the second argument for op + * @param op a binary operator that operates on TableFactor + */ + TableFactor apply(const TableFactor& f, Binary op) const; + + /// Return keys in contract mode. + DiscreteKeys contractDkeys(const TableFactor& f) const; + + /// Return keys in free mode. + DiscreteKeys freeDkeys(const TableFactor& f) const; + + /// Return union of DiscreteKeys in two factors. + DiscreteKeys unionDkeys(const TableFactor& f) const; + + /// Create unique representation of union modes. + uint64_t unionRep(const DiscreteKeys& keys, const DiscreteValues& assign, + const uint64_t idx) const; + + /// Create a hash map of input factor with assignment of contract modes as + /// keys and vector of hashed assignment of free modes and value as values. + std::unordered_map createMap( + const DiscreteKeys& contract, const DiscreteKeys& free) const; + + /// Create unique representation + uint64_t uniqueRep(const DiscreteKeys& keys, const uint64_t idx) const; + + /// Create unique representation with DiscreteValues + uint64_t uniqueRep(const DiscreteValues& assignments) const; + + /// Find DiscreteValues for corresponding index. + DiscreteValues findAssignments(const uint64_t idx) const; + + /// Find value for corresponding DiscreteValues. + double findValue(const DiscreteValues& values) const; + + /** + * Combine frontal variables using binary operator "op" + * @param nrFrontals nr. of frontal to combine variables in this factor + * @param op a binary operator that operates on TableFactor + * @return shared pointer to newly created TableFactor + */ + shared_ptr combine(size_t nrFrontals, Binary op) const; + + /** + * Combine frontal variables in an Ordering using binary operator "op" + * @param nrFrontals nr. of frontal to combine variables in this factor + * @param op a binary operator that operates on TableFactor + * @return shared pointer to newly created TableFactor + */ + shared_ptr combine(const Ordering& keys, Binary op) const; + + /// Enumerate all values into a map from values to double. + std::vector> enumerate() const; + + /// Return all the discrete keys associated with this factor. + DiscreteKeys discreteKeys() const; + + /** + * @brief Prune the decision tree of discrete variables. + * + * Pruning will set the values to be "pruned" to 0 indicating a 0 + * probability. An assignment is pruned if it is not in the top + * `maxNrAssignments` values. + * + * A violation can occur if there are more + * duplicate values than `maxNrAssignments`. A violation here is the need to + * un-prune the decision tree (e.g. all assignment values are 1.0). We could + * have another case where some subset of duplicates exist (e.g. for a tree + * with 8 assignments we have 1, 1, 1, 1, 0.8, 0.7, 0.6, 0.5), but this is + * not a violation since the for `maxNrAssignments=5` the top values are (1, + * 0.8). + * + * @param maxNrAssignments The maximum number of assignments to keep. + * @return TableFactor + */ + TableFactor prune(size_t maxNrAssignments) const; + + /// @} + /// @name Wrapper support + /// @{ + + /** + * @brief Render as markdown table + * + * @param keyFormatter GTSAM-style Key formatter. + * @param names optional, category names corresponding to choices. + * @return std::string a markdown string. + */ + std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter, + const Names& names = {}) const override; + + /** + * @brief Render as html table + * + * @param keyFormatter GTSAM-style Key formatter. + * @param names optional, category names corresponding to choices. + * @return std::string a html string. + */ + std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter, + const Names& names = {}) const override; + + /// @} + /// @name HybridValues methods. + /// @{ + + /** + * Calculate error for HybridValues `x`, is -log(probability) + * Simply dispatches to DiscreteValues version. + */ + double error(const HybridValues& values) const override; + + /// @} +}; + +// traits +template <> +struct traits : public Testable {}; +} // namespace gtsam diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp new file mode 100644 index 000000000..3ad757347 --- /dev/null +++ b/gtsam/discrete/tests/testTableFactor.cpp @@ -0,0 +1,360 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* + * testTableFactor.cpp + * + * @date Feb 15, 2023 + * @author Yoonwoo Kim + */ + +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +vector genArr(double dropout, size_t size) { + random_device rd; + mt19937 g(rd()); + vector dropoutmask(size); // Chance of 0 + + uniform_int_distribution<> dist(1, 9); + auto gen = [&dist, &g]() { return dist(g); }; + generate(dropoutmask.begin(), dropoutmask.end(), gen); + + fill_n(dropoutmask.begin(), dropoutmask.size() * (dropout), 0); + shuffle(dropoutmask.begin(), dropoutmask.end(), g); + + return dropoutmask; +} + +map> measureTime( + DiscreteKeys keys1, DiscreteKeys keys2, size_t size) { + vector dropouts = {0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9}; + map> measured_times; + + for (auto dropout : dropouts) { + vector arr1 = genArr(dropout, size); + vector arr2 = genArr(dropout, size); + TableFactor f1(keys1, arr1); + TableFactor f2(keys2, arr2); + DecisionTreeFactor f1_dt(keys1, arr1); + DecisionTreeFactor f2_dt(keys2, arr2); + + // measure time TableFactor + auto tb_start = chrono::high_resolution_clock::now(); + TableFactor actual = f1 * f2; + auto tb_end = chrono::high_resolution_clock::now(); + auto tb_time_diff = + chrono::duration_cast(tb_end - tb_start); + + // measure time DT + auto dt_start = chrono::high_resolution_clock::now(); + DecisionTreeFactor actual_dt = f1_dt * f2_dt; + auto dt_end = chrono::high_resolution_clock::now(); + auto dt_time_diff = + chrono::duration_cast(dt_end - dt_start); + + bool flag = true; + for (auto assignmentVal : actual_dt.enumerate()) { + flag = actual_dt(assignmentVal.first) != actual(assignmentVal.first); + if (flag) { + std::cout << "something is wrong: " << std::endl; + assignmentVal.first.print(); + std::cout << "dt: " << actual_dt(assignmentVal.first) << std::endl; + std::cout << "tb: " << actual(assignmentVal.first) << std::endl; + break; + } + } + if (flag) break; + measured_times[dropout] = make_pair(tb_time_diff, dt_time_diff); + } + return measured_times; +} + +void printTime(map> + measured_time) { + for (auto&& kv : measured_time) { + cout << "dropout: " << kv.first + << " | TableFactor time: " << kv.second.first.count() + << " | DecisionTreeFactor time: " << kv.second.second.count() << endl; + } +} + +/* ************************************************************************* */ +// Check constructors for TableFactor. +TEST(TableFactor, constructors) { + // Declare a bunch of keys + DiscreteKey X(0, 2), Y(1, 3), Z(2, 2), A(3, 5); + + // Create factors + TableFactor f_zeros(A, {0, 0, 0, 0, 1}); + TableFactor f1(X, {2, 8}); + TableFactor f2(X & Y, "2 5 3 6 4 7"); + TableFactor f3(X & Y & Z, "2 5 3 6 4 7 25 55 35 65 45 75"); + EXPECT_LONGS_EQUAL(1, f1.size()); + EXPECT_LONGS_EQUAL(2, f2.size()); + EXPECT_LONGS_EQUAL(3, f3.size()); + + DiscreteValues values; + values[0] = 1; // x + values[1] = 2; // y + values[2] = 1; // z + values[3] = 4; // a + EXPECT_DOUBLES_EQUAL(1, f_zeros(values), 1e-9); + EXPECT_DOUBLES_EQUAL(8, f1(values), 1e-9); + EXPECT_DOUBLES_EQUAL(7, f2(values), 1e-9); + EXPECT_DOUBLES_EQUAL(75, f3(values), 1e-9); + + // Assert that error = -log(value) + EXPECT_DOUBLES_EQUAL(-log(f1(values)), f1.error(values), 1e-9); +} + +/* ************************************************************************* */ +// Check multiplication between two TableFactors. +TEST(TableFactor, multiplication) { + DiscreteKey v0(0, 2), v1(1, 2), v2(2, 2); + + // Multiply with a DiscreteDistribution, i.e., Bayes Law! + DiscreteDistribution prior(v1 % "1/3"); + TableFactor f1(v0 & v1, "1 2 3 4"); + DecisionTreeFactor expected(v0 & v1, "0.25 1.5 0.75 3"); + CHECK(assert_equal(expected, static_cast(prior) * + f1.toDecisionTreeFactor())); + CHECK(assert_equal(expected, f1 * prior)); + + // Multiply two factors + TableFactor f2(v1 & v2, "5 6 7 8"); + TableFactor actual = f1 * f2; + TableFactor expected2(v0 & v1 & v2, "5 6 14 16 15 18 28 32"); + CHECK(assert_equal(expected2, actual)); + + DiscreteKey A(0, 3), B(1, 2), C(2, 2); + TableFactor f_zeros1(A & C, "0 0 0 2 0 3"); + TableFactor f_zeros2(B & C, "4 0 0 5"); + TableFactor actual_zeros = f_zeros1 * f_zeros2; + TableFactor expected3(A & B & C, "0 0 0 0 0 0 0 10 0 0 0 15"); + CHECK(assert_equal(expected3, actual_zeros)); +} + +/* ************************************************************************* */ +// Benchmark which compares runtime of multiplication of two TableFactors +// and two DecisionTreeFactors given sparsity from dense to 90% sparsity. +TEST(TableFactor, benchmark) { + DiscreteKey A(0, 5), B(1, 2), C(2, 5), D(3, 2), E(4, 5), F(5, 2), G(6, 3), + H(7, 2), I(8, 5), J(9, 7), K(10, 2), L(11, 3); + + // 100 + DiscreteKeys one_1 = {A, B, C, D}; + DiscreteKeys one_2 = {C, D, E, F}; + map> time_map_1 = + measureTime(one_1, one_2, 100); + printTime(time_map_1); + // 200 + DiscreteKeys two_1 = {A, B, C, D, F}; + DiscreteKeys two_2 = {B, C, D, E, F}; + map> time_map_2 = + measureTime(two_1, two_2, 200); + printTime(time_map_2); + // 300 + DiscreteKeys three_1 = {A, B, C, D, G}; + DiscreteKeys three_2 = {C, D, E, F, G}; + map> time_map_3 = + measureTime(three_1, three_2, 300); + printTime(time_map_3); + // 400 + DiscreteKeys four_1 = {A, B, C, D, F, H}; + DiscreteKeys four_2 = {B, C, D, E, F, H}; + map> time_map_4 = + measureTime(four_1, four_2, 400); + printTime(time_map_4); + // 500 + DiscreteKeys five_1 = {A, B, C, D, I}; + DiscreteKeys five_2 = {C, D, E, F, I}; + map> time_map_5 = + measureTime(five_1, five_2, 500); + printTime(time_map_5); + // 600 + DiscreteKeys six_1 = {A, B, C, D, F, G}; + DiscreteKeys six_2 = {B, C, D, E, F, G}; + map> time_map_6 = + measureTime(six_1, six_2, 600); + printTime(time_map_6); + // 700 + DiscreteKeys seven_1 = {A, B, C, D, J}; + DiscreteKeys seven_2 = {C, D, E, F, J}; + map> time_map_7 = + measureTime(seven_1, seven_2, 700); + printTime(time_map_7); + // 800 + DiscreteKeys eight_1 = {A, B, C, D, F, H, K}; + DiscreteKeys eight_2 = {B, C, D, E, F, H, K}; + map> time_map_8 = + measureTime(eight_1, eight_2, 800); + printTime(time_map_8); + // 900 + DiscreteKeys nine_1 = {A, B, C, D, G, L}; + DiscreteKeys nine_2 = {C, D, E, F, G, L}; + map> time_map_9 = + measureTime(nine_1, nine_2, 900); + printTime(time_map_9); +} + +/* ************************************************************************* */ +// Check sum and max over frontals. +TEST(TableFactor, sum_max) { + DiscreteKey v0(0, 3), v1(1, 2); + TableFactor f1(v0 & v1, "1 2 3 4 5 6"); + + TableFactor expected(v1, "9 12"); + TableFactor::shared_ptr actual = f1.sum(1); + CHECK(assert_equal(expected, *actual, 1e-5)); + + TableFactor expected2(v1, "5 6"); + TableFactor::shared_ptr actual2 = f1.max(1); + CHECK(assert_equal(expected2, *actual2)); + + TableFactor f2(v1 & v0, "1 2 3 4 5 6"); + TableFactor::shared_ptr actual22 = f2.sum(1); +} + +/* ************************************************************************* */ +// Check enumerate yields the correct list of assignment/value pairs. +TEST(TableFactor, enumerate) { + DiscreteKey A(12, 3), B(5, 2); + TableFactor f(A & B, "1 2 3 4 5 6"); + auto actual = f.enumerate(); + std::vector> expected; + DiscreteValues values; + for (size_t a : {0, 1, 2}) { + for (size_t b : {0, 1}) { + values[12] = a; + values[5] = b; + expected.emplace_back(values, f(values)); + } + } + EXPECT(actual == expected); +} + +/* ************************************************************************* */ +// Check pruning of the decision tree works as expected. +TEST(TableFactor, Prune) { + DiscreteKey A(1, 2), B(2, 2), C(3, 2); + TableFactor f(A & B & C, "1 5 3 7 2 6 4 8"); + + // Only keep the leaves with the top 5 values. + size_t maxNrAssignments = 5; + auto pruned5 = f.prune(maxNrAssignments); + + // Pruned leaves should be 0 + TableFactor expected(A & B & C, "0 5 0 7 0 6 4 8"); + EXPECT(assert_equal(expected, pruned5)); + + // Check for more extreme pruning where we only keep the top 2 leaves + maxNrAssignments = 2; + auto pruned2 = f.prune(maxNrAssignments); + TableFactor expected2(A & B & C, "0 0 0 7 0 0 0 8"); + EXPECT(assert_equal(expected2, pruned2)); + + DiscreteKey D(4, 2); + TableFactor factor( + D & C & B & A, + "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " + "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0"); + + TableFactor expected3(D & C & B & A, + "0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 " + "0.999952870000 1.0 1.0 1.0 1.0"); + maxNrAssignments = 5; + auto pruned3 = factor.prune(maxNrAssignments); + EXPECT(assert_equal(expected3, pruned3)); +} + +/* ************************************************************************* */ +// Check markdown representation looks as expected. +TEST(TableFactor, markdown) { + DiscreteKey A(12, 3), B(5, 2); + TableFactor f(A & B, "1 2 3 4 5 6"); + string expected = + "|A|B|value|\n" + "|:-:|:-:|:-:|\n" + "|0|0|1|\n" + "|0|1|2|\n" + "|1|0|3|\n" + "|1|1|4|\n" + "|2|0|5|\n" + "|2|1|6|\n"; + auto formatter = [](Key key) { return key == 12 ? "A" : "B"; }; + string actual = f.markdown(formatter); + EXPECT(actual == expected); +} + +/* ************************************************************************* */ +// Check markdown representation with a value formatter. +TEST(TableFactor, markdownWithValueFormatter) { + DiscreteKey A(12, 3), B(5, 2); + TableFactor f(A & B, "1 2 3 4 5 6"); + string expected = + "|A|B|value|\n" + "|:-:|:-:|:-:|\n" + "|Zero|-|1|\n" + "|Zero|+|2|\n" + "|One|-|3|\n" + "|One|+|4|\n" + "|Two|-|5|\n" + "|Two|+|6|\n"; + auto keyFormatter = [](Key key) { return key == 12 ? "A" : "B"; }; + TableFactor::Names names{{12, {"Zero", "One", "Two"}}, {5, {"-", "+"}}}; + string actual = f.markdown(keyFormatter, names); + EXPECT(actual == expected); +} + +/* ************************************************************************* */ +// Check html representation with a value formatter. +TEST(TableFactor, htmlWithValueFormatter) { + DiscreteKey A(12, 3), B(5, 2); + TableFactor f(A & B, "1 2 3 4 5 6"); + string expected = + "
\n" + "\n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + "
ABvalue
Zero-1
Zero+2
One-3
One+4
Two-5
Two+6
\n" + "
"; + auto keyFormatter = [](Key key) { return key == 12 ? "A" : "B"; }; + TableFactor::Names names{{12, {"Zero", "One", "Two"}}, {5, {"-", "+"}}}; + string actual = f.html(keyFormatter, names); + EXPECT(actual == expected); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 0d9f1bc01..05678dc27 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -97,7 +97,7 @@ Vector3 Pose2::Logmap(const Pose2& p, OptionalJacobian<3, 3> H) { /* ************************************************************************* */ Pose2 Pose2::ChartAtOrigin::Retract(const Vector3& v, ChartJacobian H) { -#ifdef SLOW_BUT_CORRECT_EXPMAP +#ifdef GTSAM_SLOW_BUT_CORRECT_EXPMAP return Expmap(v, H); #else if (H) { @@ -109,7 +109,7 @@ Pose2 Pose2::ChartAtOrigin::Retract(const Vector3& v, ChartJacobian H) { } /* ************************************************************************* */ Vector3 Pose2::ChartAtOrigin::Local(const Pose2& r, ChartJacobian H) { -#ifdef SLOW_BUT_CORRECT_EXPMAP +#ifdef GTSAM_SLOW_BUT_CORRECT_EXPMAP return Logmap(r, H); #else if (H) { diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index c3f588dcc..f1b38c5a6 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -258,10 +258,19 @@ public: inline const Rot2& r() const { return r_; } /// translation - inline const Point2& translation() const { return t_; } + inline const Point2& translation(OptionalJacobian<2, 3> Hself={}) const { + if (Hself) { + *Hself = Matrix::Zero(2, 3); + (*Hself).block<2, 2>(0, 0) = rotation().matrix(); + } + return t_; + } /// rotation - inline const Rot2& rotation() const { return r_; } + inline const Rot2& rotation(OptionalJacobian<1, 3> Hself={}) const { + if (Hself) *Hself << 0, 0, 1; + return r_; + } //// return transformation matrix GTSAM_EXPORT Matrix3 matrix() const; diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 0b3798baa..2b9c5a45a 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -560,8 +560,8 @@ class GTSAM_EXPORT Rot3 : public LieGroup { #endif }; - /// std::vector of Rot3s, mainly for wrapper - using Rot3Vector = std::vector >; + /// std::vector of Rot3s, used in Matlab wrapper + using Rot3Vector = std::vector>; /** * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index bd335dfbb..4383e212e 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -46,7 +46,9 @@ public: uL_(0), uR_(0), v_(0) { } - /** constructor */ + /** uL and uR represent the x-axis value of left and right frame coordinates respectively. + v represents the y coordinate value. The y-axis value should be the same under the + stereo constraint. */ StereoPoint2(double uL, double uR, double v) : uL_(uL), uR_(uR), v_(v) { } diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 630f6d252..57f2acffe 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -28,7 +28,8 @@ class Point2 { // enabling serialization functionality void serialize() const; }; - + +// Used in Matlab wrapper class Point2Pairs { Point2Pairs(); size_t size() const; @@ -38,6 +39,7 @@ class Point2Pairs { }; // std::vector +// Used in Matlab wrapper class Point2Vector { // Constructors Point2Vector(); @@ -131,6 +133,7 @@ class Point3 { gtsam::Point3 normalize(const gtsam::Point3 &p, Eigen::Ref H) const; }; +// Used in Matlab wrapper class Point3Pairs { Point3Pairs(); size_t size() const; @@ -163,7 +166,9 @@ class Rot2 { // Manifold gtsam::Rot2 retract(Vector v) const; + gtsam::Rot2 retract(Vector v, Eigen::Ref H1, Eigen::Ref H2) const; Vector localCoordinates(const gtsam::Rot2& p) const; + Vector localCoordinates(const gtsam::Rot2& p, Eigen::Ref H1, Eigen::Ref H2) const; // Lie Group static gtsam::Rot2 Expmap(Vector v); @@ -394,19 +399,24 @@ class Pose2 { static gtsam::Pose2 Identity(); gtsam::Pose2 inverse() const; gtsam::Pose2 compose(const gtsam::Pose2& p2) const; + gtsam::Pose2 compose(const gtsam::Pose2& p2, Eigen::Ref H1, Eigen::Ref H2) const; gtsam::Pose2 between(const gtsam::Pose2& p2) const; + gtsam::Pose2 between(const gtsam::Pose2& p2, Eigen::Ref H1, Eigen::Ref H2) const; // Operator Overloads gtsam::Pose2 operator*(const gtsam::Pose2& p2) const; // Manifold gtsam::Pose2 retract(Vector v) const; + gtsam::Pose2 retract(Vector v, Eigen::Ref H1, Eigen::Ref H2) const; Vector localCoordinates(const gtsam::Pose2& p) const; + Vector localCoordinates(const gtsam::Pose2& p, Eigen::Ref H1, Eigen::Ref H2) const; // Lie Group static gtsam::Pose2 Expmap(Vector v); static Vector Logmap(const gtsam::Pose2& p); Vector logmap(const gtsam::Pose2& p); + Vector logmap(const gtsam::Pose2& p, Eigen::Ref H); static Matrix ExpmapDerivative(Vector v); static Matrix LogmapDerivative(const gtsam::Pose2& v); Matrix AdjointMap() const; @@ -431,7 +441,9 @@ class Pose2 { gtsam::Rot2 bearing(const gtsam::Point2& point) const; double range(const gtsam::Point2& point) const; gtsam::Point2 translation() const; + gtsam::Point2 translation(Eigen::Ref Hself) const; gtsam::Rot2 rotation() const; + gtsam::Rot2 rotation(Eigen::Ref Hself) const; Matrix matrix() const; // enabling serialization functionality @@ -542,6 +554,7 @@ class Pose3 { void serialize() const; }; +// Used in Matlab wrapper class Pose3Pairs { Pose3Pairs(); size_t size() const; @@ -550,6 +563,7 @@ class Pose3Pairs { void push_back(const gtsam::Pose3Pair& pose_pair); }; +// Used in Matlab wrapper class Pose3Vector { Pose3Vector(); size_t size() const; @@ -575,13 +589,13 @@ class Unit3 { gtsam::Point3 point3() const; gtsam::Point3 point3(Eigen::Ref H) const; - Vector3 unitVector() const; - Vector3 unitVector(Eigen::Ref H) const; + gtsam::Vector3 unitVector() const; + gtsam::Vector3 unitVector(Eigen::Ref H) const; double dot(const gtsam::Unit3& q) const; double dot(const gtsam::Unit3& q, Eigen::Ref H1, Eigen::Ref H2) const; - Vector2 errorVector(const gtsam::Unit3& q) const; - Vector2 errorVector(const gtsam::Unit3& q, Eigen::Ref H_p, + gtsam::Vector2 errorVector(const gtsam::Unit3& q) const; + gtsam::Vector2 errorVector(const gtsam::Unit3& q, Eigen::Ref H_p, Eigen::Ref H_q) const; // Manifold @@ -1174,8 +1188,8 @@ class TriangulationParameters { const gtsam::SharedNoiseModel& noiseModel = nullptr); }; -// Templates appear not yet supported for free functions - issue raised at -// borglab/wrap#14 to add support +// Can be templated but overloaded for convenience. +// We can now call `triangulatePoint3` with any template type. // Cal3_S2 versions gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 8c1bdccc0..9e4efe799 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -66,7 +66,7 @@ TEST(Pose2, manifold) { /* ************************************************************************* */ TEST(Pose2, retract) { Pose2 pose(M_PI/2.0, Point2(1, 2)); -#ifdef SLOW_BUT_CORRECT_EXPMAP +#ifdef GTSAM_SLOW_BUT_CORRECT_EXPMAP Pose2 expected(1.00811, 2.01528, 2.5608); #else Pose2 expected(M_PI/2.0+0.99, Point2(1.015, 2.01)); @@ -204,7 +204,7 @@ TEST(Pose2, Adjoint_hat) { TEST(Pose2, logmap) { Pose2 pose0(M_PI/2.0, Point2(1, 2)); Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01)); -#ifdef SLOW_BUT_CORRECT_EXPMAP +#ifdef GTSAM_SLOW_BUT_CORRECT_EXPMAP Vector3 expected(0.00986473, -0.0150896, 0.018); #else Vector3 expected(0.01, -0.015, 0.018); @@ -474,6 +474,33 @@ TEST( Pose2, compose_matrix ) EXPECT(assert_equal(gM1*_1M2,matrix(gT1.compose(_1T2)))); // RIGHT DOES NOT } + + +/* ************************************************************************* */ +TEST( Pose2, translation ) { + Pose2 pose(3.5, -8.2, 4.2); + + Matrix actualH; + EXPECT(assert_equal((Vector2() << 3.5, -8.2).finished(), pose.translation(actualH), 1e-8)); + + std::function f = [](const Pose2& T) { return T.translation(); }; + Matrix numericalH = numericalDerivative11(f, pose); + EXPECT(assert_equal(numericalH, actualH, 1e-6)); +} + +/* ************************************************************************* */ +TEST( Pose2, rotation ) { + Pose2 pose(3.5, -8.2, 4.2); + + Matrix actualH(4, 3); + EXPECT(assert_equal(Rot2(4.2), pose.rotation(actualH), 1e-8)); + + std::function f = [](const Pose2& T) { return T.rotation(); }; + Matrix numericalH = numericalDerivative11(f, pose); + EXPECT(assert_equal(numericalH, actualH, 1e-6)); +} + + /* ************************************************************************* */ TEST( Pose2, between ) { diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 06fb8fafe..039dfd74d 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -85,7 +85,8 @@ Vector4 triangulateHomogeneousDLT( Point3 triangulateLOST(const std::vector& poses, const Point3Vector& calibratedMeasurements, - const SharedIsotropic& measurementNoise) { + const SharedIsotropic& measurementNoise, + double rank_tol) { size_t m = calibratedMeasurements.size(); assert(m == poses.size()); @@ -96,17 +97,38 @@ Point3 triangulateLOST(const std::vector& poses, for (size_t i = 0; i < m; i++) { const Pose3& wTi = poses[i]; // TODO(akshay-krishnan): are there better ways to select j? - const int j = (i + 1) % m; + int j = (i + 1) % m; const Pose3& wTj = poses[j]; - const Point3 d_ij = wTj.translation() - wTi.translation(); + Point3 d_ij = wTj.translation() - wTi.translation(); + Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]); + Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]); + double num_i = wZi.cross(wZj).norm(); + double den_i = d_ij.cross(wZj).norm(); - const Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]); - const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]); + // Handle q_i = 0 (or NaN), which arises if the measurement vectors, wZi and + // wZj, coincide (or the baseline vector coincides with the jth measurement + // vector). + if (num_i == 0 || den_i == 0) { + bool success = false; + for (size_t k = 2; k < m; k++) { + j = (i + k) % m; + const Pose3& wTj = poses[j]; + + d_ij = wTj.translation() - wTi.translation(); + wZj = wTj.rotation().rotate(calibratedMeasurements[j]); + num_i = wZi.cross(wZj).norm(); + den_i = d_ij.cross(wZj).norm(); + if (num_i > 0 && den_i > 0) { + success = true; + break; + } + } + if (!success) throw(TriangulationUnderconstrainedException()); + } // Note: Setting q_i = 1.0 gives same results as DLT. - const double q_i = wZi.cross(wZj).norm() / - (measurementNoise->sigma() * d_ij.cross(wZj).norm()); + const double q_i = num_i / (measurementNoise->sigma() * den_i); const Matrix23 coefficientMat = q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) * @@ -115,7 +137,13 @@ Point3 triangulateLOST(const std::vector& poses, A.block<2, 3>(2 * i, 0) << coefficientMat; b.block<2, 1>(2 * i, 0) << coefficientMat * wTi.translation(); } - return A.colPivHouseholderQr().solve(b); + + Eigen::ColPivHouseholderQR A_Qr = A.colPivHouseholderQr(); + A_Qr.setThreshold(rank_tol); + + if (A_Qr.rank() < 3) throw(TriangulationUnderconstrainedException()); + + return A_Qr.solve(b); } Point3 triangulateDLT( diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 7e58cee2d..3a8398804 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -110,7 +110,8 @@ GTSAM_EXPORT Point3 triangulateDLT( */ GTSAM_EXPORT Point3 triangulateLOST(const std::vector& poses, const Point3Vector& calibratedMeasurements, - const SharedIsotropic& measurementNoise); + const SharedIsotropic& measurementNoise, + double rank_tol = 1e-9); /** * Create a factor graph with projection factors from poses and one calibration @@ -439,7 +440,8 @@ Point3 triangulatePoint3(const std::vector& poses, auto calibratedMeasurements = calibrateMeasurementsShared(*sharedCal, measurements); - point = triangulateLOST(poses, calibratedMeasurements, measurementNoise); + point = triangulateLOST(poses, calibratedMeasurements, measurementNoise, + rank_tol); } else { // construct projection matrices from poses & calibration auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal); @@ -512,7 +514,8 @@ Point3 triangulatePoint3(const CameraSet& cameras, auto calibratedMeasurements = calibrateMeasurements(cameras, measurements); - point = triangulateLOST(poses, calibratedMeasurements, measurementNoise); + point = triangulateLOST(poses, calibratedMeasurements, measurementNoise, + rank_tol); } else { // construct projection matrices from poses & calibration auto projection_matrices = projectionMatricesFromCameras(cameras); @@ -750,4 +753,3 @@ using CameraSetCal3Fisheye = CameraSet>; using CameraSetCal3Unified = CameraSet>; using CameraSetSpherical = CameraSet; } // \namespace gtsam - diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 00b4d05f8..834d5a147 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -105,6 +105,7 @@ class KeyGroupMap { }; // Actually a FastSet +// Used in Matlab wrapper class FactorIndexSet { FactorIndexSet(); FactorIndexSet(const gtsam::FactorIndexSet& set); @@ -121,6 +122,7 @@ class FactorIndexSet { }; // Actually a vector +// Used in Matlab wrapper class FactorIndices { FactorIndices(); FactorIndices(const gtsam::FactorIndices& other); diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i index d4c90e356..f9696258a 100644 --- a/gtsam/inference/inference.i +++ b/gtsam/inference/inference.i @@ -109,6 +109,7 @@ class Ordering { FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph); + static gtsam::Ordering Colamd(const gtsam::VariableIndex& variableIndex); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, @@ -176,13 +177,9 @@ class DotWriter { class VariableIndex { // Standard Constructors and Named Constructors VariableIndex(); - // TODO: Templetize constructor when wrap supports it - // template - // VariableIndex(const T& factorGraph, size_t nVariables); - // VariableIndex(const T& factorGraph); - VariableIndex(const gtsam::SymbolicFactorGraph& sfg); - VariableIndex(const gtsam::GaussianFactorGraph& gfg); - VariableIndex(const gtsam::NonlinearFactorGraph& fg); + template + VariableIndex(const T& factorGraph); VariableIndex(const gtsam::VariableIndex& other); // Testable diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index 70166d97e..c78bb512c 100644 --- a/gtsam/linear/LossFunctions.h +++ b/gtsam/linear/LossFunctions.h @@ -91,7 +91,7 @@ class GTSAM_EXPORT Base { * functions. It would be better for this function to accept the vector and * internally call the norm if necessary. * - * This returns \rho(x) in \ref mEstimator + * This returns \f$\rho(x)\f$ in \ref mEstimator */ virtual double loss(double distance) const { return 0; } @@ -143,9 +143,9 @@ class GTSAM_EXPORT Base { * * This model has no additional parameters. * - * - Loss \rho(x) = 0.5 x² - * - Derivative \phi(x) = x - * - Weight w(x) = \phi(x)/x = 1 + * - Loss \f$ \rho(x) = 0.5 x² \f$ + * - Derivative \f$ \phi(x) = x \f$ + * - Weight \f$ w(x) = \phi(x)/x = 1 \f$ */ class GTSAM_EXPORT Null : public Base { public: @@ -285,9 +285,9 @@ class GTSAM_EXPORT Cauchy : public Base { * * This model has a scalar parameter "c". * - * - Loss \rho(x) = c² (1 - (1-x²/c²)³)/6 if |x|k, (k+x) if x<-k - * - Weight w(x) = \phi(x)/x = 0 if |x|k, (k+x)/x if x<-k + * - Loss \f$ \rho(x) = 0 \f$ if |x|k, (k+x) if x<-k + * - Weight \f$ w(x) = \phi(x)/x = 0 \f$ if |x|k, (k+x)/x if x<-k */ class GTSAM_EXPORT L2WithDeadZone : public Base { protected: diff --git a/gtsam/nonlinear/CustomFactor.cpp b/gtsam/nonlinear/CustomFactor.cpp index bb0a77078..a6415df20 100644 --- a/gtsam/nonlinear/CustomFactor.cpp +++ b/gtsam/nonlinear/CustomFactor.cpp @@ -28,7 +28,7 @@ Vector CustomFactor::unwhitenedError(const Values& x, OptionalMatrixVecType H) c if(H) { /* * In this case, we pass the raw pointer to the `std::vector` object directly to pybind. - * As the type `std::vector` has been marked as opaque in `preamble.h`, any changes in + * As the type `std::vector` has been marked as opaque in `preamble/custom.h`, any changes in * Python will be immediately reflected on the C++ side. * * Example: diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 19f4ae588..4d0d847c1 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -25,7 +25,6 @@ namespace gtsam { #include #include #include -#include #include class GraphvizFormatting : gtsam::DotWriter { @@ -305,8 +304,8 @@ virtual class GncParams { double relativeCostTol; double weightsTol; gtsam::This::Verbosity verbosity; - gtsam::KeyVector knownInliers; - gtsam::KeyVector knownOutliers; + gtsam::This::IndexVector knownInliers; + gtsam::This::IndexVector knownOutliers; void setLossType(const gtsam::GncLossType type); void setMaxIterations(const size_t maxIter); @@ -314,8 +313,8 @@ virtual class GncParams { void setRelativeCostTol(double value); void setWeightsTol(double value); void setVerbosityGNC(const gtsam::This::Verbosity value); - void setKnownInliers(const gtsam::KeyVector& knownIn); - void setKnownOutliers(const gtsam::KeyVector& knownOut); + void setKnownInliers(const gtsam::This::IndexVector& knownIn); + void setKnownOutliers(const gtsam::This::IndexVector& knownOut); void print(const string& str = "GncParams: ") const; enum Verbosity { @@ -508,26 +507,22 @@ class ISAM2 { gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, - gtsam::KeyGroupMap& constrainedKeys, + const gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, - const gtsam::Values& newTheta, - const gtsam::FactorIndices& removeFactorIndices, - gtsam::KeyGroupMap& constrainedKeys, - const gtsam::KeyList& noRelinKeys, - const gtsam::KeyList& extraReelimKeys); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys, - bool force_relinearize); + bool force_relinearize = false); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::ISAM2UpdateParams& updateParams); + double error(const gtsam::VectorValues& values) const; + gtsam::Values getLinearizationPoint() const; bool valueExists(gtsam::Key key) const; gtsam::Values calculateEstimate() const; @@ -553,9 +548,8 @@ class ISAM2 { string dot(const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - void saveGraph(string s, - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; + void saveGraph(string s, const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; }; #include diff --git a/gtsam/nonlinear/values.i b/gtsam/nonlinear/values.i index 680cafc31..e36b1cf1c 100644 --- a/gtsam/nonlinear/values.i +++ b/gtsam/nonlinear/values.i @@ -25,7 +25,6 @@ namespace gtsam { #include #include #include -#include #include @@ -96,21 +95,6 @@ class Values { void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert(size_t j, const gtsam::NavState& nav_state); void insert(size_t j, double c); - void insert(size_t j, const gtsam::ParameterMatrix<1>& X); - void insert(size_t j, const gtsam::ParameterMatrix<2>& X); - void insert(size_t j, const gtsam::ParameterMatrix<3>& X); - void insert(size_t j, const gtsam::ParameterMatrix<4>& X); - void insert(size_t j, const gtsam::ParameterMatrix<5>& X); - void insert(size_t j, const gtsam::ParameterMatrix<6>& X); - void insert(size_t j, const gtsam::ParameterMatrix<7>& X); - void insert(size_t j, const gtsam::ParameterMatrix<8>& X); - void insert(size_t j, const gtsam::ParameterMatrix<9>& X); - void insert(size_t j, const gtsam::ParameterMatrix<10>& X); - void insert(size_t j, const gtsam::ParameterMatrix<11>& X); - void insert(size_t j, const gtsam::ParameterMatrix<12>& X); - void insert(size_t j, const gtsam::ParameterMatrix<13>& X); - void insert(size_t j, const gtsam::ParameterMatrix<14>& X); - void insert(size_t j, const gtsam::ParameterMatrix<15>& X); template void insert(size_t j, const T& val); @@ -144,21 +128,6 @@ class Values { void update(size_t j, Vector vector); void update(size_t j, Matrix matrix); void update(size_t j, double c); - void update(size_t j, const gtsam::ParameterMatrix<1>& X); - void update(size_t j, const gtsam::ParameterMatrix<2>& X); - void update(size_t j, const gtsam::ParameterMatrix<3>& X); - void update(size_t j, const gtsam::ParameterMatrix<4>& X); - void update(size_t j, const gtsam::ParameterMatrix<5>& X); - void update(size_t j, const gtsam::ParameterMatrix<6>& X); - void update(size_t j, const gtsam::ParameterMatrix<7>& X); - void update(size_t j, const gtsam::ParameterMatrix<8>& X); - void update(size_t j, const gtsam::ParameterMatrix<9>& X); - void update(size_t j, const gtsam::ParameterMatrix<10>& X); - void update(size_t j, const gtsam::ParameterMatrix<11>& X); - void update(size_t j, const gtsam::ParameterMatrix<12>& X); - void update(size_t j, const gtsam::ParameterMatrix<13>& X); - void update(size_t j, const gtsam::ParameterMatrix<14>& X); - void update(size_t j, const gtsam::ParameterMatrix<15>& X); void insert_or_assign(size_t j, const gtsam::Point2& point2); void insert_or_assign(size_t j, const gtsam::Point3& point3); @@ -199,21 +168,6 @@ class Values { void insert_or_assign(size_t j, Vector vector); void insert_or_assign(size_t j, Matrix matrix); void insert_or_assign(size_t j, double c); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<1>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<2>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<3>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<4>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<5>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<6>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<7>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<8>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<9>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<10>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<11>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<12>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<13>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<14>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<15>& X); template , - gtsam::ParameterMatrix<2>, - gtsam::ParameterMatrix<3>, - gtsam::ParameterMatrix<4>, - gtsam::ParameterMatrix<5>, - gtsam::ParameterMatrix<6>, - gtsam::ParameterMatrix<7>, - gtsam::ParameterMatrix<8>, - gtsam::ParameterMatrix<9>, - gtsam::ParameterMatrix<10>, - gtsam::ParameterMatrix<11>, - gtsam::ParameterMatrix<12>, - gtsam::ParameterMatrix<13>, - gtsam::ParameterMatrix<14>, - gtsam::ParameterMatrix<15>}> + double}> T at(size_t j); }; diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index c57e03174..930a0dd46 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -6,15 +6,14 @@ namespace gtsam { #include class SfmTrack2d { - std::vector> measurements; + std::vector measurements; SfmTrack2d(); SfmTrack2d(const std::vector& measurements); size_t numberMeasurements() const; - pair measurement(size_t idx) const; + gtsam::SfmMeasurement measurement(size_t idx) const; pair siftIndex(size_t idx) const; void addMeasurement(size_t idx, const gtsam::Point2& m); - gtsam::SfmMeasurement measurement(size_t idx) const; bool hasUniqueCameras() const; Eigen::MatrixX2d measurementMatrix() const; Eigen::VectorXi indexVector() const; @@ -100,6 +99,7 @@ typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; typedef gtsam::BinaryMeasurement BinaryMeasurementPoint3; +// Used in Matlab wrapper class BinaryMeasurementsUnit3 { BinaryMeasurementsUnit3(); size_t size() const; @@ -107,6 +107,7 @@ class BinaryMeasurementsUnit3 { void push_back(const gtsam::BinaryMeasurement& measurement); }; +// Used in Matlab wrapper class BinaryMeasurementsPoint3 { BinaryMeasurementsPoint3(); size_t size() const; @@ -114,6 +115,7 @@ class BinaryMeasurementsPoint3 { void push_back(const gtsam::BinaryMeasurement& measurement); }; +// Used in Matlab wrapper class BinaryMeasurementsRot3 { BinaryMeasurementsRot3(); size_t size() const; @@ -121,41 +123,19 @@ class BinaryMeasurementsRot3 { void push_back(const gtsam::BinaryMeasurement& measurement); }; +#include #include -// TODO(frank): copy/pasta below until we have integer template parameters in -// wrap! - -class ShonanAveragingParameters2 { - ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm); - ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm, +template +class ShonanAveragingParameters { + ShonanAveragingParameters(const gtsam::LevenbergMarquardtParams& lm); + ShonanAveragingParameters(const gtsam::LevenbergMarquardtParams& lm, string method); gtsam::LevenbergMarquardtParams getLMParams() const; void setOptimalityThreshold(double value); double getOptimalityThreshold() const; - void setAnchor(size_t index, const gtsam::Rot2& value); - pair getAnchor(); - void setAnchorWeight(double value); - double getAnchorWeight() const; - void setKarcherWeight(double value); - double getKarcherWeight() const; - void setGaugesWeight(double value); - double getGaugesWeight() const; - void setUseHuber(bool value); - bool getUseHuber() const; - void setCertifyOptimality(bool value); - bool getCertifyOptimality() const; -}; - -class ShonanAveragingParameters3 { - ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm); - ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm, - string method); - gtsam::LevenbergMarquardtParams getLMParams() const; - void setOptimalityThreshold(double value); - double getOptimalityThreshold() const; - void setAnchor(size_t index, const gtsam::Rot3& value); - pair getAnchor(); + void setAnchor(size_t index, const gtsam::This::Rot& value); + pair getAnchor(); void setAnchorWeight(double value); double getAnchorWeight() const; void setKarcherWeight(double value); @@ -168,6 +148,7 @@ class ShonanAveragingParameters3 { bool getCertifyOptimality() const; }; +// NOTE(Varun): Not templated because each class has specializations defined. class ShonanAveraging2 { ShonanAveraging2(string g2oFile); ShonanAveraging2(string g2oFile, @@ -214,18 +195,16 @@ class ShonanAveraging2 { }; class ShonanAveraging3 { - ShonanAveraging3( - const std::vector>& measurements, - const gtsam::ShonanAveragingParameters3& parameters = - gtsam::ShonanAveragingParameters3()); + ShonanAveraging3(const gtsam::This::Measurements& measurements, + const gtsam::ShonanAveragingParameters3& parameters = + gtsam::ShonanAveragingParameters3()); ShonanAveraging3(string g2oFile); ShonanAveraging3(string g2oFile, const gtsam::ShonanAveragingParameters3& parameters); - // TODO(frank): deprecate once we land pybind wrapper - ShonanAveraging3(const gtsam::BetweenFactorPose3s& factors); ShonanAveraging3(const gtsam::BetweenFactorPose3s& factors, - const gtsam::ShonanAveragingParameters3& parameters); + const gtsam::ShonanAveragingParameters3& parameters = + gtsam::ShonanAveragingParameters3()); // Query properties size_t nrUnknowns() const; @@ -267,6 +246,7 @@ class ShonanAveraging3 { #include +// Used in Matlab wrapper class KeyPairDoubleMap { KeyPairDoubleMap(); KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other); @@ -310,12 +290,9 @@ class TranslationRecovery { const gtsam::BinaryMeasurementsUnit3& relativeTranslations, const double scale, const gtsam::BinaryMeasurementsPoint3& betweenTranslations) const; - // default empty betweenTranslations - gtsam::Values run(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, - const double scale) const; // default scale = 1.0, empty betweenTranslations - gtsam::Values run( - const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const; + gtsam::Values run(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + const double scale = 1.0) const; }; namespace gtsfm { diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 8e1e06d5b..d35a87eee 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -251,7 +251,7 @@ void save2D(const gtsam::NonlinearFactorGraph& graph, string filename); // std::vector::shared_ptr> -// Ignored by pybind -> will be List[BetweenFactorPose2] +// Used in Matlab wrapper class BetweenFactorPose2s { BetweenFactorPose2s(); size_t size() const; @@ -261,13 +261,14 @@ class BetweenFactorPose2s { gtsam::BetweenFactorPose2s parse2DFactors(string filename); // std::vector::shared_ptr> -// Ignored by pybind -> will be List[BetweenFactorPose3] +// Used in Matlab wrapper class BetweenFactorPose3s { BetweenFactorPose3s(); size_t size() const; gtsam::BetweenFactor* at(size_t i) const; void push_back(const gtsam::BetweenFactor* factor); }; + gtsam::BetweenFactorPose3s parse3DFactors(string filename); pair load3D(string filename); diff --git a/gtsam_unstable/slam/tests/CMakeLists.txt b/gtsam_unstable/slam/tests/CMakeLists.txt index 6872dd575..bb5259ef2 100644 --- a/gtsam_unstable/slam/tests/CMakeLists.txt +++ b/gtsam_unstable/slam/tests/CMakeLists.txt @@ -2,7 +2,6 @@ # Exclude tests that don't work set (slam_excluded_tests testSerialization.cpp - testSmartStereoProjectionFactorPP.cpp # unstable after PR #1442 ) gtsamAddTestsGlob(slam_unstable "test*.cpp" "${slam_excluded_tests}" "gtsam_unstable") diff --git a/matlab/gtsam_tests/testCal3Unified.m b/matlab/gtsam_tests/testCal3Unified.m index 498c65343..ec5bff871 100644 --- a/matlab/gtsam_tests/testCal3Unified.m +++ b/matlab/gtsam_tests/testCal3Unified.m @@ -5,3 +5,8 @@ K = Cal3Unified; EXPECT('fx',K.fx()==1); EXPECT('fy',K.fy()==1); +params = PreintegrationParams.MakeSharedU(-9.81); +%params.getOmegaCoriolis() + +expectedBodyPSensor = gtsam.Pose3(gtsam.Rot3(0, 0, 0, 0, 0, 0, 0, 0, 0), gtsam.Point3(0, 0, 0)); +EXPECT('getBodyPSensor', expectedBodyPSensor.equals(params.getBodyPSensor(), 1e-9)); diff --git a/matlab/gtsam_tests/testEnum.m b/matlab/gtsam_tests/testEnum.m new file mode 100644 index 000000000..8e5e935f6 --- /dev/null +++ b/matlab/gtsam_tests/testEnum.m @@ -0,0 +1,12 @@ +% test Enum +import gtsam.*; + +params = GncLMParams(); + +EXPECT('Get lossType',params.lossType==GncLossType.TLS); + +params.lossType = GncLossType.GM; +EXPECT('Set lossType',params.lossType==GncLossType.GM); + +params.setLossType(GncLossType.TLS); +EXPECT('setLossType',params.lossType==GncLossType.TLS); diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 2557da237..2b2abf507 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -48,10 +48,12 @@ set(ignore gtsam::ISAM2ThresholdMapValue gtsam::FactorIndices gtsam::FactorIndexSet + gtsam::IndexPairSet gtsam::IndexPairSetMap gtsam::IndexPairVector gtsam::BetweenFactorPose2s gtsam::BetweenFactorPose3s + gtsam::FixedLagSmootherKeyTimestampMap gtsam::FixedLagSmootherKeyTimestampMapValue gtsam::Point2Vector gtsam::Point2Pairs diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py index d00e47b65..904afd2e0 100644 --- a/python/gtsam/__init__.py +++ b/python/gtsam/__init__.py @@ -4,9 +4,49 @@ import sys +from gtsam.utils import findExampleDataFile + from gtsam import gtsam, utils from gtsam.gtsam import * -from gtsam.utils import findExampleDataFile + +#### Typedefs to allow for backwards compatibility +#TODO(Varun) deprecate in future release +# gtsam +KeyVector = list +# base +IndexPairSetMap = dict +IndexPairVector = list +# geometry +Point2Vector = list +Pose3Vector = list +Rot3Vector = list +Point2Pairs = list +Point3Pairs = list +Pose2Pairs = list +Pose3Pairs = list +# sfm +BinaryMeasurementsPoint3 = list +BinaryMeasurementsUnit3 = list +BinaryMeasurementsRot3 = list +KeyPairDoubleMap = dict +SfmTrack2dVector = list +SfmTracks = list +SfmCameras = list +SfmMeasurementVector = list +MatchIndicesMap = dict +KeypointsVector = list +# slam +BetweenFactorPose3s = list +BetweenFactorPose2s = list + + +class FixedLagSmootherKeyTimestampMap(dict): + """Class to provide backwards compatibility""" + def insert(self, key_value): + self[key_value[0]] = key_value[1] + + +#### End typedefs def _init(): diff --git a/python/gtsam/examples/FixedLagSmootherExample.py b/python/gtsam/examples/FixedLagSmootherExample.py index 99af0edcf..c56ebbe07 100644 --- a/python/gtsam/examples/FixedLagSmootherExample.py +++ b/python/gtsam/examples/FixedLagSmootherExample.py @@ -16,6 +16,7 @@ import numpy as np import gtsam import gtsam_unstable + def BatchFixedLagSmootherExample(): """ Runs a batch fixed smoother on an agent with two odometry @@ -31,7 +32,7 @@ def BatchFixedLagSmootherExample(): # that will be sent to the smoothers new_factors = gtsam.NonlinearFactorGraph() new_values = gtsam.Values() - new_timestamps = gtsam.FixedLagSmootherKeyTimestampMap() + new_timestamps = {} # Create a prior on the first pose, placing it at the origin prior_mean = gtsam.Pose2(0, 0, 0) @@ -39,7 +40,7 @@ def BatchFixedLagSmootherExample(): 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)) + new_timestamps[X1] = 0.0 delta_time = 0.25 time = 0.25 @@ -49,7 +50,7 @@ def BatchFixedLagSmootherExample(): current_key = int(1000 * time) # assign current key to the current timestamp - new_timestamps.insert((current_key, time)) + new_timestamps[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] diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index 92a7d04e3..ea1cab88d 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -17,9 +17,8 @@ Date: September 2020 from collections import defaultdict from typing import List, Tuple -import numpy as np - import gtsam +import numpy as np from gtsam.examples import SFMdata # Hyperparameters for 1dsfm, values used from Kyle Wilson's code. @@ -59,7 +58,7 @@ def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]: return wRc_values, i_iZj_list -def filter_outliers(w_iZj_list: gtsam.BinaryMeasurementsUnit3) -> gtsam.BinaryMeasurementsUnit3: +def filter_outliers(w_iZj_list: List[gtsam.BinaryMeasurementUnit3]) -> List[gtsam.BinaryMeasurementUnit3]: """Removes outliers from a list of Unit3 measurements that are the translation directions from camera i to camera j in the world frame.""" @@ -89,14 +88,14 @@ def filter_outliers(w_iZj_list: gtsam.BinaryMeasurementsUnit3) -> gtsam.BinaryMe avg_outlier_weights[keypair] += weight / len(outlier_weights) # Remove w_iZj that have weight greater than threshold, these are outliers. - w_iZj_inliers = gtsam.BinaryMeasurementsUnit3() + w_iZj_inliers = [] [w_iZj_inliers.append(w_iZj) for w_iZj in w_iZj_list if avg_outlier_weights[( w_iZj.key1(), w_iZj.key2())] < OUTLIER_WEIGHT_THRESHOLD] return w_iZj_inliers -def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, +def estimate_poses(i_iZj_list: List[gtsam.BinaryMeasurementUnit3], wRc_values: gtsam.Values) -> gtsam.Values: """Estimate poses given rotations and normalized translation directions between cameras. @@ -112,7 +111,7 @@ def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, """ # Convert the translation direction measurements to world frame using the rotations. - w_iZj_list = gtsam.BinaryMeasurementsUnit3() + w_iZj_list = [] for i_iZj in i_iZj_list: w_iZj = gtsam.Unit3(wRc_values.atRot3(i_iZj.key1()) .rotate(i_iZj.measured().point3())) diff --git a/python/gtsam/gtsam.tpl b/python/gtsam/gtsam.tpl index d748fd65e..c72a216a2 100644 --- a/python/gtsam/gtsam.tpl +++ b/python/gtsam/gtsam.tpl @@ -11,6 +11,7 @@ #include #include +#include #include #include #include diff --git a/python/gtsam/preamble/base.h b/python/gtsam/preamble/base.h index 5cf633e65..d07a75f6f 100644 --- a/python/gtsam/preamble/base.h +++ b/python/gtsam/preamble/base.h @@ -10,9 +10,3 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ - -PYBIND11_MAKE_OPAQUE(gtsam::IndexPairVector); - -PYBIND11_MAKE_OPAQUE(gtsam::IndexPairSet); - -PYBIND11_MAKE_OPAQUE(std::vector); // JacobianVector diff --git a/python/gtsam/preamble/basis.h b/python/gtsam/preamble/basis.h index 56a07cfdd..d07a75f6f 100644 --- a/python/gtsam/preamble/basis.h +++ b/python/gtsam/preamble/basis.h @@ -10,5 +10,3 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ - -#include diff --git a/python/gtsam/preamble/custom.h b/python/gtsam/preamble/custom.h index d07a75f6f..d7bc6360f 100644 --- a/python/gtsam/preamble/custom.h +++ b/python/gtsam/preamble/custom.h @@ -10,3 +10,6 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ + +// Added so that CustomFactor can pass the JacobianVector to the C++ side +PYBIND11_MAKE_OPAQUE(std::vector); // JacobianVector diff --git a/python/gtsam/preamble/discrete.h b/python/gtsam/preamble/discrete.h index 320e0ac71..598ab626d 100644 --- a/python/gtsam/preamble/discrete.h +++ b/python/gtsam/preamble/discrete.h @@ -11,5 +11,4 @@ * mutations on Python side will not be reflected on C++. */ -#include - +PYBIND11_MAKE_OPAQUE(gtsam::DiscreteKeys); diff --git a/python/gtsam/preamble/geometry.h b/python/gtsam/preamble/geometry.h index f36221d38..40b841abc 100644 --- a/python/gtsam/preamble/geometry.h +++ b/python/gtsam/preamble/geometry.h @@ -10,14 +10,8 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ -#include -PYBIND11_MAKE_OPAQUE( - std::vector>); -PYBIND11_MAKE_OPAQUE(gtsam::Point2Pairs); -PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs); -PYBIND11_MAKE_OPAQUE(gtsam::Pose2Pairs); -PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs); -PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); diff --git a/python/gtsam/preamble/gtsam.h b/python/gtsam/preamble/gtsam.h index ec39c410a..d07a75f6f 100644 --- a/python/gtsam/preamble/gtsam.h +++ b/python/gtsam/preamble/gtsam.h @@ -10,8 +10,3 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ -#ifdef GTSAM_ALLOCATOR_TBB -PYBIND11_MAKE_OPAQUE(std::vector>); -#else -PYBIND11_MAKE_OPAQUE(std::vector); -#endif diff --git a/python/gtsam/preamble/hybrid.h b/python/gtsam/preamble/hybrid.h index 90a062d66..d07a75f6f 100644 --- a/python/gtsam/preamble/hybrid.h +++ b/python/gtsam/preamble/hybrid.h @@ -10,11 +10,3 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ -#include - -// NOTE: Needed since we are including pybind11/stl.h. -#ifdef GTSAM_ALLOCATOR_TBB -PYBIND11_MAKE_OPAQUE(std::vector>); -#else -PYBIND11_MAKE_OPAQUE(std::vector); -#endif diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h index 27a4e5de9..bd71c0817 100644 --- a/python/gtsam/preamble/sfm.h +++ b/python/gtsam/preamble/sfm.h @@ -11,17 +11,3 @@ * mutations on Python side will not be reflected on C++. */ -// Including can cause some serious hard-to-debug bugs!!! -// #include -#include - -PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE( - std::vector>); -PYBIND11_MAKE_OPAQUE( - std::vector>); -PYBIND11_MAKE_OPAQUE( - std::vector); -PYBIND11_MAKE_OPAQUE(gtsam::gtsfm::MatchIndicesMap); \ No newline at end of file diff --git a/python/gtsam/preamble/slam.h b/python/gtsam/preamble/slam.h index 6788bd3c9..d07a75f6f 100644 --- a/python/gtsam/preamble/slam.h +++ b/python/gtsam/preamble/slam.h @@ -10,9 +10,3 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ - -PYBIND11_MAKE_OPAQUE( - std::vector > >); -PYBIND11_MAKE_OPAQUE( - std::vector > >); -PYBIND11_MAKE_OPAQUE(gtsam::Rot3Vector); diff --git a/python/gtsam/specializations/base.h b/python/gtsam/specializations/base.h index 9eefdeca8..22fe3beff 100644 --- a/python/gtsam/specializations/base.h +++ b/python/gtsam/specializations/base.h @@ -11,7 +11,3 @@ * and saves one copy operation. */ -py::bind_map(m_, "IndexPairSetMap"); -py::bind_vector(m_, "IndexPairVector"); - -py::bind_vector >(m_, "JacobianVector"); diff --git a/python/gtsam/specializations/custom.h b/python/gtsam/specializations/custom.h index d46ccdc66..cffb5115f 100644 --- a/python/gtsam/specializations/custom.h +++ b/python/gtsam/specializations/custom.h @@ -9,4 +9,7 @@ * interface, but without the `` copying mechanism. Combined * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, * and saves one copy operation. - */ \ No newline at end of file + */ + +// Added so that CustomFactor can pass the JacobianVector to the C++ side +py::bind_vector >(m_, "JacobianVector"); diff --git a/python/gtsam/specializations/geometry.h b/python/gtsam/specializations/geometry.h index 99f40253f..4aea1b5cf 100644 --- a/python/gtsam/specializations/geometry.h +++ b/python/gtsam/specializations/geometry.h @@ -11,14 +11,6 @@ * and saves one copy operation. */ -py::bind_vector< - std::vector>>( - m_, "Point2Vector"); -py::bind_vector>(m_, "Point2Pairs"); -py::bind_vector>(m_, "Point3Pairs"); -py::bind_vector>(m_, "Pose2Pairs"); -py::bind_vector>(m_, "Pose3Pairs"); -py::bind_vector>(m_, "Pose3Vector"); py::bind_vector>>( m_, "CameraSetCal3_S2"); py::bind_vector>>( diff --git a/python/gtsam/specializations/gtsam.h b/python/gtsam/specializations/gtsam.h index 490d71902..da8842eaf 100644 --- a/python/gtsam/specializations/gtsam.h +++ b/python/gtsam/specializations/gtsam.h @@ -10,11 +10,3 @@ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, * and saves one copy operation. */ - -#ifdef GTSAM_ALLOCATOR_TBB -py::bind_vector > >(m_, "KeyVector"); -py::implicitly_convertible > >(); -#else -py::bind_vector >(m_, "KeyVector"); -py::implicitly_convertible >(); -#endif diff --git a/python/gtsam/specializations/inference.h b/python/gtsam/specializations/inference.h index 9e23444ea..da8842eaf 100644 --- a/python/gtsam/specializations/inference.h +++ b/python/gtsam/specializations/inference.h @@ -10,5 +10,3 @@ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, * and saves one copy operation. */ - -py::bind_map>(m_, "__MapCharDouble"); diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h index c4817f555..22fe3beff 100644 --- a/python/gtsam/specializations/sfm.h +++ b/python/gtsam/specializations/sfm.h @@ -11,18 +11,3 @@ * and saves one copy operation. */ -py::bind_vector > >( - m_, "BinaryMeasurementsPoint3"); -py::bind_vector > >( - m_, "BinaryMeasurementsUnit3"); -py::bind_vector > >( - m_, "BinaryMeasurementsRot3"); -py::bind_map(m_, "KeyPairDoubleMap"); -py::bind_vector>(m_, "SfmTrack2dVector"); -py::bind_vector>(m_, "SfmTracks"); -py::bind_vector>(m_, "SfmCameras"); -py::bind_vector>>( - m_, "SfmMeasurementVector"); - -py::bind_map(m_, "MatchIndicesMap"); -py::bind_vector>(m_, "KeypointsVector"); diff --git a/python/gtsam/specializations/slam.h b/python/gtsam/specializations/slam.h index af579d9df..da8842eaf 100644 --- a/python/gtsam/specializations/slam.h +++ b/python/gtsam/specializations/slam.h @@ -10,11 +10,3 @@ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, * and saves one copy operation. */ - -py::bind_vector< - std::vector>>>( - m_, "BetweenFactorPose3s"); -py::bind_vector< - std::vector>>>( - m_, "BetweenFactorPose2s"); -py::bind_vector(m_, "Rot3Vector"); diff --git a/python/gtsam/symbol_shorthand.py b/python/gtsam/symbol_shorthand.py index 748d36558..5f7866d18 100644 --- a/python/gtsam/symbol_shorthand.py +++ b/python/gtsam/symbol_shorthand.py @@ -1,4 +1,4 @@ # This trick is to allow direct import of sub-modules # without this, we can only do `from gtsam.gtsam.symbol_shorthand import X` # with this trick, we can do `from gtsam.symbol_shorthand import X` -from .gtsam.symbol_shorthand import * \ No newline at end of file +from .gtsam.symbol_shorthand import * diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index e54afc757..50a15dc6e 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -12,10 +12,10 @@ Refactored: Roderick Koehle import unittest import numpy as np +from gtsam.symbol_shorthand import K, L, P +from gtsam.utils.test_case import GtsamTestCase import gtsam -from gtsam.utils.test_case import GtsamTestCase -from gtsam.symbol_shorthand import K, L, P def ulp(ftype=np.float64): @@ -27,7 +27,7 @@ def ulp(ftype=np.float64): class TestCal3Fisheye(GtsamTestCase): - + @classmethod def setUpClass(cls): """ @@ -51,10 +51,10 @@ class TestCal3Fisheye(GtsamTestCase): camera1 = gtsam.PinholeCameraCal3Fisheye(pose1) camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) cls.origin = np.array([0.0, 0.0, 0.0]) - cls.poses = gtsam.Pose3Vector([pose1, pose2]) - cls.cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) - cls.measurements = gtsam.Point2Vector([k.project(cls.origin) for k in cls.cameras]) - + cls.poses = [pose1, pose2] + cls.cameras = [camera1, camera2] + cls.measurements = [k.project(cls.origin) for k in cls.cameras] + def test_Cal3Fisheye(self): K = gtsam.Cal3Fisheye() self.assertEqual(K.fx(), 1.) @@ -63,7 +63,7 @@ class TestCal3Fisheye(GtsamTestCase): def test_distortion(self): """Fisheye distortion and rectification""" equidistant = gtsam.Cal3Fisheye() - perspective_pt = self.obj_point[0:2]/self.obj_point[2] + perspective_pt = self.obj_point[0:2] / self.obj_point[2] distorted_pt = equidistant.uncalibrate(perspective_pt) rectified_pt = equidistant.calibrate(distorted_pt) self.gtsamAssertEquals(distorted_pt, self.img_point) @@ -167,7 +167,7 @@ class TestCal3Fisheye(GtsamTestCase): pose = gtsam.Pose3() camera = gtsam.Cal3Fisheye() state = gtsam.Values() - camera_key, pose_key, landmark_key = K(0), P(0), L(0) + pose_key, landmark_key = P(0), L(0) state.insert_point3(landmark_key, obj_point) state.insert_pose3(pose_key, pose) g = gtsam.NonlinearFactorGraph() @@ -187,7 +187,7 @@ class TestCal3Fisheye(GtsamTestCase): def test_triangulation_rectify(self): """Estimate spatial point from image measurements using rectification""" - rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]) + rectified = [k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)] shared_cal = gtsam.Cal3_S2() triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) self.gtsamAssertEquals(triangulated, self.origin) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index 630109d66..fce741999 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -11,10 +11,10 @@ Author: Frank Dellaert & Duy Nguyen Ta (Python) import unittest import numpy as np +from gtsam.symbol_shorthand import K, L, P +from gtsam.utils.test_case import GtsamTestCase import gtsam -from gtsam.utils.test_case import GtsamTestCase -from gtsam.symbol_shorthand import K, L, P class TestCal3Unified(GtsamTestCase): @@ -48,9 +48,9 @@ class TestCal3Unified(GtsamTestCase): camera1 = gtsam.PinholeCameraCal3Unified(pose1, cls.stereographic) camera2 = gtsam.PinholeCameraCal3Unified(pose2, cls.stereographic) cls.origin = np.array([0.0, 0.0, 0.0]) - cls.poses = gtsam.Pose3Vector([pose1, pose2]) - cls.cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) - cls.measurements = gtsam.Point2Vector([k.project(cls.origin) for k in cls.cameras]) + cls.poses = [pose1, pose2] + cls.cameras = [camera1, camera2] + cls.measurements = [k.project(cls.origin) for k in cls.cameras] def test_Cal3Unified(self): K = gtsam.Cal3Unified() @@ -107,7 +107,7 @@ class TestCal3Unified(GtsamTestCase): state = gtsam.Values() measured = self.img_point noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) - camera_key, pose_key, landmark_key = K(0), P(0), L(0) + camera_key, pose_key, landmark_key = K(0), P(0), L(0) k = self.stereographic state.insert_cal3unified(camera_key, k) state.insert_pose3(pose_key, gtsam.Pose3()) @@ -142,7 +142,7 @@ class TestCal3Unified(GtsamTestCase): Dcal = np.zeros((2, 10), order='F') Dp = np.zeros((2, 2), order='F') camera.calibrate(img_point, Dcal, Dp) - + self.gtsamAssertEquals(Dcal, np.array( [[ 0., 0., 0., -1., 0., 0., 0., 0., 0., 0.], [ 0., 0., 0., 0., -1., 0., 0., 0., 0., 0.]])) @@ -158,7 +158,7 @@ class TestCal3Unified(GtsamTestCase): def test_triangulation_rectify(self): """Estimate spatial point from image measurements using rectification""" - rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]) + rectified = [k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)] shared_cal = gtsam.Cal3_S2() triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) self.gtsamAssertEquals(triangulated, self.origin) diff --git a/python/gtsam/tests/test_DecisionTreeFactor.py b/python/gtsam/tests/test_DecisionTreeFactor.py index 0499e7215..1b8598953 100644 --- a/python/gtsam/tests/test_DecisionTreeFactor.py +++ b/python/gtsam/tests/test_DecisionTreeFactor.py @@ -13,7 +13,8 @@ Author: Frank Dellaert import unittest -from gtsam import DecisionTreeFactor, DiscreteValues, DiscreteDistribution, Ordering +from gtsam import (DecisionTreeFactor, DiscreteDistribution, DiscreteValues, + Ordering) from gtsam.utils.test_case import GtsamTestCase diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 284c3f5cc..be6aa0796 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -5,13 +5,13 @@ Authors: John Lambert import unittest -import gtsam import numpy as np -from gtsam import (IndexPair, KeypointsVector, MatchIndicesMap, Point2, - SfmMeasurementVector, SfmTrack2d) from gtsam.gtsfm import Keypoints from gtsam.utils.test_case import GtsamTestCase +import gtsam +from gtsam import IndexPair, Point2, SfmTrack2d + class TestDsfTrackGenerator(GtsamTestCase): """Tests for DsfTrackGenerator.""" @@ -23,14 +23,14 @@ class TestDsfTrackGenerator(GtsamTestCase): kps_i1 = Keypoints(np.array([[50.0, 60], [70, 80], [90, 100]])) kps_i2 = Keypoints(np.array([[110.0, 120], [130, 140]])) - keypoints_list = KeypointsVector() + keypoints_list = [] keypoints_list.append(kps_i0) keypoints_list.append(kps_i1) keypoints_list.append(kps_i2) # For each image pair (i1,i2), we provide a (K,2) matrix # of corresponding image indices (k1,k2). - matches_dict = MatchIndicesMap() + matches_dict = {} matches_dict[IndexPair(0, 1)] = np.array([[0, 0], [1, 1]]) matches_dict[IndexPair(1, 2)] = np.array([[2, 0], [1, 1]]) @@ -86,7 +86,7 @@ class TestSfmTrack2d(GtsamTestCase): def test_sfm_track_2d_constructor(self) -> None: """Test construction of 2D SfM track.""" - measurements = SfmMeasurementVector() + measurements = [] measurements.append((0, Point2(10, 20))) track = SfmTrack2d(measurements=measurements) track.measurement(0) diff --git a/python/gtsam/tests/test_FixedLagSmootherExample.py b/python/gtsam/tests/test_FixedLagSmootherExample.py index ddd2d49e4..64914ab38 100644 --- a/python/gtsam/tests/test_FixedLagSmootherExample.py +++ b/python/gtsam/tests/test_FixedLagSmootherExample.py @@ -11,10 +11,10 @@ Author: Frank Dellaert & Duy Nguyen Ta (Python) import unittest import numpy as np +from gtsam.utils.test_case import GtsamTestCase import gtsam -import gtsam_unstable -from gtsam.utils.test_case import GtsamTestCase + class TestFixedLagSmootherExample(GtsamTestCase): ''' @@ -36,7 +36,7 @@ class TestFixedLagSmootherExample(GtsamTestCase): # that will be sent to the smoothers new_factors = gtsam.NonlinearFactorGraph() new_values = gtsam.Values() - new_timestamps = gtsam.FixedLagSmootherKeyTimestampMap() + new_timestamps = {} # Create a prior on the first pose, placing it at the origin prior_mean = gtsam.Pose2(0, 0, 0) @@ -47,7 +47,7 @@ class TestFixedLagSmootherExample(GtsamTestCase): gtsam.PriorFactorPose2( X1, prior_mean, prior_noise)) new_values.insert(X1, prior_mean) - new_timestamps.insert((X1, 0.0)) + new_timestamps[X1] = 0.0 delta_time = 0.25 time = 0.25 @@ -77,7 +77,7 @@ class TestFixedLagSmootherExample(GtsamTestCase): current_key = int(1000 * time) # assign current key to the current timestamp - new_timestamps.insert((current_key, time)) + new_timestamps[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] * diff --git a/python/gtsam/tests/test_GaussianFactorGraph.py b/python/gtsam/tests/test_GaussianFactorGraph.py index 09ac4c564..326f62d5a 100644 --- a/python/gtsam/tests/test_GaussianFactorGraph.py +++ b/python/gtsam/tests/test_GaussianFactorGraph.py @@ -14,11 +14,12 @@ 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 +import gtsam + def create_graph(): """Create a basic linear factor graph for testing""" @@ -40,6 +41,7 @@ def create_graph(): class TestGaussianFactorGraph(GtsamTestCase): """Tests for Gaussian Factor Graphs.""" + def test_fg(self): """Test solving a linear factor graph""" graph, X = create_graph() @@ -98,12 +100,11 @@ class TestGaussianFactorGraph(GtsamTestCase): bn = gfg.eliminateSequential(ordering) self.assertEqual(bn.size(), 3) - keyVector = gtsam.KeyVector() - keyVector.append(keys[2]) - #TODO(Varun) Below code causes segfault in Debug config - # ordering = gtsam.Ordering.ColamdConstrainedLastGaussianFactorGraph(gfg, keyVector) - # bn = gfg.eliminateSequential(ordering) - # self.assertEqual(bn.size(), 3) + keyVector = [keys[2]] + ordering = gtsam.Ordering.ColamdConstrainedLastGaussianFactorGraph( + gfg, keyVector) + bn = gfg.eliminateSequential(ordering) + self.assertEqual(bn.size(), 3) if __name__ == '__main__': diff --git a/python/gtsam/tests/test_HybridBayesNet.py b/python/gtsam/tests/test_HybridBayesNet.py index 01e1c5a5d..bc685dec6 100644 --- a/python/gtsam/tests/test_HybridBayesNet.py +++ b/python/gtsam/tests/test_HybridBayesNet.py @@ -17,8 +17,9 @@ import numpy as np from gtsam.symbol_shorthand import A, X from gtsam.utils.test_case import GtsamTestCase -from gtsam import (DiscreteConditional, DiscreteKeys, DiscreteValues, GaussianConditional, - GaussianMixture, HybridBayesNet, HybridValues, noiseModel, VectorValues) +from gtsam import (DiscreteConditional, DiscreteKeys, DiscreteValues, + GaussianConditional, GaussianMixture, HybridBayesNet, + HybridValues, VectorValues, noiseModel) class TestHybridBayesNet(GtsamTestCase): @@ -31,8 +32,8 @@ class TestHybridBayesNet(GtsamTestCase): # Create the continuous conditional I_1x1 = np.eye(1) - conditional = GaussianConditional.FromMeanAndStddev(X(0), 2 * I_1x1, X(1), [-4], - 5.0) + conditional = GaussianConditional.FromMeanAndStddev( + X(0), 2 * I_1x1, X(1), [-4], 5.0) # Create the noise models model0 = noiseModel.Diagonal.Sigmas([2.0]) @@ -47,8 +48,9 @@ class TestHybridBayesNet(GtsamTestCase): # Create hybrid Bayes net. bayesNet = HybridBayesNet() bayesNet.push_back(conditional) - bayesNet.push_back(GaussianMixture( - [X(1)], [], discrete_keys, [conditional0, conditional1])) + bayesNet.push_back( + GaussianMixture([X(1)], [], discrete_keys, + [conditional0, conditional1])) bayesNet.push_back(DiscreteConditional(Asia, "99/1")) # Create values at which to evaluate. @@ -76,7 +78,7 @@ class TestHybridBayesNet(GtsamTestCase): self.check_invariance(bayesNet.at(0).asGaussian(), continuous) self.check_invariance(bayesNet.at(0).asGaussian(), values) self.check_invariance(bayesNet.at(0), values) - + self.check_invariance(bayesNet.at(1), values) self.check_invariance(bayesNet.at(2).asDiscrete(), discrete) @@ -89,7 +91,8 @@ class TestHybridBayesNet(GtsamTestCase): self.assertTrue(probability >= 0.0) logProb = conditional.logProbability(values) self.assertAlmostEqual(probability, np.exp(logProb)) - expected = conditional.logNormalizationConstant() - conditional.error(values) + expected = conditional.logNormalizationConstant() - \ + conditional.error(values) self.assertAlmostEqual(logProb, expected) diff --git a/python/gtsam/tests/test_KarcherMeanFactor.py b/python/gtsam/tests/test_KarcherMeanFactor.py index f4ec64283..0bc942341 100644 --- a/python/gtsam/tests/test_KarcherMeanFactor.py +++ b/python/gtsam/tests/test_KarcherMeanFactor.py @@ -13,15 +13,15 @@ Author: Frank Dellaert import unittest -import gtsam import numpy as np -from gtsam import Rot3 from gtsam.utils.test_case import GtsamTestCase +import gtsam +from gtsam import Rot3 + KEY = 0 MODEL = gtsam.noiseModel.Unit.Create(3) - # Rot3 version R = Rot3.Expmap(np.array([0.1, 0, 0])) @@ -29,9 +29,11 @@ R = 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 = gtsam.Rot3Vector([R, R.inverse()]) + """ + Check that optimizing for Karcher mean (which minimizes Between distance) + gets correct result. + """ + rotations = [R, R.inverse()] expected = Rot3() actual = gtsam.FindKarcherMean(rotations) self.gtsamAssertEquals(expected, actual) @@ -42,7 +44,7 @@ class TestKarcherMean(GtsamTestCase): a2Rb2 = Rot3() a3Rb3 = Rot3() - aRb_list = gtsam.Rot3Vector([a1Rb1, a2Rb2, a3Rb3]) + aRb_list = [a1Rb1, a2Rb2, a3Rb3] aRb_expected = Rot3() aRb = gtsam.FindKarcherMean(aRb_list) @@ -58,9 +60,7 @@ class TestKarcherMean(GtsamTestCase): 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) + keys = [1, 2] graph.add(gtsam.KarcherMeanFactorRot3(keys)) initial = gtsam.Values() @@ -69,11 +69,9 @@ class TestKarcherMean(GtsamTestCase): expected = Rot3() result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() - actual = gtsam.FindKarcherMean( - gtsam.Rot3Vector([result.atRot3(1), result.atRot3(2)])) + actual = gtsam.FindKarcherMean([result.atRot3(1), result.atRot3(2)]) self.gtsamAssertEquals(expected, actual) - self.gtsamAssertEquals( - R12, result.atRot3(1).between(result.atRot3(2))) + self.gtsamAssertEquals(R12, result.atRot3(1).between(result.atRot3(2))) if __name__ == "__main__": diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index d3a51d638..d29448194 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -11,14 +11,15 @@ Author: Frank Dellaert & Duy Nguyen Ta & John Lambert import math import unittest -import gtsam import numpy as np -from gtsam import Point2, Point2Pairs, Pose2 from gtsam.utils.test_case import GtsamTestCase +from gtsam import Point2, Point2Pairs, Pose2 + class TestPose2(GtsamTestCase): """Test selected Pose2 methods.""" + def test_adjoint(self) -> None: """Test adjoint method.""" xi = np.array([1, 2, 3]) @@ -28,7 +29,7 @@ class TestPose2(GtsamTestCase): def test_transformTo(self): """Test transformTo method.""" - pose = Pose2(2, 4, -math.pi/2) + pose = Pose2(2, 4, -math.pi / 2) actual = pose.transformTo(Point2(3, 2)) expected = Point2(2, 1) self.gtsamAssertEquals(actual, expected, 1e-6) @@ -42,7 +43,7 @@ class TestPose2(GtsamTestCase): def test_transformFrom(self): """Test transformFrom method.""" - pose = Pose2(2, 4, -math.pi/2) + pose = Pose2(2, 4, -math.pi / 2) actual = pose.transformFrom(Point2(2, 1)) expected = Point2(3, 2) self.gtsamAssertEquals(actual, expected, 1e-6) @@ -83,7 +84,7 @@ class TestPose2(GtsamTestCase): ] # fmt: on - ab_pairs = Point2Pairs(list(zip(pts_a, pts_b))) + ab_pairs = list(zip(pts_a, pts_b)) aTb = Pose2.Align(ab_pairs) self.assertIsNotNone(aTb) diff --git a/python/gtsam/tests/test_Pose3.py b/python/gtsam/tests/test_Pose3.py index 4464e8d47..32b3ad47f 100644 --- a/python/gtsam/tests/test_Pose3.py +++ b/python/gtsam/tests/test_Pose3.py @@ -13,10 +13,10 @@ import math import unittest import numpy as np +from gtsam.utils.test_case import GtsamTestCase import gtsam -from gtsam import Point3, Pose3, Rot3, Point3Pairs -from gtsam.utils.test_case import GtsamTestCase +from gtsam import Point3, Pose3, Rot3 def numerical_derivative_pose(pose, method, delta=1e-5): @@ -223,7 +223,7 @@ class TestPose3(GtsamTestCase): sTt = Pose3(Rot3.Rodrigues(0, 0, -math.pi), Point3(2, 4, 0)) transformed = sTt.transformTo(square) - st_pairs = Point3Pairs() + st_pairs = [] for j in range(4): st_pairs.append((square[:,j], transformed[:,j])) @@ -237,4 +237,4 @@ class TestPose3(GtsamTestCase): if __name__ == "__main__": - unittest.main() \ No newline at end of file + unittest.main() diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index 19b9f8dc1..845f6cf1c 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -12,20 +12,14 @@ Author: Frank Dellaert import unittest -import gtsam import numpy as np -from gtsam import ( - BetweenFactorPose2, - LevenbergMarquardtParams, - Rot2, - Pose2, - ShonanAveraging2, - ShonanAveragingParameters2, - ShonanAveraging3, - ShonanAveragingParameters3, -) from gtsam.utils.test_case import GtsamTestCase +import gtsam +from gtsam import (BetweenFactorPose2, LevenbergMarquardtParams, Pose2, Rot2, + ShonanAveraging2, ShonanAveraging3, + ShonanAveragingParameters2, ShonanAveragingParameters3) + DEFAULT_PARAMS = ShonanAveragingParameters3( gtsam.LevenbergMarquardtParams.CeresDefaults() ) @@ -146,7 +140,6 @@ class TestShonanAveraging(GtsamTestCase): self.assertAlmostEqual(3.0756, shonan.cost(initial), places=3) result, _lambdaMin = shonan.run(initial, 3, 3) self.assertAlmostEqual(0.0015, shonan.cost(result), places=3) - def test_constructorBetweenFactorPose2s(self) -> None: """Check if ShonanAveraging2 constructor works when not initialized from g2o file. @@ -183,7 +176,7 @@ class TestShonanAveraging(GtsamTestCase): shonan_params.setCertifyOptimality(True) noise_model = gtsam.noiseModel.Unit.Create(3) - between_factors = gtsam.BetweenFactorPose2s() + between_factors = [] for (i1, i2), i2Ri1 in i2Ri1_dict.items(): i2Ti1 = Pose2(i2Ri1, np.zeros(2)) between_factors.append( @@ -196,11 +189,11 @@ class TestShonanAveraging(GtsamTestCase): wRi_list = [result_values.atRot2(i) for i in range(num_images)] thetas_deg = np.array([wRi.degrees() for wRi in wRi_list]) - + # map all angles to [0,360) thetas_deg = thetas_deg % 360 thetas_deg -= thetas_deg[0] - + expected_thetas_deg = np.array([0.0, 90.0, 0.0]) np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1) diff --git a/python/gtsam/tests/test_Sim2.py b/python/gtsam/tests/test_Sim2.py index ea809b965..5501322c3 100644 --- a/python/gtsam/tests/test_Sim2.py +++ b/python/gtsam/tests/test_Sim2.py @@ -12,15 +12,16 @@ Author: John Lambert import unittest import numpy as np -from gtsam import Pose2, Pose2Pairs, Rot2, Similarity2 from gtsam.utils.test_case import GtsamTestCase +from gtsam import Pose2, Rot2, Similarity2 + class TestSim2(GtsamTestCase): """Test selected Sim2 methods.""" def test_align_poses_along_straight_line(self) -> None: - """Test Align Pose2Pairs method. + """Test Align of list of Pose2Pair. Scenario: 3 object poses @@ -46,7 +47,7 @@ class TestSim2(GtsamTestCase): wToi_list = [wTo0, wTo1, wTo2] - we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list))) + we_pairs = list(zip(wToi_list, eToi_list)) # Recover the transformation wSe (i.e. world_S_egovehicle) wSe = Similarity2.Align(we_pairs) @@ -55,7 +56,7 @@ class TestSim2(GtsamTestCase): self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) def test_align_poses_along_straight_line_gauge(self): - """Test if Align Pose3Pairs method can account for gauge ambiguity. + """Test if Pose2 Align method can account for gauge ambiguity. Scenario: 3 object poses @@ -81,7 +82,7 @@ class TestSim2(GtsamTestCase): wToi_list = [wTo0, wTo1, wTo2] - we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list))) + we_pairs = list(zip(wToi_list, eToi_list)) # Recover the transformation wSe (i.e. world_S_egovehicle) wSe = Similarity2.Align(we_pairs) @@ -90,7 +91,7 @@ class TestSim2(GtsamTestCase): self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) def test_align_poses_scaled_squares(self): - """Test if Align Pose2Pairs method can account for gauge ambiguity. + """Test if Align method can account for gauge ambiguity. Make sure a big and small square can be aligned. The u's represent a big square (10x10), and v's represents a small square (4x4). @@ -119,7 +120,7 @@ class TestSim2(GtsamTestCase): bTi_list = [bTi0, bTi1, bTi2, bTi3] - ab_pairs = Pose2Pairs(list(zip(aTi_list, bTi_list))) + ab_pairs = list(zip(aTi_list, bTi_list)) # Recover the transformation wSe (i.e. world_S_egovehicle) aSb = Similarity2.Align(ab_pairs) diff --git a/python/gtsam/tests/test_Sim3.py b/python/gtsam/tests/test_Sim3.py index c00a36435..7723738c8 100644 --- a/python/gtsam/tests/test_Sim3.py +++ b/python/gtsam/tests/test_Sim3.py @@ -13,17 +13,17 @@ import unittest from typing import List, Optional import numpy as np +from gtsam.utils.test_case import GtsamTestCase import gtsam -from gtsam import Point3, Pose3, Pose3Pairs, Rot3, Similarity3 -from gtsam.utils.test_case import GtsamTestCase +from gtsam import Point3, Pose3, Rot3, Similarity3 class TestSim3(GtsamTestCase): """Test selected Sim3 methods.""" def test_align_poses_along_straight_line(self): - """Test Align Pose3Pairs method. + """Test Pose3 Align method. Scenario: 3 object poses @@ -49,7 +49,7 @@ class TestSim3(GtsamTestCase): wToi_list = [wTo0, wTo1, wTo2] - we_pairs = Pose3Pairs(list(zip(wToi_list, eToi_list))) + we_pairs = list(zip(wToi_list, eToi_list)) # Recover the transformation wSe (i.e. world_S_egovehicle) wSe = Similarity3.Align(we_pairs) @@ -58,7 +58,7 @@ class TestSim3(GtsamTestCase): self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) def test_align_poses_along_straight_line_gauge(self): - """Test if Align Pose3Pairs method can account for gauge ambiguity. + """Test if Pose3 Align method can account for gauge ambiguity. Scenario: 3 object poses @@ -84,7 +84,7 @@ class TestSim3(GtsamTestCase): wToi_list = [wTo0, wTo1, wTo2] - we_pairs = Pose3Pairs(list(zip(wToi_list, eToi_list))) + we_pairs = list(zip(wToi_list, eToi_list)) # Recover the transformation wSe (i.e. world_S_egovehicle) wSe = Similarity3.Align(we_pairs) @@ -93,7 +93,7 @@ class TestSim3(GtsamTestCase): self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) def test_align_poses_scaled_squares(self): - """Test if Align Pose3Pairs method can account for gauge ambiguity. + """Test if Pose3 Align method can account for gauge ambiguity. Make sure a big and small square can be aligned. The u's represent a big square (10x10), and v's represents a small square (4x4). @@ -122,7 +122,7 @@ class TestSim3(GtsamTestCase): bTi_list = [bTi0, bTi1, bTi2, bTi3] - ab_pairs = Pose3Pairs(list(zip(aTi_list, bTi_list))) + ab_pairs = list(zip(aTi_list, bTi_list)) # Recover the transformation wSe (i.e. world_S_egovehicle) aSb = Similarity3.Align(ab_pairs) @@ -689,7 +689,7 @@ def align_poses_sim3(aTi_list: List[Pose3], bTi_list: List[Pose3]) -> Similarity assert len(aTi_list) == len(bTi_list) assert n_to_align >= 2, "SIM(3) alignment uses at least 2 frames" - ab_pairs = Pose3Pairs(list(zip(aTi_list, bTi_list))) + ab_pairs = list(zip(aTi_list, bTi_list)) aSb = Similarity3.Align(ab_pairs) diff --git a/python/gtsam/tests/test_TranslationRecovery.py b/python/gtsam/tests/test_TranslationRecovery.py index 99fbce89e..d8e061435 100644 --- a/python/gtsam/tests/test_TranslationRecovery.py +++ b/python/gtsam/tests/test_TranslationRecovery.py @@ -1,25 +1,25 @@ -from __future__ import print_function - -import numpy as np import unittest import gtsam +import numpy as np + -""" Returns example pose values of 3 points A, B and C in the world frame """ def ExampleValues(): + """ Returns example pose values of 3 points A, B and C in the world frame """ T = [] T.append(gtsam.Point3(np.array([3.14, 1.59, 2.65]))) T.append(gtsam.Point3(np.array([-1.0590e+00, -3.6017e-02, -1.5720e+00]))) T.append(gtsam.Point3(np.array([8.5034e+00, 6.7499e+00, -3.6383e+00]))) - + data = gtsam.Values() - for i in range(len(T)): - data.insert(i, gtsam.Pose3(gtsam.Rot3(), T[i])) + for i, t in enumerate(T): + data.insert(i, gtsam.Pose3(gtsam.Rot3(), t)) return data -""" Returns binary measurements for the points in the given edges.""" + def SimulateMeasurements(gt_poses, graph_edges): - measurements = gtsam.BinaryMeasurementsUnit3() + """ Returns binary measurements for the points in the given edges.""" + measurements = [] for edge in graph_edges: Ta = gt_poses.atPose3(edge[0]).translation() Tb = gt_poses.atPose3(edge[1]).translation() @@ -28,18 +28,20 @@ def SimulateMeasurements(gt_poses, graph_edges): gtsam.noiseModel.Isotropic.Sigma(3, 0.01))) return measurements -""" Tests for the translation recovery class """ + class TestTranslationRecovery(unittest.TestCase): - """Test selected Translation Recovery methods.""" + """ Tests for the translation recovery class.""" def test_constructor(self): """Construct from binary measurements.""" algorithm = gtsam.TranslationRecovery() self.assertIsInstance(algorithm, gtsam.TranslationRecovery) - algorithm_params = gtsam.TranslationRecovery(gtsam.LevenbergMarquardtParams()) + algorithm_params = gtsam.TranslationRecovery( + gtsam.LevenbergMarquardtParams()) self.assertIsInstance(algorithm_params, gtsam.TranslationRecovery) def test_run(self): + """Test selected Translation Recovery methods.""" gt_poses = ExampleValues() measurements = SimulateMeasurements(gt_poses, [[0, 1], [0, 2], [1, 2]]) @@ -51,15 +53,23 @@ class TestTranslationRecovery(unittest.TestCase): scale = 2.0 result = algorithm.run(measurements, scale) - w_aTc = gt_poses.atPose3(2).translation() - gt_poses.atPose3(0).translation() - w_aTb = gt_poses.atPose3(1).translation() - gt_poses.atPose3(0).translation() - w_aTc_expected = w_aTc*scale/np.linalg.norm(w_aTb) - w_aTb_expected = w_aTb*scale/np.linalg.norm(w_aTb) + w_aTc = gt_poses.atPose3(2).translation() - gt_poses.atPose3( + 0).translation() + w_aTb = gt_poses.atPose3(1).translation() - gt_poses.atPose3( + 0).translation() + w_aTc_expected = w_aTc * scale / np.linalg.norm(w_aTb) + w_aTb_expected = w_aTb * scale / np.linalg.norm(w_aTb) + + np.testing.assert_array_almost_equal(result.atPoint3(0), + np.array([0, 0, 0]), 6, + "Origin result is incorrect.") + np.testing.assert_array_almost_equal(result.atPoint3(1), + w_aTb_expected, 6, + "Point B result is incorrect.") + np.testing.assert_array_almost_equal(result.atPoint3(2), + w_aTc_expected, 6, + "Point C result is incorrect.") - np.testing.assert_array_almost_equal(result.atPoint3(0), np.array([0,0,0]), 6, "Origin result is incorrect.") - np.testing.assert_array_almost_equal(result.atPoint3(1), w_aTb_expected, 6, "Point B result is incorrect.") - np.testing.assert_array_almost_equal(result.atPoint3(2), w_aTc_expected, 6, "Point C result is incorrect.") if __name__ == "__main__": unittest.main() - diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index 8630e1da7..8e245093b 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -11,15 +11,15 @@ Authors: Frank Dellaert & Fan Jiang (Python) & Sushmita Warrier & John Lambert # pylint: disable=no-name-in-module, invalid-name, no-member import unittest from typing import Iterable, List, Optional, Tuple, Union + import numpy as np +from gtsam.utils.test_case import GtsamTestCase import gtsam from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2, CameraSetCal3Bundler, PinholeCameraCal3_S2, - PinholeCameraCal3Bundler, Point2, Point2Vector, Point3, - Pose3, Pose3Vector, Rot3, TriangulationParameters, - TriangulationResult) -from gtsam.utils.test_case import GtsamTestCase + PinholeCameraCal3Bundler, Point2, Point3, Pose3, Rot3, + TriangulationParameters, TriangulationResult) UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2) @@ -35,9 +35,7 @@ class TestTriangulationExample(GtsamTestCase): # create second camera 1 meter to the right of first camera pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))) # twoPoses - self.poses = Pose3Vector() - self.poses.append(pose1) - self.poses.append(pose2) + self.poses = [pose1, pose2] # landmark ~5 meters infront of camera self.landmark = Point3(5, 0.5, 1.2) @@ -49,7 +47,7 @@ class TestTriangulationExample(GtsamTestCase): cal_params: Iterable[Iterable[Union[int, float]]], camera_set: Optional[Union[CameraSetCal3Bundler, CameraSetCal3_S2]] = None, - ) -> Tuple[Point2Vector, Union[CameraSetCal3Bundler, CameraSetCal3_S2, + ) -> Tuple[List[Point2], Union[CameraSetCal3Bundler, CameraSetCal3_S2, List[Cal3Bundler], List[Cal3_S2]]]: """ Generate vector of measurements for given calibration and camera model. @@ -67,7 +65,7 @@ class TestTriangulationExample(GtsamTestCase): cameras = camera_set() else: cameras = [] - measurements = Point2Vector() + measurements = [] for k, pose in zip(cal_params, self.poses): K = calibration(*k) @@ -96,7 +94,7 @@ class TestTriangulationExample(GtsamTestCase): self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) # Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) - measurements_noisy = Point2Vector() + measurements_noisy = [] measurements_noisy.append(measurements[0] - np.array([0.1, 0.5])) measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3])) @@ -163,8 +161,8 @@ class TestTriangulationExample(GtsamTestCase): z2: Point2 = camera2.project(landmark) z3: Point2 = camera3.project(landmark) - poses = gtsam.Pose3Vector([pose1, pose2, pose3]) - measurements = Point2Vector([z1, z2, z3]) + poses = [pose1, pose2, pose3] + measurements = [z1, z2, z3] # noise free, so should give exactly the landmark actual = gtsam.triangulatePoint3(poses, @@ -226,15 +224,15 @@ class TestTriangulationExample(GtsamTestCase): z2 = camera2.project(self.landmark) cameras = CameraSetCal3_S2() - measurements = Point2Vector() - cameras.append(camera1) cameras.append(camera2) + + measurements = [] measurements.append(z1) measurements.append(z2) landmarkDistanceThreshold = 10 # landmark is closer than that - # all default except landmarkDistanceThreshold: + # all default except landmarkDistanceThreshold: params = TriangulationParameters(1.0, False, landmarkDistanceThreshold) actual: TriangulationResult = gtsam.triangulateSafe( cameras, measurements, params) @@ -242,8 +240,8 @@ class TestTriangulationExample(GtsamTestCase): self.assertTrue(actual.valid()) landmarkDistanceThreshold = 4 # landmark is farther than that - params2 = TriangulationParameters( - 1.0, False, landmarkDistanceThreshold) + params2 = TriangulationParameters(1.0, False, + landmarkDistanceThreshold) actual = gtsam.triangulateSafe(cameras, measurements, params2) self.assertTrue(actual.farPoint()) @@ -258,15 +256,17 @@ class TestTriangulationExample(GtsamTestCase): measurements.append(z3 + Point2(10, -10)) landmarkDistanceThreshold = 10 # landmark is closer than that - outlierThreshold = 100 # loose, the outlier is going to pass - params3 = TriangulationParameters(1.0, False, landmarkDistanceThreshold, + outlierThreshold = 100 # loose, the outlier is going to pass + params3 = TriangulationParameters(1.0, False, + landmarkDistanceThreshold, outlierThreshold) actual = gtsam.triangulateSafe(cameras, measurements, params3) self.assertTrue(actual.valid()) # now set stricter threshold for outlier rejection outlierThreshold = 5 # tighter, the outlier is not going to pass - params4 = TriangulationParameters(1.0, False, landmarkDistanceThreshold, + params4 = TriangulationParameters(1.0, False, + landmarkDistanceThreshold, outlierThreshold) actual = gtsam.triangulateSafe(cameras, measurements, params4) self.assertTrue(actual.outlier()) diff --git a/python/gtsam/tests/test_VisualISAMExample.py b/python/gtsam/tests/test_VisualISAMExample.py index ebc77e2e3..39f563a35 100644 --- a/python/gtsam/tests/test_VisualISAMExample.py +++ b/python/gtsam/tests/test_VisualISAMExample.py @@ -6,24 +6,29 @@ All Rights Reserved See LICENSE for the license information visual_isam unit tests. -Author: Frank Dellaert & Duy Nguyen Ta (Python) +Author: Frank Dellaert & Duy Nguyen Ta & Varun Agrawal (Python) """ +# pylint: disable=maybe-no-member,invalid-name + import unittest 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 +import gtsam +from gtsam import symbol + class TestVisualISAMExample(GtsamTestCase): """Test class for ISAM2 with visual landmarks.""" - def test_VisualISAMExample(self): - """Test to see if ISAM works as expected for a simple visual SLAM example.""" + + def setUp(self): # Data Options options = generator.Options() options.triangle = False options.nrCameras = 20 + self.options = options # iSAM Options isamOptions = visual_isam.Options() @@ -32,26 +37,82 @@ class TestVisualISAMExample(GtsamTestCase): isamOptions.batchInitialization = True isamOptions.reorderInterval = 10 isamOptions.alwaysRelinearize = False + self.isamOptions = isamOptions # Generate data - data, truth = generator.generate_data(options) + self.data, self.truth = generator.generate_data(options) + def test_VisualISAMExample(self): + """Test to see if ISAM works as expected for a simple visual SLAM example.""" # Initialize iSAM with the first pose and points isam, result, nextPose = visual_isam.initialize( - data, truth, isamOptions) + self.data, self.truth, self.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 currentPose in range(nextPose, self.options.nrCameras): + isam, result = visual_isam.step(self.data, isam, result, + self.truth, currentPose) - for i, _ in enumerate(truth.cameras): + for i, true_camera in enumerate(self.truth.cameras): pose_i = result.atPose3(symbol('x', i)) - self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) + self.gtsamAssertEquals(pose_i, true_camera.pose(), 1e-5) - for j, _ in enumerate(truth.points): + for j, expected_point in enumerate(self.truth.points): point_j = result.atPoint3(symbol('l', j)) - self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) + self.gtsamAssertEquals(point_j, expected_point, 1e-5) + + def test_isam2_error(self): + """Test for isam2 error() method.""" + # Initialize iSAM with the first pose and points + isam, result, nextPose = visual_isam.initialize( + self.data, self.truth, self.isamOptions) + + # Main loop for iSAM: stepping through all poses + for currentPose in range(nextPose, self.options.nrCameras): + isam, result = visual_isam.step(self.data, isam, result, + self.truth, currentPose) + + values = gtsam.VectorValues() + + estimate = isam.calculateBestEstimate() + + for key in estimate.keys(): + try: + v = gtsam.Pose3.Logmap(estimate.atPose3(key)) + except RuntimeError: + v = estimate.atPoint3(key) + + values.insert(key, v) + + self.assertAlmostEqual(isam.error(values), 34212421.14731998) + + def test_isam2_update(self): + """ + Test for full version of ISAM2::update method + """ + # Initialize iSAM with the first pose and points + isam, result, nextPose = visual_isam.initialize( + self.data, self.truth, self.isamOptions) + + remove_factor_indices = [] + constrained_keys = gtsam.KeyGroupMap() + no_relin_keys = gtsam.KeyList() + extra_reelim_keys = gtsam.KeyList() + isamArgs = (remove_factor_indices, constrained_keys, no_relin_keys, + extra_reelim_keys, False) + + # Main loop for iSAM: stepping through all poses + for currentPose in range(nextPose, self.options.nrCameras): + isam, result = visual_isam.step(self.data, isam, result, + self.truth, currentPose, isamArgs) + + for i in range(len(self.truth.cameras)): + pose_i = result.atPose3(symbol('x', i)) + self.gtsamAssertEquals(pose_i, self.truth.cameras[i].pose(), 1e-5) + + for j in range(len(self.truth.points)): + point_j = result.atPoint3(symbol('l', j)) + self.gtsamAssertEquals(point_j, self.truth.points[j], 1e-5) if __name__ == "__main__": diff --git a/python/gtsam/tests/test_backwards_compatibility.py b/python/gtsam/tests/test_backwards_compatibility.py new file mode 100644 index 000000000..414b65e8c --- /dev/null +++ b/python/gtsam/tests/test_backwards_compatibility.py @@ -0,0 +1,897 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests to ensure backwards compatibility of the Python wrapper. +Author: Varun Agrawal +""" +import unittest +from typing import Iterable, List, Optional, Tuple, Union + +import numpy as np +from gtsam.gtsfm import Keypoints +from gtsam.symbol_shorthand import X +from gtsam.utils.test_case import GtsamTestCase + +import gtsam +from gtsam import (BetweenFactorPose2, Cal3_S2, Cal3Bundler, CameraSetCal3_S2, + CameraSetCal3Bundler, IndexPair, LevenbergMarquardtParams, + PinholeCameraCal3_S2, PinholeCameraCal3Bundler, Point2, + Point2Pairs, Point3, Pose2, Pose2Pairs, Pose3, Rot2, Rot3, + SfmTrack2d, ShonanAveraging2, ShonanAveragingParameters2, + Similarity2, Similarity3, TriangulationParameters, + TriangulationResult) + +UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2) + + +class TestBackwardsCompatibility(GtsamTestCase): + """Tests for the backwards compatibility for the Python wrapper.""" + + def setUp(self): + """Setup test fixtures""" + p1 = [-1.0, 0.0, -1.0] + p2 = [1.0, 0.0, -1.0] + q1 = Rot3(1.0, 0.0, 0.0, 0.0) + q2 = Rot3(1.0, 0.0, 0.0, 0.0) + pose1 = Pose3(q1, p1) + pose2 = Pose3(q2, p2) + camera1 = gtsam.PinholeCameraCal3Fisheye(pose1) + camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) + self.origin = np.array([0.0, 0.0, 0.0]) + self.poses = gtsam.Pose3Vector([pose1, pose2]) + + self.fisheye_cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) + self.fisheye_measurements = gtsam.Point2Vector( + [k.project(self.origin) for k in self.fisheye_cameras]) + + fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 + k1, k2, p1, p2 = 0, 0, 0, 0 + xi = 1 + self.stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, + p2, xi) + camera1 = gtsam.PinholeCameraCal3Unified(pose1, self.stereographic) + camera2 = gtsam.PinholeCameraCal3Unified(pose2, self.stereographic) + self.unified_cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) + self.unified_measurements = gtsam.Point2Vector( + [k.project(self.origin) for k in self.unified_cameras]) + + ## Set up two camera poses + # Looking along X-axis, 1 meter above ground plane (x-y) + pose1 = Pose3(UPRIGHT, Point3(0, 0, 1)) + + # create second camera 1 meter to the right of first camera + pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))) + # twoPoses + self.triangulation_poses = gtsam.Pose3Vector() + self.triangulation_poses.append(pose1) + self.triangulation_poses.append(pose2) + + # landmark ~5 meters infront of camera + self.landmark = Point3(5, 0.5, 1.2) + + def test_Cal3Fisheye_triangulation_rectify(self): + """ + Estimate spatial point from image measurements using + rectification from a Cal3Fisheye camera model. + """ + rectified = gtsam.Point2Vector([ + k.calibration().calibrate(pt) + for k, pt in zip(self.fisheye_cameras, self.fisheye_measurements) + ]) + shared_cal = gtsam.Cal3_S2() + triangulated = gtsam.triangulatePoint3(self.poses, + shared_cal, + rectified, + rank_tol=1e-9, + optimize=False) + self.gtsamAssertEquals(triangulated, self.origin) + + def test_Cal3Unified_triangulation_rectify(self): + """ + Estimate spatial point from image measurements using + rectification from a Cal3Unified camera model. + """ + rectified = gtsam.Point2Vector([ + k.calibration().calibrate(pt) + for k, pt in zip(self.unified_cameras, self.unified_measurements) + ]) + shared_cal = gtsam.Cal3_S2() + triangulated = gtsam.triangulatePoint3(self.poses, + shared_cal, + rectified, + rank_tol=1e-9, + optimize=False) + self.gtsamAssertEquals(triangulated, self.origin) + + def test_track_generation(self) -> None: + """Ensures that DSF generates three tracks from measurements + in 3 images (H=200,W=400).""" + kps_i0 = Keypoints(np.array([[10.0, 20], [30, 40]])) + kps_i1 = Keypoints(np.array([[50.0, 60], [70, 80], [90, 100]])) + kps_i2 = Keypoints(np.array([[110.0, 120], [130, 140]])) + + keypoints_list = gtsam.KeypointsVector() + keypoints_list.append(kps_i0) + keypoints_list.append(kps_i1) + keypoints_list.append(kps_i2) + + # For each image pair (i1,i2), we provide a (K,2) matrix + # of corresponding image indices (k1,k2). + matches_dict = gtsam.MatchIndicesMap() + matches_dict[IndexPair(0, 1)] = np.array([[0, 0], [1, 1]]) + matches_dict[IndexPair(1, 2)] = np.array([[2, 0], [1, 1]]) + + tracks = gtsam.gtsfm.tracksFromPairwiseMatches( + matches_dict, + keypoints_list, + verbose=False, + ) + assert len(tracks) == 3 + + # Verify track 0. + track0 = tracks[0] + assert track0.numberMeasurements() == 2 + np.testing.assert_allclose(track0.measurements[0][1], Point2(10, 20)) + np.testing.assert_allclose(track0.measurements[1][1], Point2(50, 60)) + assert track0.measurements[0][0] == 0 + assert track0.measurements[1][0] == 1 + np.testing.assert_allclose( + track0.measurementMatrix(), + [ + [10, 20], + [50, 60], + ], + ) + np.testing.assert_allclose(track0.indexVector(), [0, 1]) + + # Verify track 1. + track1 = tracks[1] + np.testing.assert_allclose( + track1.measurementMatrix(), + [ + [30, 40], + [70, 80], + [130, 140], + ], + ) + np.testing.assert_allclose(track1.indexVector(), [0, 1, 2]) + + # Verify track 2. + track2 = tracks[2] + np.testing.assert_allclose( + track2.measurementMatrix(), + [ + [90, 100], + [110, 120], + ], + ) + np.testing.assert_allclose(track2.indexVector(), [1, 2]) + + def test_sfm_track_2d_constructor(self) -> None: + """Test construction of 2D SfM track.""" + measurements = gtsam.SfmMeasurementVector() + measurements.append((0, Point2(10, 20))) + track = SfmTrack2d(measurements=measurements) + track.measurement(0) + assert track.numberMeasurements() == 1 + + 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.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.FixedLagSmootherKeyTimestampMap() + + # Create a prior on the first pose, placing it at the origin + prior_mean = 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 = [ + Pose2(0.995821, 0.0231012, 0.0300001), + Pose2(1.49284, 0.0457247, 0.045), + Pose2(1.98981, 0.0758879, 0.06), + Pose2(2.48627, 0.113502, 0.075), + Pose2(2.98211, 0.158558, 0.09), + Pose2(3.47722, 0.211047, 0.105), + Pose2(3.97149, 0.270956, 0.12), + Pose2(4.4648, 0.338272, 0.135), + Pose2(4.95705, 0.41298, 0.15), + Pose2(5.44812, 0.495063, 0.165), + 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 = 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 = 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 = 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 + + def test_ordering(self): + """Test ordering""" + gfg = 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)) + + gfg.add(x1, np.eye(1), x0, -np.eye(1), np.ones(1), BETWEEN_NOISE) + gfg.add(x2, np.eye(1), x1, -np.eye(1), 2 * np.ones(1), BETWEEN_NOISE) + gfg.add(x0, np.eye(1), np.zeros(1), PRIOR_NOISE) + + keys = (x0, x1, x2) + ordering = gtsam.Ordering() + for key in keys[::-1]: + ordering.push_back(key) + + bn = gfg.eliminateSequential(ordering) + self.assertEqual(bn.size(), 3) + + keyVector = gtsam.KeyVector() + keyVector.append(keys[2]) + ordering = gtsam.Ordering.ColamdConstrainedLastGaussianFactorGraph( + gfg, keyVector) + bn = gfg.eliminateSequential(ordering) + self.assertEqual(bn.size(), 3) + + def test_find(self): + """ + Check that optimizing for Karcher mean (which minimizes Between distance) + gets correct result. + """ + R = Rot3.Expmap(np.array([0.1, 0, 0])) + + rotations = gtsam.Rot3Vector([R, R.inverse()]) + expected = Rot3() + actual = gtsam.FindKarcherMean(rotations) + self.gtsamAssertEquals(expected, actual) + + def test_find_karcher_mean_identity(self): + """Averaging 3 identity rotations should yield the identity.""" + a1Rb1 = Rot3() + a2Rb2 = Rot3() + a3Rb3 = Rot3() + + aRb_list = gtsam.Rot3Vector([a1Rb1, a2Rb2, a3Rb3]) + aRb_expected = Rot3() + + aRb = gtsam.FindKarcherMean(aRb_list) + self.gtsamAssertEquals(aRb, aRb_expected) + + 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. + R = Rot3.Expmap(np.array([0.1, 0, 0])) + MODEL = gtsam.noiseModel.Unit.Create(3) + + 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 = Rot3() + + result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() + actual = gtsam.FindKarcherMean( + gtsam.Rot3Vector([result.atRot3(1), + result.atRot3(2)])) + self.gtsamAssertEquals(expected, actual) + self.gtsamAssertEquals(R12, result.atRot3(1).between(result.atRot3(2))) + + def test_align(self) -> None: + """Ensure estimation of the Pose2 element to align two 2d point clouds succeeds. + + Two point clouds represent horseshoe-shapes of the same size, just rotated and translated: + + | X---X + | | + | X---X + ------------------ + | + | + O | O + | | | + O---O + """ + pts_a = [ + Point2(1, -3), + Point2(1, -5), + Point2(-1, -5), + Point2(-1, -3), + ] + pts_b = [ + Point2(3, 1), + Point2(1, 1), + Point2(1, 3), + Point2(3, 3), + ] + + ab_pairs = Point2Pairs(list(zip(pts_a, pts_b))) + aTb = Pose2.Align(ab_pairs) + self.assertIsNotNone(aTb) + + for pt_a, pt_b in zip(pts_a, pts_b): + pt_a_ = aTb.transformFrom(pt_b) + np.testing.assert_allclose(pt_a, pt_a_) + + # Matrix version + A = np.array(pts_a).T + B = np.array(pts_b).T + aTb = Pose2.Align(A, B) + self.assertIsNotNone(aTb) + + for pt_a, pt_b in zip(pts_a, pts_b): + pt_a_ = aTb.transformFrom(pt_b) + np.testing.assert_allclose(pt_a, pt_a_) + + def test_align_squares(self): + """Test if Align method can align 2 squares.""" + square = np.array([[0, 0, 0], [0, 1, 0], [1, 1, 0], [1, 0, 0]], + float).T + sTt = Pose3(Rot3.Rodrigues(0, 0, -np.pi), gtsam.Point3(2, 4, 0)) + transformed = sTt.transformTo(square) + + st_pairs = gtsam.Point3Pairs() + for j in range(4): + st_pairs.append((square[:, j], transformed[:, j])) + + # Recover the transformation sTt + estimated_sTt = Pose3.Align(st_pairs) + self.gtsamAssertEquals(estimated_sTt, sTt, 1e-10) + + # Matrix version + estimated_sTt = Pose3.Align(square, transformed) + self.gtsamAssertEquals(estimated_sTt, sTt, 1e-10) + + def test_constructorBetweenFactorPose2s(self) -> None: + """Check if ShonanAveraging2 constructor works when not initialized from g2o file. + + GT pose graph: + + | cam 1 = (0,4) + --o + | . + . . + . . + | | + o-- ... o-- + cam 0 cam 2 = (4,0) + (0,0) + """ + num_images = 3 + + wTi_list = [ + Pose2(Rot2.fromDegrees(0), np.array([0, 0])), + Pose2(Rot2.fromDegrees(90), np.array([0, 4])), + Pose2(Rot2.fromDegrees(0), np.array([4, 0])), + ] + + edges = [(0, 1), (1, 2), (0, 2)] + i2Ri1_dict = {(i1, i2): + wTi_list[i2].inverse().compose(wTi_list[i1]).rotation() + for (i1, i2) in edges} + + lm_params = LevenbergMarquardtParams.CeresDefaults() + shonan_params = ShonanAveragingParameters2(lm_params) + shonan_params.setUseHuber(False) + shonan_params.setCertifyOptimality(True) + + noise_model = gtsam.noiseModel.Unit.Create(3) + between_factors = gtsam.BetweenFactorPose2s() + for (i1, i2), i2Ri1 in i2Ri1_dict.items(): + i2Ti1 = Pose2(i2Ri1, np.zeros(2)) + between_factors.append( + BetweenFactorPose2(i2, i1, i2Ti1, noise_model)) + + obj = ShonanAveraging2(between_factors, shonan_params) + initial = obj.initializeRandomly() + result_values, _ = obj.run(initial, min_p=2, max_p=100) + + wRi_list = [result_values.atRot2(i) for i in range(num_images)] + thetas_deg = np.array([wRi.degrees() for wRi in wRi_list]) + + # map all angles to [0,360) + thetas_deg = thetas_deg % 360 + thetas_deg -= thetas_deg[0] + + expected_thetas_deg = np.array([0.0, 90.0, 0.0]) + np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1) + + def test_align_poses2_along_straight_line(self) -> None: + """Test Align of list of Pose2Pair. + + Scenario: + 3 object poses + same scale (no gauge ambiguity) + world frame has poses rotated about 180 degrees. + world and egovehicle frame translated by 15 meters w.r.t. each other + """ + R180 = Rot2.fromDegrees(180) + + # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame) + # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame + eTo0 = Pose2(Rot2(), np.array([5, 0])) + eTo1 = Pose2(Rot2(), np.array([10, 0])) + eTo2 = Pose2(Rot2(), np.array([15, 0])) + + eToi_list = [eTo0, eTo1, eTo2] + + # Create destination poses + # (same three objects, but instead living in the world "w" frame) + wTo0 = Pose2(R180, np.array([-10, 0])) + wTo1 = Pose2(R180, np.array([-5, 0])) + wTo2 = Pose2(R180, np.array([0, 0])) + + wToi_list = [wTo0, wTo1, wTo2] + + we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list))) + + # Recover the transformation wSe (i.e. world_S_egovehicle) + wSe = Similarity2.Align(we_pairs) + + for wToi, eToi in zip(wToi_list, eToi_list): + self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) + + def test_align_poses2_along_straight_line_gauge(self): + """Test if Align Pose2Pairs method can account for gauge ambiguity. + + Scenario: + 3 object poses + with gauge ambiguity (2x scale) + world frame has poses rotated by 90 degrees. + world and egovehicle frame translated by 11 meters w.r.t. each other + """ + R90 = Rot2.fromDegrees(90) + + # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame) + # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame + eTo0 = Pose2(Rot2(), np.array([1, 0])) + eTo1 = Pose2(Rot2(), np.array([2, 0])) + eTo2 = Pose2(Rot2(), np.array([4, 0])) + + eToi_list = [eTo0, eTo1, eTo2] + + # Create destination poses + # (same three objects, but instead living in the world/city "w" frame) + wTo0 = Pose2(R90, np.array([0, 12])) + wTo1 = Pose2(R90, np.array([0, 14])) + wTo2 = Pose2(R90, np.array([0, 18])) + + wToi_list = [wTo0, wTo1, wTo2] + + we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list))) + + # Recover the transformation wSe (i.e. world_S_egovehicle) + wSe = Similarity2.Align(we_pairs) + + for wToi, eToi in zip(wToi_list, eToi_list): + self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) + + def test_align_poses2_scaled_squares(self): + """Test if Align Pose2Pairs method can account for gauge ambiguity. + + Make sure a big and small square can be aligned. + The u's represent a big square (10x10), and v's represents a small square (4x4). + + Scenario: + 4 object poses + with gauge ambiguity (2.5x scale) + """ + # 0, 90, 180, and 270 degrees yaw + R0 = Rot2.fromDegrees(0) + R90 = Rot2.fromDegrees(90) + R180 = Rot2.fromDegrees(180) + R270 = Rot2.fromDegrees(270) + + aTi0 = Pose2(R0, np.array([2, 3])) + aTi1 = Pose2(R90, np.array([12, 3])) + aTi2 = Pose2(R180, np.array([12, 13])) + aTi3 = Pose2(R270, np.array([2, 13])) + + aTi_list = [aTi0, aTi1, aTi2, aTi3] + + bTi0 = Pose2(R0, np.array([4, 3])) + bTi1 = Pose2(R90, np.array([8, 3])) + bTi2 = Pose2(R180, np.array([8, 7])) + bTi3 = Pose2(R270, np.array([4, 7])) + + bTi_list = [bTi0, bTi1, bTi2, bTi3] + + ab_pairs = Pose2Pairs(list(zip(aTi_list, bTi_list))) + + # Recover the transformation wSe (i.e. world_S_egovehicle) + aSb = Similarity2.Align(ab_pairs) + + for aTi, bTi in zip(aTi_list, bTi_list): + self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi)) + + def test_align_poses3_along_straight_line(self): + """Test Align Pose3Pairs method. + + Scenario: + 3 object poses + same scale (no gauge ambiguity) + world frame has poses rotated about x-axis (90 degree roll) + world and egovehicle frame translated by 15 meters w.r.t. each other + """ + Rx90 = Rot3.Rx(np.deg2rad(90)) + + # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame) + # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame + eTo0 = Pose3(Rot3(), np.array([5, 0, 0])) + eTo1 = Pose3(Rot3(), np.array([10, 0, 0])) + eTo2 = Pose3(Rot3(), np.array([15, 0, 0])) + + eToi_list = [eTo0, eTo1, eTo2] + + # Create destination poses + # (same three objects, but instead living in the world/city "w" frame) + wTo0 = Pose3(Rx90, np.array([-10, 0, 0])) + wTo1 = Pose3(Rx90, np.array([-5, 0, 0])) + wTo2 = Pose3(Rx90, np.array([0, 0, 0])) + + wToi_list = [wTo0, wTo1, wTo2] + + we_pairs = gtsam.Pose3Pairs(list(zip(wToi_list, eToi_list))) + + # Recover the transformation wSe (i.e. world_S_egovehicle) + wSe = Similarity3.Align(we_pairs) + + for wToi, eToi in zip(wToi_list, eToi_list): + self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) + + def test_align_poses3_along_straight_line_gauge(self): + """Test if Align Pose3Pairs method can account for gauge ambiguity. + + Scenario: + 3 object poses + with gauge ambiguity (2x scale) + world frame has poses rotated about z-axis (90 degree yaw) + world and egovehicle frame translated by 11 meters w.r.t. each other + """ + Rz90 = Rot3.Rz(np.deg2rad(90)) + + # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame) + # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame + eTo0 = Pose3(Rot3(), np.array([1, 0, 0])) + eTo1 = Pose3(Rot3(), np.array([2, 0, 0])) + eTo2 = Pose3(Rot3(), np.array([4, 0, 0])) + + eToi_list = [eTo0, eTo1, eTo2] + + # Create destination poses + # (same three objects, but instead living in the world/city "w" frame) + wTo0 = Pose3(Rz90, np.array([0, 12, 0])) + wTo1 = Pose3(Rz90, np.array([0, 14, 0])) + wTo2 = Pose3(Rz90, np.array([0, 18, 0])) + + wToi_list = [wTo0, wTo1, wTo2] + + we_pairs = gtsam.Pose3Pairs(list(zip(wToi_list, eToi_list))) + + # Recover the transformation wSe (i.e. world_S_egovehicle) + wSe = Similarity3.Align(we_pairs) + + for wToi, eToi in zip(wToi_list, eToi_list): + self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) + + def test_align_poses3_scaled_squares(self): + """Test if Align Pose3Pairs method can account for gauge ambiguity. + + Make sure a big and small square can be aligned. + The u's represent a big square (10x10), and v's represents a small square (4x4). + + Scenario: + 4 object poses + with gauge ambiguity (2.5x scale) + """ + # 0, 90, 180, and 270 degrees yaw + R0 = Rot3.Rz(np.deg2rad(0)) + R90 = Rot3.Rz(np.deg2rad(90)) + R180 = Rot3.Rz(np.deg2rad(180)) + R270 = Rot3.Rz(np.deg2rad(270)) + + aTi0 = Pose3(R0, np.array([2, 3, 0])) + aTi1 = Pose3(R90, np.array([12, 3, 0])) + aTi2 = Pose3(R180, np.array([12, 13, 0])) + aTi3 = Pose3(R270, np.array([2, 13, 0])) + + aTi_list = [aTi0, aTi1, aTi2, aTi3] + + bTi0 = Pose3(R0, np.array([4, 3, 0])) + bTi1 = Pose3(R90, np.array([8, 3, 0])) + bTi2 = Pose3(R180, np.array([8, 7, 0])) + bTi3 = Pose3(R270, np.array([4, 7, 0])) + + bTi_list = [bTi0, bTi1, bTi2, bTi3] + + ab_pairs = gtsam.Pose3Pairs(list(zip(aTi_list, bTi_list))) + + # Recover the transformation wSe (i.e. world_S_egovehicle) + aSb = Similarity3.Align(ab_pairs) + + for aTi, bTi in zip(aTi_list, bTi_list): + self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi)) + + def generate_measurements( + self, + calibration: Union[Cal3Bundler, Cal3_S2], + camera_model: Union[PinholeCameraCal3Bundler, PinholeCameraCal3_S2], + cal_params: Iterable[Iterable[Union[int, float]]], + camera_set: Optional[Union[CameraSetCal3Bundler, + CameraSetCal3_S2]] = None, + ) -> Tuple[List[Point2], Union[CameraSetCal3Bundler, CameraSetCal3_S2, + List[Cal3Bundler], List[Cal3_S2]]]: + """ + Generate vector of measurements for given calibration and camera model. + + Args: + calibration: Camera calibration e.g. Cal3_S2 + camera_model: Camera model e.g. PinholeCameraCal3_S2 + cal_params: Iterable of camera parameters for `calibration` e.g. [K1, K2] + camera_set: Cameraset object (for individual calibrations) + + Returns: + list of measurements and list/CameraSet object for cameras + """ + if camera_set is not None: + cameras = camera_set() + else: + cameras = [] + measurements = gtsam.Point2Vector() + + for k, pose in zip(cal_params, self.triangulation_poses): + K = calibration(*k) + camera = camera_model(pose, K) + cameras.append(camera) + z = camera.project(self.landmark) + measurements.append(z) + + return measurements, cameras + + def test_TriangulationExample(self) -> None: + """Tests triangulation with shared Cal3_S2 calibration""" + # Some common constants + sharedCal = (1500, 1200, 0, 640, 480) + + measurements, _ = self.generate_measurements( + calibration=Cal3_S2, + camera_model=PinholeCameraCal3_S2, + cal_params=(sharedCal, sharedCal)) + + triangulated_landmark = gtsam.triangulatePoint3( + self.triangulation_poses, + Cal3_S2(sharedCal), + measurements, + rank_tol=1e-9, + optimize=True) + self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) + + # Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) + measurements_noisy = gtsam.Point2Vector() + measurements_noisy.append(measurements[0] - np.array([0.1, 0.5])) + measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3])) + + triangulated_landmark = gtsam.triangulatePoint3( + self.triangulation_poses, + Cal3_S2(sharedCal), + measurements_noisy, + rank_tol=1e-9, + optimize=True) + + self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-2) + + def test_triangulation_robust_three_poses(self) -> None: + """Ensure triangulation with a robust model works.""" + sharedCal = Cal3_S2(1500, 1200, 0, 640, 480) + + # landmark ~5 meters infront of camera + landmark = Point3(5, 0.5, 1.2) + + pose1 = Pose3(UPRIGHT, Point3(0, 0, 1)) + pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)) + pose3 = pose1 * Pose3(Rot3.Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -0.1)) + + camera1 = PinholeCameraCal3_S2(pose1, sharedCal) + camera2 = PinholeCameraCal3_S2(pose2, sharedCal) + camera3 = PinholeCameraCal3_S2(pose3, sharedCal) + + z1: Point2 = camera1.project(landmark) + z2: Point2 = camera2.project(landmark) + z3: Point2 = camera3.project(landmark) + + poses = gtsam.Pose3Vector([pose1, pose2, pose3]) + measurements = gtsam.Point2Vector([z1, z2, z3]) + + # noise free, so should give exactly the landmark + actual = gtsam.triangulatePoint3(poses, + sharedCal, + measurements, + rank_tol=1e-9, + optimize=False) + self.assertTrue(np.allclose(landmark, actual, atol=1e-2)) + + # Add outlier + measurements[0] += Point2(100, 120) # very large pixel noise! + + # now estimate does not match landmark + actual2 = gtsam.triangulatePoint3(poses, + sharedCal, + measurements, + rank_tol=1e-9, + optimize=False) + # DLT is surprisingly robust, but still off (actual error is around 0.26m) + self.assertTrue(np.linalg.norm(landmark - actual2) >= 0.2) + self.assertTrue(np.linalg.norm(landmark - actual2) <= 0.5) + + # Again with nonlinear optimization + actual3 = gtsam.triangulatePoint3(poses, + sharedCal, + measurements, + rank_tol=1e-9, + optimize=True) + # result from nonlinear (but non-robust optimization) is close to DLT and still off + self.assertTrue(np.allclose(actual2, actual3, atol=0.1)) + + # Again with nonlinear optimization, this time with robust loss + model = gtsam.noiseModel.Robust.Create( + gtsam.noiseModel.mEstimator.Huber.Create(1.345), + gtsam.noiseModel.Unit.Create(2)) + actual4 = gtsam.triangulatePoint3(poses, + sharedCal, + measurements, + rank_tol=1e-9, + optimize=True, + model=model) + # using the Huber loss we now have a quite small error!! nice! + self.assertTrue(np.allclose(landmark, actual4, atol=0.05)) + + def test_outliers_and_far_landmarks(self) -> None: + """Check safe triangulation function.""" + pose1, pose2 = self.poses + + K1 = Cal3_S2(1500, 1200, 0, 640, 480) + # create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + camera1 = PinholeCameraCal3_S2(pose1, K1) + + # create second camera 1 meter to the right of first camera + K2 = Cal3_S2(1600, 1300, 0, 650, 440) + camera2 = PinholeCameraCal3_S2(pose2, K2) + + # 1. Project two landmarks into two cameras and triangulate + z1 = camera1.project(self.landmark) + z2 = camera2.project(self.landmark) + + cameras = CameraSetCal3_S2() + cameras.append(camera1) + cameras.append(camera2) + + measurements = gtsam.Point2Vector() + measurements.append(z1) + measurements.append(z2) + + landmarkDistanceThreshold = 10 # landmark is closer than that + # all default except landmarkDistanceThreshold: + params = TriangulationParameters(1.0, False, landmarkDistanceThreshold) + actual: TriangulationResult = gtsam.triangulateSafe( + cameras, measurements, params) + self.gtsamAssertEquals(actual.get(), self.landmark, 1e-2) + self.assertTrue(actual.valid()) + + landmarkDistanceThreshold = 4 # landmark is farther than that + params2 = TriangulationParameters(1.0, False, + landmarkDistanceThreshold) + actual = gtsam.triangulateSafe(cameras, measurements, params2) + self.assertTrue(actual.farPoint()) + + # 3. Add a slightly rotated third camera above with a wrong measurement + # (OUTLIER) + pose3 = pose1 * Pose3(Rot3.Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)) + K3 = Cal3_S2(700, 500, 0, 640, 480) + camera3 = PinholeCameraCal3_S2(pose3, K3) + z3 = camera3.project(self.landmark) + + cameras.append(camera3) + measurements.append(z3 + Point2(10, -10)) + + landmarkDistanceThreshold = 10 # landmark is closer than that + outlierThreshold = 100 # loose, the outlier is going to pass + params3 = TriangulationParameters(1.0, False, + landmarkDistanceThreshold, + outlierThreshold) + actual = gtsam.triangulateSafe(cameras, measurements, params3) + self.assertTrue(actual.valid()) + + # now set stricter threshold for outlier rejection + outlierThreshold = 5 # tighter, the outlier is not going to pass + params4 = TriangulationParameters(1.0, False, + landmarkDistanceThreshold, + outlierThreshold) + actual = gtsam.triangulateSafe(cameras, measurements, params4) + self.assertTrue(actual.outlier()) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam/tests/test_custom_factor.py b/python/gtsam/tests/test_custom_factor.py index 4f0f33361..d19706c49 100644 --- a/python/gtsam/tests/test_custom_factor.py +++ b/python/gtsam/tests/test_custom_factor.py @@ -8,31 +8,34 @@ See LICENSE for the license information CustomFactor unit tests. Author: Fan Jiang """ -from typing import List import unittest -from gtsam import Values, Pose2, CustomFactor +from typing import List import numpy as np +from gtsam.utils.test_case import GtsamTestCase import gtsam -from gtsam.utils.test_case import GtsamTestCase +from gtsam import CustomFactor, Pose2, Values class TestCustomFactor(GtsamTestCase): + def test_new(self): """Test the creation of a new CustomFactor""" - def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + def error_func(this: CustomFactor, v: gtsam.Values, + H: List[np.ndarray]): """Minimal error function stub""" - return np.array([1, 0, 0]) + return np.array([1, 0, 0]), H noise_model = gtsam.noiseModel.Unit.Create(3) - cf = CustomFactor(noise_model, gtsam.KeyVector([0]), error_func) + cf = CustomFactor(noise_model, [0], error_func) def test_new_keylist(self): """Test the creation of a new CustomFactor""" - def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + def error_func(this: CustomFactor, v: gtsam.Values, + H: List[np.ndarray]): """Minimal error function stub""" return np.array([1, 0, 0]) @@ -43,7 +46,8 @@ class TestCustomFactor(GtsamTestCase): """Test if calling the factor works (only error)""" expected_pose = Pose2(1, 1, 0) - def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]) -> np.ndarray: + def error_func(this: CustomFactor, v: gtsam.Values, + H: List[np.ndarray]) -> np.ndarray: """Minimal error function with no Jacobian""" key0 = this.keys()[0] error = -v.atPose2(key0).localCoordinates(expected_pose) @@ -65,7 +69,8 @@ class TestCustomFactor(GtsamTestCase): expected = Pose2(2, 2, np.pi / 2) - def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + def error_func(this: CustomFactor, v: gtsam.Values, + H: List[np.ndarray]): """ the custom error function. One can freely use variables captured from the outside scope. Or the variables can be acquired by indexing `v`. @@ -84,7 +89,7 @@ class TestCustomFactor(GtsamTestCase): return error noise_model = gtsam.noiseModel.Unit.Create(3) - cf = CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func) + cf = CustomFactor(noise_model, [0, 1], error_func) v = Values() v.insert(0, gT1) v.insert(1, gT2) @@ -104,7 +109,8 @@ class TestCustomFactor(GtsamTestCase): gT1 = Pose2(1, 2, np.pi / 2) gT2 = Pose2(-1, 4, np.pi) - def error_func(this: CustomFactor, v: gtsam.Values, _: List[np.ndarray]): + def error_func(this: CustomFactor, v: gtsam.Values, + _: List[np.ndarray]): """Minimal error function stub""" return np.array([1, 0, 0]) @@ -125,7 +131,8 @@ class TestCustomFactor(GtsamTestCase): expected = Pose2(2, 2, np.pi / 2) - def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + def error_func(this: CustomFactor, v: gtsam.Values, + H: List[np.ndarray]): """ Error function that mimics a BetweenFactor :param this: reference to the current CustomFactor being evaluated @@ -138,7 +145,8 @@ class TestCustomFactor(GtsamTestCase): gT1, gT2 = v.atPose2(key0), v.atPose2(key1) error = expected.localCoordinates(gT1.between(gT2)) - self.assertTrue(H is None) # Should be true if we only request the error + self.assertTrue( + H is None) # Should be true if we only request the error if H is not None: result = gT1.between(gT2) @@ -147,7 +155,7 @@ class TestCustomFactor(GtsamTestCase): return error noise_model = gtsam.noiseModel.Unit.Create(3) - cf = CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func) + cf = CustomFactor(noise_model, [0, 1], error_func) v = Values() v.insert(0, gT1) v.insert(1, gT2) @@ -165,7 +173,8 @@ class TestCustomFactor(GtsamTestCase): expected = Pose2(2, 2, np.pi / 2) - def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + def error_func(this: CustomFactor, v: gtsam.Values, + H: List[np.ndarray]): """ Error function that mimics a BetweenFactor :param this: reference to the current CustomFactor being evaluated diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index 880c436e8..fe067e787 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -12,7 +12,6 @@ from mpl_toolkits.mplot3d import Axes3D # pylint: disable=unused-import import gtsam from gtsam import Marginals, Point2, Point3, Pose2, Pose3, Values - # For translation between a scaling of the uncertainty ellipse and the # percentage of inliers see discussion in # [PR 1067](https://github.com/borglab/gtsam/pull/1067) @@ -557,7 +556,7 @@ def plot_incremental_trajectory(fignum: int, axes = fig.axes[0] poses = gtsam.utilities.allPose3s(values) - keys = gtsam.KeyVector(poses.keys()) + keys = poses.keys() for key in keys[start:]: if values.exists(key): diff --git a/python/gtsam/utils/visual_isam.py b/python/gtsam/utils/visual_isam.py index 4ebd8accd..52ec8e32f 100644 --- a/python/gtsam/utils/visual_isam.py +++ b/python/gtsam/utils/visual_isam.py @@ -79,7 +79,7 @@ def initialize(data, truth, options): return isam, result, nextPoseIndex -def step(data, isam, result, truth, currPoseIndex): +def step(data, isam, result, truth, currPoseIndex, isamArgs=()): ''' Do one step isam update @param[in] data: measurement data (odometry and visual measurements and their noiseModels) @@ -123,7 +123,7 @@ def step(data, isam, result, truth, currPoseIndex): # Update ISAM # figure(1)tic - isam.update(newFactors, initialEstimates) + isam.update(newFactors, initialEstimates, *isamArgs) # t=toc plot(frame_i,t,'r.') tic newResult = isam.calculateEstimate() # t=toc plot(frame_i,t,'g.') diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 66812d6bb..082b261ad 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -12,7 +12,7 @@ if (NOT GTSAM_USE_BOOST_FEATURES) endif() if (NOT GTSAM_ENABLE_BOOST_SERIALIZATION) - list(APPEND excluded_tests "testSerializationSLAM.cpp") + list(APPEND excluded_tests "testSerializationSlam.cpp") endif() # Build tests diff --git a/timing/timeTest.cpp b/timing/timeTest.cpp index 9da3cfb2e..c450b9cf0 100644 --- a/timing/timeTest.cpp +++ b/timing/timeTest.cpp @@ -21,7 +21,6 @@ using namespace gtsam; int main(int argc, char *argv[]) { - // FIXME: ticPush_ does not exist { gttic_(top1); gttic_(sub1); diff --git a/wrap/.github/workflows/linux-ci.yml b/wrap/.github/workflows/linux-ci.yml index 34623385e..6c7ef1285 100644 --- a/wrap/.github/workflows/linux-ci.yml +++ b/wrap/.github/workflows/linux-ci.yml @@ -5,12 +5,12 @@ on: [pull_request] jobs: build: name: Tests for 🐍 ${{ matrix.python-version }} - runs-on: ubuntu-18.04 + runs-on: ubuntu-22.04 strategy: fail-fast: false matrix: - python-version: [3.6, 3.7, 3.8, 3.9] + python-version: ["3.7", "3.8", "3.9", "3.10"] steps: - name: Checkout @@ -19,7 +19,7 @@ jobs: - name: Install Dependencies run: | sudo apt-get -y update - sudo apt install cmake build-essential pkg-config libpython-dev python-numpy libboost-all-dev + sudo apt install cmake build-essential pkg-config libpython3-dev python3-numpy libboost-all-dev - name: Set up Python ${{ matrix.python-version }} uses: actions/setup-python@v2 diff --git a/wrap/.github/workflows/macos-ci.yml b/wrap/.github/workflows/macos-ci.yml index 8119a3acb..adba486c5 100644 --- a/wrap/.github/workflows/macos-ci.yml +++ b/wrap/.github/workflows/macos-ci.yml @@ -5,12 +5,12 @@ on: [pull_request] jobs: build: name: Tests for 🐍 ${{ matrix.python-version }} - runs-on: macos-10.15 + runs-on: macos-12 strategy: fail-fast: false matrix: - python-version: [3.6, 3.7, 3.8, 3.9] + python-version: ["3.7", "3.8", "3.9", "3.10"] steps: - name: Checkout diff --git a/wrap/cmake/MatlabWrap.cmake b/wrap/cmake/MatlabWrap.cmake index c45d8c050..55b7cdb99 100644 --- a/wrap/cmake/MatlabWrap.cmake +++ b/wrap/cmake/MatlabWrap.cmake @@ -105,7 +105,12 @@ function(wrap_library_internal interfaceHeader moduleName linkLibraries extraInc set(mexModuleExt mexglx) endif() elseif(APPLE) - set(mexModuleExt mexmaci64) + check_cxx_compiler_flag("-arch arm64" arm64Supported) + if (arm64Supported) + set(mexModuleExt mexmaca64) + else() + set(mexModuleExt mexmaci64) + endif() elseif(MSVC) if(CMAKE_CL_64) set(mexModuleExt mexw64) @@ -299,7 +304,12 @@ function(wrap_library_internal interfaceHeader moduleName linkLibraries extraInc APPEND PROPERTY COMPILE_FLAGS "/bigobj") elseif(APPLE) - set(mxLibPath "${MATLAB_ROOT}/bin/maci64") + check_cxx_compiler_flag("-arch arm64" arm64Supported) + if (arm64Supported) + set(mxLibPath "${MATLAB_ROOT}/bin/maca64") + else() + set(mxLibPath "${MATLAB_ROOT}/bin/maci64") + endif() target_link_libraries( ${moduleName}_matlab_wrapper "${mxLibPath}/libmex.dylib" "${mxLibPath}/libmx.dylib" "${mxLibPath}/libmat.dylib") @@ -367,7 +377,12 @@ function(check_conflicting_libraries_internal libraries) if(UNIX) # Set path for matlab's built-in libraries if(APPLE) - set(mxLibPath "${MATLAB_ROOT}/bin/maci64") + check_cxx_compiler_flag("-arch arm64" arm64Supported) + if (arm64Supported) + set(mxLibPath "${MATLAB_ROOT}/bin/maca64") + else() + set(mxLibPath "${MATLAB_ROOT}/bin/maci64") + endif() else() if(CMAKE_CL_64) set(mxLibPath "${MATLAB_ROOT}/bin/glnxa64") diff --git a/wrap/gtwrap/interface_parser/tokens.py b/wrap/gtwrap/interface_parser/tokens.py index 0f8d38d86..02e6d82f8 100644 --- a/wrap/gtwrap/interface_parser/tokens.py +++ b/wrap/gtwrap/interface_parser/tokens.py @@ -10,9 +10,10 @@ All the token definitions. Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert """ -from pyparsing import (Keyword, Literal, OneOrMore, Or, # type: ignore - QuotedString, Suppress, Word, alphanums, alphas, - nestedExpr, nums, originalTextFor, printables) +from pyparsing import Or # type: ignore +from pyparsing import (Keyword, Literal, OneOrMore, QuotedString, Suppress, + Word, alphanums, alphas, nestedExpr, nums, + originalTextFor, printables) # rule for identifiers (e.g. variable names) IDENT = Word(alphas + '_', alphanums + '_') ^ Word(nums) @@ -52,7 +53,7 @@ CONST, VIRTUAL, CLASS, STATIC, PAIR, TEMPLATE, TYPEDEF, INCLUDE = map( ) ENUM = Keyword("enum") ^ Keyword("enum class") ^ Keyword("enum struct") NAMESPACE = Keyword("namespace") -BASIS_TYPES = map( +BASIC_TYPES = map( Keyword, [ "void", diff --git a/wrap/gtwrap/interface_parser/type.py b/wrap/gtwrap/interface_parser/type.py index deb2e2256..e56a2f015 100644 --- a/wrap/gtwrap/interface_parser/type.py +++ b/wrap/gtwrap/interface_parser/type.py @@ -17,15 +17,13 @@ from typing import List, Sequence, Union from pyparsing import ParseResults # type: ignore from pyparsing import Forward, Optional, Or, delimitedList -from .tokens import (BASIS_TYPES, CONST, IDENT, LOPBRACK, RAW_POINTER, REF, +from .tokens import (BASIC_TYPES, CONST, IDENT, LOPBRACK, RAW_POINTER, REF, ROPBRACK, SHARED_POINTER) class Typename: """ - Generic type which can be either a basic type or a class type, - similar to C++'s `typename` aka a qualified dependent type. - Contains type name with full namespace and template arguments. + Class which holds a type's name, full namespace, and template arguments. E.g. ``` @@ -89,7 +87,6 @@ class Typename: def to_cpp(self) -> str: """Generate the C++ code for wrapping.""" - idx = 1 if self.namespaces and not self.namespaces[0] else 0 if self.instantiations: cpp_name = self.name + "<{}>".format(", ".join( [inst.to_cpp() for inst in self.instantiations])) @@ -116,7 +113,7 @@ class BasicType: """ Basic types are the fundamental built-in types in C++ such as double, int, char, etc. - When using templates, the basis type will take on the same form as the template. + When using templates, the basic type will take on the same form as the template. E.g. ``` @@ -127,16 +124,16 @@ class BasicType: will give ``` - m_.def("CoolFunctionDoubleDouble",[](const double& s) { - return wrap_example::CoolFunction(s); - }, py::arg("s")); + m_.def("funcDouble",[](const double& x){ + ::func(x); + }, py::arg("x")); ``` """ - rule = (Or(BASIS_TYPES)("typename")).setParseAction(lambda t: BasicType(t)) + rule = (Or(BASIC_TYPES)("typename")).setParseAction(lambda t: BasicType(t)) def __init__(self, t: ParseResults): - self.typename = Typename(t.asList()) + self.typename = Typename(t) class CustomType: @@ -160,9 +157,9 @@ class CustomType: class Type: """ - Parsed datatype, can be either a fundamental type or a custom datatype. + Parsed datatype, can be either a fundamental/basic type or a custom datatype. E.g. void, double, size_t, Matrix. - Think of this as a high-level type which encodes the typename and other + Think of this as a high-level type which encodes the typename and other characteristics of the type. The type can optionally be a raw pointer, shared pointer or reference. @@ -170,7 +167,7 @@ class Type: """ rule = ( Optional(CONST("is_const")) # - + (BasicType.rule("basis") | CustomType.rule("qualified")) # BR + + (BasicType.rule("basic") | CustomType.rule("qualified")) # BR + Optional( SHARED_POINTER("is_shared_ptr") | RAW_POINTER("is_ptr") | REF("is_ref")) # @@ -188,9 +185,10 @@ class Type: @staticmethod def from_parse_result(t: ParseResults): """Return the resulting Type from parsing the source.""" - if t.basis: + # If the type is a basic/fundamental c++ type (e.g int, bool) + if t.basic: return Type( - typename=t.basis.typename, + typename=t.basic.typename, is_const=t.is_const, is_shared_ptr=t.is_shared_ptr, is_ptr=t.is_ptr, diff --git a/wrap/gtwrap/matlab_wrapper/mixins.py b/wrap/gtwrap/matlab_wrapper/mixins.py index 4c2b005b7..df4de98f3 100644 --- a/wrap/gtwrap/matlab_wrapper/mixins.py +++ b/wrap/gtwrap/matlab_wrapper/mixins.py @@ -60,6 +60,31 @@ class CheckMixin: arg_type.typename.name not in self.not_ptr_type and \ arg_type.is_ref + def is_class_enum(self, arg_type: parser.Type, class_: parser.Class): + """Check if arg_type is an enum in the class `class_`.""" + if class_: + class_enums = [enum.name for enum in class_.enums] + return arg_type.typename.name in class_enums + else: + return False + + def is_global_enum(self, arg_type: parser.Type, class_: parser.Class): + """Check if arg_type is a global enum.""" + if class_: + # Get the enums in the class' namespace + global_enums = [ + member.name for member in class_.parent.content + if isinstance(member, parser.Enum) + ] + return arg_type.typename.name in global_enums + else: + return False + + def is_enum(self, arg_type: parser.Type, class_: parser.Class): + """Check if `arg_type` is an enum.""" + return self.is_class_enum(arg_type, class_) or self.is_global_enum( + arg_type, class_) + class FormatMixin: """Mixin to provide formatting utilities.""" diff --git a/wrap/gtwrap/matlab_wrapper/templates.py b/wrap/gtwrap/matlab_wrapper/templates.py index 7783c8e9c..c1c7e75ce 100644 --- a/wrap/gtwrap/matlab_wrapper/templates.py +++ b/wrap/gtwrap/matlab_wrapper/templates.py @@ -1,3 +1,5 @@ +"""Code generation templates for the Matlab wrapper.""" + import textwrap diff --git a/wrap/gtwrap/matlab_wrapper/wrapper.py b/wrap/gtwrap/matlab_wrapper/wrapper.py index 0f156a6de..146209c44 100755 --- a/wrap/gtwrap/matlab_wrapper/wrapper.py +++ b/wrap/gtwrap/matlab_wrapper/wrapper.py @@ -341,11 +341,17 @@ class MatlabWrapper(CheckMixin, FormatMixin): return check_statement - def _unwrap_argument(self, arg, arg_id=0, constructor=False): + def _unwrap_argument(self, arg, arg_id=0, instantiated_class=None): ctype_camel = self._format_type_name(arg.ctype.typename, separator='') ctype_sep = self._format_type_name(arg.ctype.typename) - if self.is_ref(arg.ctype): # and not constructor: + if instantiated_class and \ + self.is_enum(arg.ctype, instantiated_class): + enum_type = f"{arg.ctype.typename}" + arg_type = f"{enum_type}" + unwrap = f'unwrap_enum<{enum_type}>(in[{arg_id}]);' + + elif self.is_ref(arg.ctype): # and not constructor: arg_type = "{ctype}&".format(ctype=ctype_sep) unwrap = '*unwrap_shared_ptr< {ctype} >(in[{id}], "ptr_{ctype_camel}");'.format( ctype=ctype_sep, ctype_camel=ctype_camel, id=arg_id) @@ -372,7 +378,10 @@ class MatlabWrapper(CheckMixin, FormatMixin): return arg_type, unwrap - def _wrapper_unwrap_arguments(self, args, arg_id=0, constructor=False): + def _wrapper_unwrap_arguments(self, + args, + arg_id=0, + instantiated_class=None): """Format the interface_parser.Arguments. Examples: @@ -383,7 +392,8 @@ class MatlabWrapper(CheckMixin, FormatMixin): body_args = '' for arg in args.list(): - arg_type, unwrap = self._unwrap_argument(arg, arg_id, constructor) + arg_type, unwrap = self._unwrap_argument( + arg, arg_id, instantiated_class=instantiated_class) body_args += textwrap.indent(textwrap.dedent('''\ {arg_type} {name} = {unwrap} @@ -405,7 +415,8 @@ class MatlabWrapper(CheckMixin, FormatMixin): continue if not self.is_ref(arg.ctype) and (self.is_shared_ptr(arg.ctype) or \ - self.is_ptr(arg.ctype) or self.can_be_pointer(arg.ctype))and \ + self.is_ptr(arg.ctype) or self.can_be_pointer(arg.ctype)) and \ + not self.is_enum(arg.ctype, instantiated_class) and \ arg.ctype.typename.name not in self.ignore_namespace: if arg.ctype.is_shared_ptr: call_type = arg.ctype.is_shared_ptr @@ -535,7 +546,7 @@ class MatlabWrapper(CheckMixin, FormatMixin): def wrap_methods(self, methods, global_funcs=False, global_ns=None): """ - Wrap a sequence of methods. Groups methods with the same names + Wrap a sequence of methods/functions. Groups methods with the same names together. If global_funcs is True then output every method into its own file. """ @@ -1027,7 +1038,7 @@ class MatlabWrapper(CheckMixin, FormatMixin): if uninstantiated_name in self.ignore_classes: return None - # Class comment + # Class docstring/comment content_text = self.class_comment(instantiated_class) content_text += self.wrap_methods(instantiated_class.methods) @@ -1108,31 +1119,73 @@ class MatlabWrapper(CheckMixin, FormatMixin): end ''') + # Enums + # Place enums into the correct submodule so we can access them + # e.g. gtsam.Class.Enum.A + for enum in instantiated_class.enums: + enum_text = self.wrap_enum(enum) + if namespace_name != '': + submodule = f"+{namespace_name}/" + else: + submodule = "" + submodule += f"+{instantiated_class.name}" + self.content.append((submodule, [enum_text])) + return file_name + '.m', content_text - def wrap_namespace(self, namespace): + def wrap_enum(self, enum): + """ + Wrap an enum definition as a Matlab class. + + Args: + enum: The interface_parser.Enum instance + """ + file_name = enum.name + '.m' + enum_template = textwrap.dedent("""\ + classdef {0} < uint32 + enumeration + {1} + end + end + """) + enumerators = "\n ".join([ + f"{enumerator.name}({idx})" + for idx, enumerator in enumerate(enum.enumerators) + ]) + + content = enum_template.format(enum.name, enumerators) + return file_name, content + + def wrap_namespace(self, namespace, add_mex_file=True): """Wrap a namespace by wrapping all of its components. Args: namespace: the interface_parser.namespace instance of the namespace - parent: parent namespace + add_cpp_file: Flag indicating whether the mex file should be added """ namespaces = namespace.full_namespaces() inner_namespace = namespace.name != '' wrapped = [] - cpp_filename = self._wrapper_name() + '.cpp' - self.content.append((cpp_filename, self.wrapper_file_headers)) - - current_scope = [] - namespace_scope = [] + top_level_scope = [] + inner_namespace_scope = [] for element in namespace.content: if isinstance(element, parser.Include): self.includes.append(element) elif isinstance(element, parser.Namespace): - self.wrap_namespace(element) + self.wrap_namespace(element, False) + + elif isinstance(element, parser.Enum): + file, content = self.wrap_enum(element) + if inner_namespace: + module = "".join([ + '+' + x + '/' for x in namespace.full_namespaces()[1:] + ])[:-1] + inner_namespace_scope.append((module, [(file, content)])) + else: + top_level_scope.append((file, content)) elif isinstance(element, instantiator.InstantiatedClass): self.add_class(element) @@ -1142,18 +1195,22 @@ class MatlabWrapper(CheckMixin, FormatMixin): element, "".join(namespace.full_namespaces())) if not class_text is None: - namespace_scope.append(("".join([ + inner_namespace_scope.append(("".join([ '+' + x + '/' for x in namespace.full_namespaces()[1:] ])[:-1], [(class_text[0], class_text[1])])) else: class_text = self.wrap_instantiated_class(element) - current_scope.append((class_text[0], class_text[1])) + top_level_scope.append((class_text[0], class_text[1])) - self.content.extend(current_scope) + self.content.extend(top_level_scope) if inner_namespace: - self.content.append(namespace_scope) + self.content.append(inner_namespace_scope) + + if add_mex_file: + cpp_filename = self._wrapper_name() + '.cpp' + self.content.append((cpp_filename, self.wrapper_file_headers)) # Global functions all_funcs = [ @@ -1213,10 +1270,30 @@ class MatlabWrapper(CheckMixin, FormatMixin): return return_type_text - def _collector_return(self, obj: str, ctype: parser.Type): + def _collector_return(self, + obj: str, + ctype: parser.Type, + instantiated_class: InstantiatedClass = None): """Helper method to get the final statement before the return in the collector function.""" expanded = '' - if self.is_shared_ptr(ctype) or self.is_ptr(ctype) or \ + + if instantiated_class and \ + self.is_enum(ctype, instantiated_class): + if self.is_class_enum(ctype, instantiated_class): + class_name = ".".join(instantiated_class.namespaces()[1:] + + [instantiated_class.name]) + else: + # Get the full namespace + class_name = ".".join(instantiated_class.parent.full_namespaces()[1:]) + + if class_name != "": + class_name += '.' + + enum_type = f"{class_name}{ctype.typename.name}" + expanded = textwrap.indent( + f'out[0] = wrap_enum({obj},\"{enum_type}\");', prefix=' ') + + elif self.is_shared_ptr(ctype) or self.is_ptr(ctype) or \ self.can_be_pointer(ctype): sep_method_name = partial(self._format_type_name, ctype.typename, @@ -1259,13 +1336,14 @@ class MatlabWrapper(CheckMixin, FormatMixin): return expanded - def wrap_collector_function_return(self, method): + def wrap_collector_function_return(self, method, instantiated_class=None): """ Wrap the complete return type of the function. """ expanded = '' - params = self._wrapper_unwrap_arguments(method.args, arg_id=1)[0] + params = self._wrapper_unwrap_arguments( + method.args, arg_id=1, instantiated_class=instantiated_class)[0] return_1 = method.return_type.type1 return_count = self._return_count(method.return_type) @@ -1301,7 +1379,8 @@ class MatlabWrapper(CheckMixin, FormatMixin): if return_1_name != 'void': if return_count == 1: - expanded += self._collector_return(obj, return_1) + expanded += self._collector_return( + obj, return_1, instantiated_class=instantiated_class) elif return_count == 2: return_2 = method.return_type.type2 @@ -1316,13 +1395,17 @@ class MatlabWrapper(CheckMixin, FormatMixin): return expanded - def wrap_collector_property_return(self, class_property: parser.Variable): + def wrap_collector_property_return( + self, + class_property: parser.Variable, + instantiated_class: InstantiatedClass = None): """Get the last collector function statement before return for a property.""" property_name = class_property.name obj = 'obj->{}'.format(property_name) - property_type = class_property.ctype - return self._collector_return(obj, property_type) + return self._collector_return(obj, + class_property.ctype, + instantiated_class=instantiated_class) def wrap_collector_function_upcast_from_void(self, class_name, func_id, cpp_name): @@ -1381,7 +1464,7 @@ class MatlabWrapper(CheckMixin, FormatMixin): elif collector_func[2] == 'constructor': base = '' params, body_args = self._wrapper_unwrap_arguments( - extra.args, constructor=True) + extra.args, instantiated_class=collector_func[1]) if collector_func[1].parent_class: base += textwrap.indent(textwrap.dedent(''' @@ -1442,8 +1525,12 @@ class MatlabWrapper(CheckMixin, FormatMixin): method_name += extra.name _, body_args = self._wrapper_unwrap_arguments( - extra.args, arg_id=1 if is_method else 0) - return_body = self.wrap_collector_function_return(extra) + extra.args, + arg_id=1 if is_method else 0, + instantiated_class=collector_func[1]) + + return_body = self.wrap_collector_function_return( + extra, collector_func[1]) shared_obj = '' @@ -1472,7 +1559,8 @@ class MatlabWrapper(CheckMixin, FormatMixin): class_name=class_name) # Unpack the property from mxArray - property_type, unwrap = self._unwrap_argument(extra, arg_id=1) + property_type, unwrap = self._unwrap_argument( + extra, arg_id=1, instantiated_class=collector_func[1]) unpack_property = textwrap.indent(textwrap.dedent('''\ {arg_type} {name} = {unwrap} '''.format(arg_type=property_type, @@ -1482,7 +1570,8 @@ class MatlabWrapper(CheckMixin, FormatMixin): # Getter if "_get_" in method_name: - return_body = self.wrap_collector_property_return(extra) + return_body = self.wrap_collector_property_return( + extra, instantiated_class=collector_func[1]) getter = ' checkArguments("{property_name}",nargout,nargin{min1},' \ '{num_args});\n' \ @@ -1498,7 +1587,8 @@ class MatlabWrapper(CheckMixin, FormatMixin): # Setter if "_set_" in method_name: - is_ptr_type = self.can_be_pointer(extra.ctype) + is_ptr_type = self.can_be_pointer(extra.ctype) and \ + not self.is_enum(extra.ctype, collector_func[1]) return_body = ' obj->{0} = {1}{0};'.format( extra.name, '*' if is_ptr_type else '') diff --git a/wrap/matlab.h b/wrap/matlab.h index b8fe53ac4..f44294770 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -118,10 +118,10 @@ void checkArguments(const string& name, int nargout, int nargin, int expected) { } //***************************************************************************** -// wrapping C++ basis types in MATLAB arrays +// wrapping C++ basic types in MATLAB arrays //***************************************************************************** -// default wrapping throws an error: only basis types are allowed in wrap +// default wrapping throws an error: only basic types are allowed in wrap template mxArray* wrap(const Class& value) { error("wrap internal error: attempted wrap of invalid type"); @@ -228,8 +228,26 @@ mxArray* wrap(const gtsam::Matrix& A) { return wrap_Matrix(A); } +/// @brief Wrap the C++ enum to Matlab mxArray +/// @tparam T The C++ enum type +/// @param x C++ enum +/// @param classname Matlab enum classdef used to call Matlab constructor +template +mxArray* wrap_enum(const T x, const std::string& classname) { + // create double array to store value in + mxArray* a = mxCreateDoubleMatrix(1, 1, mxREAL); + double* data = mxGetPr(a); + data[0] = static_cast(x); + + // convert to Matlab enumeration type + mxArray* result; + mexCallMATLAB(1, &result, 1, &a, classname.c_str()); + + return result; +} + //***************************************************************************** -// unwrapping MATLAB arrays into C++ basis types +// unwrapping MATLAB arrays into C++ basic types //***************************************************************************** // default unwrapping throws an error @@ -240,6 +258,24 @@ T unwrap(const mxArray* array) { return T(); } +/// @brief Unwrap from matlab array to C++ enum type +/// @tparam T The C++ enum type +/// @param array Matlab mxArray +template +T unwrap_enum(const mxArray* array) { + // Make duplicate to remove const-ness + mxArray* a = mxDuplicateArray(array); + + // convert void* to int32* array + mxArray* a_int32; + mexCallMATLAB(1, &a_int32, 1, &a, "int32"); + + // Get the value in the input array + int32_T* value = (int32_T*)mxGetData(a_int32); + // cast int32 to enum type + return static_cast(*value); +} + // specialization to string // expects a character array // Warning: relies on mxChar==char diff --git a/wrap/tests/expected/matlab/+Pet/Kind.m b/wrap/tests/expected/matlab/+Pet/Kind.m new file mode 100644 index 000000000..0d1836feb --- /dev/null +++ b/wrap/tests/expected/matlab/+Pet/Kind.m @@ -0,0 +1,6 @@ +classdef Kind < uint32 + enumeration + Dog(0) + Cat(1) + end +end diff --git a/wrap/tests/expected/matlab/+gtsam/+MCU/Avengers.m b/wrap/tests/expected/matlab/+gtsam/+MCU/Avengers.m new file mode 100644 index 000000000..9daca71f5 --- /dev/null +++ b/wrap/tests/expected/matlab/+gtsam/+MCU/Avengers.m @@ -0,0 +1,9 @@ +classdef Avengers < uint32 + enumeration + CaptainAmerica(0) + IronMan(1) + Hulk(2) + Hawkeye(3) + Thor(4) + end +end diff --git a/wrap/tests/expected/matlab/+gtsam/+MCU/GotG.m b/wrap/tests/expected/matlab/+gtsam/+MCU/GotG.m new file mode 100644 index 000000000..78a80d2cd --- /dev/null +++ b/wrap/tests/expected/matlab/+gtsam/+MCU/GotG.m @@ -0,0 +1,9 @@ +classdef GotG < uint32 + enumeration + Starlord(0) + Gamorra(1) + Rocket(2) + Drax(3) + Groot(4) + end +end diff --git a/wrap/tests/expected/matlab/+gtsam/+OptimizerGaussNewtonParams/Verbosity.m b/wrap/tests/expected/matlab/+gtsam/+OptimizerGaussNewtonParams/Verbosity.m new file mode 100644 index 000000000..7b8264157 --- /dev/null +++ b/wrap/tests/expected/matlab/+gtsam/+OptimizerGaussNewtonParams/Verbosity.m @@ -0,0 +1,7 @@ +classdef Verbosity < uint32 + enumeration + SILENT(0) + SUMMARY(1) + VERBOSE(2) + end +end diff --git a/wrap/tests/expected/matlab/+gtsam/VerbosityLM.m b/wrap/tests/expected/matlab/+gtsam/VerbosityLM.m new file mode 100644 index 000000000..636585543 --- /dev/null +++ b/wrap/tests/expected/matlab/+gtsam/VerbosityLM.m @@ -0,0 +1,12 @@ +classdef VerbosityLM < uint32 + enumeration + SILENT(0) + SUMMARY(1) + TERMINATION(2) + LAMBDA(3) + TRYLAMBDA(4) + TRYCONFIG(5) + DAMPED(6) + TRYDELTA(7) + end +end diff --git a/wrap/tests/expected/matlab/Color.m b/wrap/tests/expected/matlab/Color.m new file mode 100644 index 000000000..bd18c4123 --- /dev/null +++ b/wrap/tests/expected/matlab/Color.m @@ -0,0 +1,7 @@ +classdef Color < uint32 + enumeration + Red(0) + Green(1) + Blue(2) + end +end diff --git a/wrap/tests/expected/matlab/enum_wrapper.cpp b/wrap/tests/expected/matlab/enum_wrapper.cpp new file mode 100644 index 000000000..4860f9b8d --- /dev/null +++ b/wrap/tests/expected/matlab/enum_wrapper.cpp @@ -0,0 +1,322 @@ +#include +#include + + + +typedef gtsam::Optimizer OptimizerGaussNewtonParams; + +typedef std::set*> Collector_Pet; +static Collector_Pet collector_Pet; +typedef std::set*> Collector_gtsamMCU; +static Collector_gtsamMCU collector_gtsamMCU; +typedef std::set*> Collector_gtsamOptimizerGaussNewtonParams; +static Collector_gtsamOptimizerGaussNewtonParams collector_gtsamOptimizerGaussNewtonParams; + + +void _deleteAllObjects() +{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout); + + bool anyDeleted = false; + { for(Collector_Pet::iterator iter = collector_Pet.begin(); + iter != collector_Pet.end(); ) { + delete *iter; + collector_Pet.erase(iter++); + anyDeleted = true; + } } + { for(Collector_gtsamMCU::iterator iter = collector_gtsamMCU.begin(); + iter != collector_gtsamMCU.end(); ) { + delete *iter; + collector_gtsamMCU.erase(iter++); + anyDeleted = true; + } } + { for(Collector_gtsamOptimizerGaussNewtonParams::iterator iter = collector_gtsamOptimizerGaussNewtonParams.begin(); + iter != collector_gtsamOptimizerGaussNewtonParams.end(); ) { + delete *iter; + collector_gtsamOptimizerGaussNewtonParams.erase(iter++); + anyDeleted = true; + } } + + if(anyDeleted) + cout << + "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" + "calling destructors, call 'clear all' again if you plan to now recompile a wrap\n" + "module, so that your recompiled module is used instead of the old one." << endl; + std::cout.rdbuf(outbuf); +} + +void _enum_RTTIRegister() { + const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_enum_rttiRegistry_created"); + if(!alreadyCreated) { + std::map types; + + + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); + if(!registry) + registry = mxCreateStructMatrix(1, 1, 0, NULL); + typedef std::pair StringPair; + for(const StringPair& rtti_matlab: types) { + int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); + if(fieldId < 0) { + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } + mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); + mxSetFieldByNumber(registry, 0, fieldId, matlabName); + } + if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) { + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } + mxDestroyArray(registry); + + mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); + if(mexPutVariable("global", "gtsam_enum_rttiRegistry_created", newAlreadyCreated) != 0) { + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } + mxDestroyArray(newAlreadyCreated); + } +} + +void Pet_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef std::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_Pet.insert(self); +} + +void Pet_constructor_1(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef std::shared_ptr Shared; + + string& name = *unwrap_shared_ptr< string >(in[0], "ptr_string"); + Pet::Kind type = unwrap_enum(in[1]); + Shared *self = new Shared(new Pet(name,type)); + collector_Pet.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void Pet_deconstructor_2(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef std::shared_ptr Shared; + checkArguments("delete_Pet",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_Pet::iterator item; + item = collector_Pet.find(self); + if(item != collector_Pet.end()) { + collector_Pet.erase(item); + } + delete self; +} + +void Pet_getColor_3(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("getColor",nargout,nargin-1,0); + auto obj = unwrap_shared_ptr(in[0], "ptr_Pet"); + out[0] = wrap_enum(obj->getColor(),"Color"); +} + +void Pet_setColor_4(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("setColor",nargout,nargin-1,1); + auto obj = unwrap_shared_ptr(in[0], "ptr_Pet"); + Color color = unwrap_enum(in[1]); + obj->setColor(color); +} + +void Pet_get_name_5(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("name",nargout,nargin-1,0); + auto obj = unwrap_shared_ptr(in[0], "ptr_Pet"); + out[0] = wrap< string >(obj->name); +} + +void Pet_set_name_6(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("name",nargout,nargin-1,1); + auto obj = unwrap_shared_ptr(in[0], "ptr_Pet"); + string name = unwrap< string >(in[1]); + obj->name = name; +} + +void Pet_get_type_7(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("type",nargout,nargin-1,0); + auto obj = unwrap_shared_ptr(in[0], "ptr_Pet"); + out[0] = wrap_enum(obj->type,"Pet.Kind"); +} + +void Pet_set_type_8(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("type",nargout,nargin-1,1); + auto obj = unwrap_shared_ptr(in[0], "ptr_Pet"); + Pet::Kind type = unwrap_enum(in[1]); + obj->type = type; +} + +void gtsamMCU_collectorInsertAndMakeBase_9(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef std::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_gtsamMCU.insert(self); +} + +void gtsamMCU_constructor_10(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef std::shared_ptr Shared; + + Shared *self = new Shared(new gtsam::MCU()); + collector_gtsamMCU.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void gtsamMCU_deconstructor_11(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef std::shared_ptr Shared; + checkArguments("delete_gtsamMCU",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_gtsamMCU::iterator item; + item = collector_gtsamMCU.find(self); + if(item != collector_gtsamMCU.end()) { + collector_gtsamMCU.erase(item); + } + delete self; +} + +void gtsamOptimizerGaussNewtonParams_collectorInsertAndMakeBase_12(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef std::shared_ptr> Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_gtsamOptimizerGaussNewtonParams.insert(self); +} + +void gtsamOptimizerGaussNewtonParams_constructor_13(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef std::shared_ptr> Shared; + + Optimizer::Verbosity verbosity = unwrap_enum::Verbosity>(in[0]); + Shared *self = new Shared(new gtsam::Optimizer(verbosity)); + collector_gtsamOptimizerGaussNewtonParams.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void gtsamOptimizerGaussNewtonParams_deconstructor_14(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef std::shared_ptr> Shared; + checkArguments("delete_gtsamOptimizerGaussNewtonParams",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_gtsamOptimizerGaussNewtonParams::iterator item; + item = collector_gtsamOptimizerGaussNewtonParams.find(self); + if(item != collector_gtsamOptimizerGaussNewtonParams.end()) { + collector_gtsamOptimizerGaussNewtonParams.erase(item); + } + delete self; +} + +void gtsamOptimizerGaussNewtonParams_getVerbosity_15(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("getVerbosity",nargout,nargin-1,0); + auto obj = unwrap_shared_ptr>(in[0], "ptr_gtsamOptimizerGaussNewtonParams"); + out[0] = wrap_enum(obj->getVerbosity(),"gtsam.OptimizerGaussNewtonParams.Verbosity"); +} + +void gtsamOptimizerGaussNewtonParams_getVerbosity_16(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("getVerbosity",nargout,nargin-1,0); + auto obj = unwrap_shared_ptr>(in[0], "ptr_gtsamOptimizerGaussNewtonParams"); + out[0] = wrap_enum(obj->getVerbosity(),"gtsam.VerbosityLM"); +} + +void gtsamOptimizerGaussNewtonParams_setVerbosity_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("setVerbosity",nargout,nargin-1,1); + auto obj = unwrap_shared_ptr>(in[0], "ptr_gtsamOptimizerGaussNewtonParams"); + Optimizer::Verbosity value = unwrap_enum::Verbosity>(in[1]); + obj->setVerbosity(value); +} + + +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout); + + _enum_RTTIRegister(); + + int id = unwrap(in[0]); + + try { + switch(id) { + case 0: + Pet_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1); + break; + case 1: + Pet_constructor_1(nargout, out, nargin-1, in+1); + break; + case 2: + Pet_deconstructor_2(nargout, out, nargin-1, in+1); + break; + case 3: + Pet_getColor_3(nargout, out, nargin-1, in+1); + break; + case 4: + Pet_setColor_4(nargout, out, nargin-1, in+1); + break; + case 5: + Pet_get_name_5(nargout, out, nargin-1, in+1); + break; + case 6: + Pet_set_name_6(nargout, out, nargin-1, in+1); + break; + case 7: + Pet_get_type_7(nargout, out, nargin-1, in+1); + break; + case 8: + Pet_set_type_8(nargout, out, nargin-1, in+1); + break; + case 9: + gtsamMCU_collectorInsertAndMakeBase_9(nargout, out, nargin-1, in+1); + break; + case 10: + gtsamMCU_constructor_10(nargout, out, nargin-1, in+1); + break; + case 11: + gtsamMCU_deconstructor_11(nargout, out, nargin-1, in+1); + break; + case 12: + gtsamOptimizerGaussNewtonParams_collectorInsertAndMakeBase_12(nargout, out, nargin-1, in+1); + break; + case 13: + gtsamOptimizerGaussNewtonParams_constructor_13(nargout, out, nargin-1, in+1); + break; + case 14: + gtsamOptimizerGaussNewtonParams_deconstructor_14(nargout, out, nargin-1, in+1); + break; + case 15: + gtsamOptimizerGaussNewtonParams_getVerbosity_15(nargout, out, nargin-1, in+1); + break; + case 16: + gtsamOptimizerGaussNewtonParams_getVerbosity_16(nargout, out, nargin-1, in+1); + break; + case 17: + gtsamOptimizerGaussNewtonParams_setVerbosity_17(nargout, out, nargin-1, in+1); + break; + } + } catch(const std::exception& e) { + mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str()); + } + + std::cout.rdbuf(outbuf); +} diff --git a/wrap/tests/expected/matlab/special_cases_wrapper.cpp b/wrap/tests/expected/matlab/special_cases_wrapper.cpp index 0669b442e..2fe55ec01 100644 --- a/wrap/tests/expected/matlab/special_cases_wrapper.cpp +++ b/wrap/tests/expected/matlab/special_cases_wrapper.cpp @@ -204,15 +204,15 @@ void gtsamGeneralSFMFactorCal3Bundler_get_verbosity_11(int nargout, mxArray *out { checkArguments("verbosity",nargout,nargin-1,0); auto obj = unwrap_shared_ptr, gtsam::Point3>>(in[0], "ptr_gtsamGeneralSFMFactorCal3Bundler"); - out[0] = wrap_shared_ptr(std::make_shared, gtsam::Point3>::Verbosity>(obj->verbosity),"gtsam.GeneralSFMFactor, gtsam::Point3>.Verbosity", false); + out[0] = wrap_enum(obj->verbosity,"gtsam.GeneralSFMFactorCal3Bundler.Verbosity"); } void gtsamGeneralSFMFactorCal3Bundler_set_verbosity_12(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("verbosity",nargout,nargin-1,1); auto obj = unwrap_shared_ptr, gtsam::Point3>>(in[0], "ptr_gtsamGeneralSFMFactorCal3Bundler"); - std::shared_ptr, gtsam::Point3>::Verbosity> verbosity = unwrap_shared_ptr< gtsam::GeneralSFMFactor, gtsam::Point3>::Verbosity >(in[1], "ptr_gtsamGeneralSFMFactor, gtsam::Point3>Verbosity"); - obj->verbosity = *verbosity; + gtsam::GeneralSFMFactor, gtsam::Point3>::Verbosity verbosity = unwrap_enum, gtsam::Point3>::Verbosity>(in[1]); + obj->verbosity = verbosity; } diff --git a/wrap/tests/expected/python/enum_pybind.cpp b/wrap/tests/expected/python/enum_pybind.cpp index 2fa804ac9..c67bf1de0 100644 --- a/wrap/tests/expected/python/enum_pybind.cpp +++ b/wrap/tests/expected/python/enum_pybind.cpp @@ -23,7 +23,9 @@ PYBIND11_MODULE(enum_py, m_) { py::class_> pet(m_, "Pet"); pet - .def(py::init(), py::arg("name"), py::arg("type")) + .def(py::init(), py::arg("name"), py::arg("type")) + .def("setColor",[](Pet* self, const Color& color){ self->setColor(color);}, py::arg("color")) + .def("getColor",[](Pet* self){return self->getColor();}) .def_readwrite("name", &Pet::name) .def_readwrite("type", &Pet::type); @@ -65,7 +67,10 @@ PYBIND11_MODULE(enum_py, m_) { py::class_, std::shared_ptr>> optimizergaussnewtonparams(m_gtsam, "OptimizerGaussNewtonParams"); optimizergaussnewtonparams - .def("setVerbosity",[](gtsam::Optimizer* self, const Optimizer::Verbosity value){ self->setVerbosity(value);}, py::arg("value")); + .def(py::init::Verbosity&>(), py::arg("verbosity")) + .def("setVerbosity",[](gtsam::Optimizer* self, const Optimizer::Verbosity value){ self->setVerbosity(value);}, py::arg("value")) + .def("getVerbosity",[](gtsam::Optimizer* self){return self->getVerbosity();}) + .def("getVerbosity",[](gtsam::Optimizer* self){return self->getVerbosity();}); py::enum_::Verbosity>(optimizergaussnewtonparams, "Verbosity", py::arithmetic()) .value("SILENT", gtsam::Optimizer::Verbosity::SILENT) diff --git a/wrap/tests/fixtures/enum.i b/wrap/tests/fixtures/enum.i index 71918c25a..6e70d9c57 100644 --- a/wrap/tests/fixtures/enum.i +++ b/wrap/tests/fixtures/enum.i @@ -3,13 +3,16 @@ enum Color { Red, Green, Blue }; class Pet { enum Kind { Dog, Cat }; - Pet(const string &name, Kind type); + Pet(const string &name, Pet::Kind type); + void setColor(const Color& color); + Color getColor() const; string name; - Kind type; + Pet::Kind type; }; namespace gtsam { +// Test global enums enum VerbosityLM { SILENT, SUMMARY, @@ -21,6 +24,7 @@ enum VerbosityLM { TRYDELTA }; +// Test multiple enums in a classs class MCU { MCU(); @@ -50,7 +54,12 @@ class Optimizer { VERBOSE }; + Optimizer(const This::Verbosity& verbosity); + void setVerbosity(const This::Verbosity value); + + gtsam::Optimizer::Verbosity getVerbosity() const; + gtsam::VerbosityLM getVerbosity() const; }; typedef gtsam::Optimizer OptimizerGaussNewtonParams; diff --git a/wrap/tests/test_interface_parser.py b/wrap/tests/test_interface_parser.py index 19462a51a..45415995f 100644 --- a/wrap/tests/test_interface_parser.py +++ b/wrap/tests/test_interface_parser.py @@ -38,7 +38,7 @@ class TestInterfaceParser(unittest.TestCase): def test_basic_type(self): """Tests for BasicType.""" - # Check basis type + # Check basic type t = Type.rule.parseString("int x")[0] self.assertEqual("int", t.typename.name) self.assertTrue(t.is_basic) @@ -243,7 +243,7 @@ class TestInterfaceParser(unittest.TestCase): self.assertEqual("void", return_type.type1.typename.name) self.assertTrue(return_type.type1.is_basic) - # Test basis type + # Test basic type return_type = ReturnType.rule.parseString("size_t")[0] self.assertEqual("size_t", return_type.type1.typename.name) self.assertTrue(not return_type.type2) diff --git a/wrap/tests/test_matlab_wrapper.py b/wrap/tests/test_matlab_wrapper.py index 17b2dd11d..0ca95b66d 100644 --- a/wrap/tests/test_matlab_wrapper.py +++ b/wrap/tests/test_matlab_wrapper.py @@ -141,6 +141,32 @@ class TestWrap(unittest.TestCase): actual = osp.join(self.MATLAB_ACTUAL_DIR, file) self.compare_and_diff(file, actual) + def test_enum(self): + """Test interface file with only enum info.""" + file = osp.join(self.INTERFACE_DIR, 'enum.i') + + wrapper = MatlabWrapper( + module_name='enum', + top_module_namespace=['gtsam'], + ignore_classes=[''], + ) + + wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) + + files = [ + 'enum_wrapper.cpp', + 'Color.m', + '+Pet/Kind.m', + '+gtsam/VerbosityLM.m', + '+gtsam/+MCU/Avengers.m', + '+gtsam/+MCU/GotG.m', + '+gtsam/+OptimizerGaussNewtonParams/Verbosity.m', + ] + + for file in files: + actual = osp.join(self.MATLAB_ACTUAL_DIR, file) + self.compare_and_diff(file, actual) + def test_templates(self): """Test interface file with template info.""" file = osp.join(self.INTERFACE_DIR, 'templates.i')