From 4fcfde07bda04b0c05f1a92802233c88ac53ac12 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 2 Oct 2020 17:06:41 -0400 Subject: [PATCH 01/43] check if mex compiler exists for Matlab wrapper, formatting --- CMakeLists.txt | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index eedc42c9e..644058604 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -83,6 +83,7 @@ option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF) option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module with pybind11" OFF) +option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON) option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) @@ -103,19 +104,25 @@ if(NOT MSVC AND NOT XCODE_VERSION) endif() endif() -# Options relating to MATLAB wrapper -# TODO: Check for matlab mex binary before handling building of binaries -option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) -set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of Python to build the wrappers against.") # Check / set dependent variables for MATLAB wrapper -if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_TYPE_POSTFIXES) - set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX}) +if(GTSAM_INSTALL_MATLAB_TOOLBOX) + find_package(Matlab COMPONENTS MEX_COMPILER REQUIRED) + if(NOT Matlab_MEX_COMPILER) + message(FATAL_ERROR "Cannot find MEX compiler binary. Please check your Matlab installation and ensure MEX in installed as well.") + endif() + + if(GTSAM_BUILD_TYPE_POSTFIXES) + set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX}) + endif() + + if(NOT BUILD_SHARED_LIBS) + message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.") + endif() endif() -if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS) - message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.") -endif() + +set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of Python to build the wrappers against.") if(GTSAM_BUILD_PYTHON) # Get info about the Python3 interpreter From 51b6fd0b43852bf14148b39c14d2563f2d87c3d4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 3 Nov 2020 12:11:17 -0500 Subject: [PATCH 02/43] Added flag for absolute error --- gtsam/base/Matrix.h | 2 +- gtsam/base/Vector.cpp | 6 ++++-- gtsam/base/Vector.h | 3 ++- gtsam/geometry/tests/testRot3.cpp | 6 +++--- 4 files changed, 10 insertions(+), 7 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 071c33fca..9adf4e1c1 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -90,7 +90,7 @@ bool equal_with_abs_tol(const Eigen::DenseBase& A, const Eigen::DenseBas for(size_t i=0; i::max())) { + else if (abs(a - b) <= + tol * min(larger, std::numeric_limits::max()) && + !absolute) { return true; } diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 319ad6ee6..b0fc74f26 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -87,7 +87,8 @@ static_assert( * * Return true if two numbers are close wrt tol. */ -GTSAM_EXPORT bool fpEqual(double a, double b, double tol); +GTSAM_EXPORT bool fpEqual(double a, double b, double tol, + bool absolute = false); /** * print without optional string, must specify cout yourself diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 7b792f8bd..889f68580 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -807,15 +807,15 @@ TEST(Rot3, RQ_derivative) { test_xyz.push_back(VecAndErr{{0, 0, 0}, error}); test_xyz.push_back(VecAndErr{{0, 0.5, -0.5}, error}); test_xyz.push_back(VecAndErr{{0.3, 0, 0.2}, error}); - test_xyz.push_back(VecAndErr{{-0.6, 1.3, 0}, error}); + test_xyz.push_back(VecAndErr{{-0.6, 1.3, 0}, 1e-8}); test_xyz.push_back(VecAndErr{{1.0, 0.7, 0.8}, error}); test_xyz.push_back(VecAndErr{{3.0, 0.7, -0.6}, error}); test_xyz.push_back(VecAndErr{{M_PI / 2, 0, 0}, error}); test_xyz.push_back(VecAndErr{{0, 0, M_PI / 2}, error}); // Test close to singularity - test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1e-1, 0}, 1e-8}); - test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1e-1, 0}, 1e-8}); + test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1e-1, 0}, 1e-7}); + test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1e-1, 0}, 1e-7}); test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1.1e-2, 0}, 1e-4}); test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1.1e-2, 0}, 1e-4}); From c68a64745e1ad99cb047286434cfda1e1b7aedc7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 3 Nov 2020 14:27:05 -0500 Subject: [PATCH 03/43] add test --- gtsam/base/tests/testMatrix.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 468e842f2..15087093a 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -1163,6 +1163,19 @@ TEST(Matrix , IsVectorSpace) { BOOST_CONCEPT_ASSERT((IsVectorSpace)); } +TEST(Matrix, AbsoluteError) { + double a = 2000, b = 1997, tol=1e-1; + bool isEqual; + + // Test absolute error + isEqual = fpEqual(a, b, tol, true); + EXPECT(isEqual == false); + + // Test relative error + isEqual = fpEqual(a, b, tol, false); + EXPECT(isEqual == true); +} + /* ************************************************************************* */ int main() { TestResult tr; From 326957b0d35ece7ae44cf5d06be42798c9f577dd Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 3 Nov 2020 19:31:39 -0500 Subject: [PATCH 04/43] cleaner assertion --- gtsam/base/tests/testMatrix.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 15087093a..d22ffc0db 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -1169,11 +1169,11 @@ TEST(Matrix, AbsoluteError) { // Test absolute error isEqual = fpEqual(a, b, tol, true); - EXPECT(isEqual == false); + EXPECT(!isEqual); // Test relative error isEqual = fpEqual(a, b, tol, false); - EXPECT(isEqual == true); + EXPECT(isEqual); } /* ************************************************************************* */ From bb313eb7ced9ec799c909611e604139e626cae5f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 7 Nov 2020 18:19:04 -0500 Subject: [PATCH 05/43] Fix python discovery for MATLAB wrapper --- cmake/HandlePython.cmake | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/cmake/HandlePython.cmake b/cmake/HandlePython.cmake index ac7401906..e5d55b451 100644 --- a/cmake/HandlePython.cmake +++ b/cmake/HandlePython.cmake @@ -1,4 +1,5 @@ -if(GTSAM_BUILD_PYTHON) +# Set Python version if either Python or MATLAB wrapper is requested. +if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX) if(${GTSAM_PYTHON_VERSION} STREQUAL "Default") # Get info about the Python3 interpreter # https://cmake.org/cmake/help/latest/module/FindPython3.html#module:FindPython3 @@ -14,7 +15,9 @@ if(GTSAM_BUILD_PYTHON) "The version of Python to build the wrappers against." FORCE) endif() +endif() +if(GTSAM_BUILD_PYTHON) if(GTSAM_UNSTABLE_BUILD_PYTHON) if (NOT GTSAM_BUILD_UNSTABLE) message(WARNING "GTSAM_UNSTABLE_BUILD_PYTHON requires the unstable module to be enabled.") From c5576c534f3a29f0077c58858321829da859dd6c Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 8 Nov 2020 11:57:47 +0100 Subject: [PATCH 06/43] Add iteration hook in non-linear optimizers --- .../NonlinearConjugateGradientOptimizer.h | 4 ++ gtsam/nonlinear/NonlinearOptimizer.cpp | 6 ++- gtsam/nonlinear/NonlinearOptimizer.h | 2 +- gtsam/nonlinear/NonlinearOptimizerParams.h | 30 ++++++----- tests/testNonlinearOptimizer.cpp | 52 +++++++++++++++++++ 5 files changed, 80 insertions(+), 14 deletions(-) diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index abf6b257a..a85f27425 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -200,6 +200,10 @@ boost::tuple nonlinearConjugateGradient(const S &system, currentValues = system.advance(prevValues, alpha, direction); currentError = system.error(currentValues); + // User hook: + if (params.iterationHook) + params.iterationHook(iteration, prevError, currentError); + // Maybe show output if (params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "iteration: " << iteration << ", currentError: " << currentError << std::endl; diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index fd9961742..0d7e9e17f 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -97,7 +97,11 @@ void NonlinearOptimizer::defaultOptimize() { // Update newError for either printouts or conditional-end checks: newError = error(); - + + // User hook: + if (params.iterationHook) + params.iterationHook(iterations(), currentError, newError); + // Maybe show output if (params.verbosity >= NonlinearOptimizerParams::VALUES) values().print("newValues"); diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index 9935f44ce..6fe369dd3 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -81,7 +81,7 @@ protected: public: /** A shared pointer to this class */ - typedef boost::shared_ptr shared_ptr; + using shared_ptr = boost::shared_ptr; /// @name Standard interface /// @{ diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 65fdd1c92..53805f5f0 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -33,22 +33,19 @@ namespace gtsam { */ class GTSAM_EXPORT NonlinearOptimizerParams { public: + NonlinearOptimizerParams() = default; + /** See NonlinearOptimizerParams::verbosity */ enum Verbosity { SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR }; - size_t maxIterations; ///< The maximum iterations to stop iterating (default 100) - double relativeErrorTol; ///< The maximum relative error decrease to stop iterating (default 1e-5) - double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5) - double errorTol; ///< The maximum total error to stop iterating (default 0.0) - Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT) - Ordering::OrderingType orderingType; ///< The method of ordering use during variable elimination (default COLAMD) - - NonlinearOptimizerParams() : - maxIterations(100), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), errorTol( - 0.0), verbosity(SILENT), orderingType(Ordering::COLAMD), - linearSolverType(MULTIFRONTAL_CHOLESKY) {} + size_t maxIterations = 100; ///< The maximum iterations to stop iterating (default 100) + double relativeErrorTol = 1e-5; ///< The maximum relative error decrease to stop iterating (default 1e-5) + double absoluteErrorTol = 1e-5; ///< The maximum absolute error decrease to stop iterating (default 1e-5) + double errorTol = 0.0; ///< The maximum total error to stop iterating (default 0.0) + Verbosity verbosity = SILENT; ///< The printing verbosity during optimization (default SILENT) + Ordering::OrderingType orderingType = Ordering::COLAMD; ///< The method of ordering use during variable elimination (default COLAMD) virtual ~NonlinearOptimizerParams() { } @@ -71,6 +68,15 @@ public: static Verbosity verbosityTranslator(const std::string &s) ; static std::string verbosityTranslator(Verbosity value) ; + /** Type for an optional user-provided hook to be called after each + * internal optimizer iteration */ + using IterationHook = std::function< + void(size_t /*iteration*/, double/*errorBefore*/, double/*errorAfter*/)>; + + /** Optional user-provided iteration hook to be called after each + * optimization iteration (Default: empty) */ + IterationHook iterationHook; + /** See NonlinearOptimizerParams::linearSolverType */ enum LinearSolverType { MULTIFRONTAL_CHOLESKY, @@ -81,7 +87,7 @@ public: CHOLMOD, /* Experimental Flag */ }; - LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer + LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY; ///< The type of linear solver to use in the nonlinear optimizer boost::optional ordering; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty) IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index dc19801a2..6415174d5 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -566,6 +566,58 @@ TEST( NonlinearOptimizer, logfile ) // EXPECT(actual.str()==expected.str()); } +/* ************************************************************************* */ +TEST( NonlinearOptimizer, iterationHook_LM ) +{ + NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph()); + + Point2 x0(3,3); + Values c0; + c0.insert(X(1), x0); + + // Levenberg-Marquardt + LevenbergMarquardtParams lmParams; + size_t lastIterCalled = 0; + lmParams.iterationHook = [&](size_t iteration, double oldError, double newError) + { + // Tests: + lastIterCalled = iteration; + EXPECT(newError " << newError <<"\n"; + }; + LevenbergMarquardtOptimizer(fg, c0, lmParams).optimize(); + + EXPECT(lastIterCalled>5); +} +/* ************************************************************************* */ +TEST( NonlinearOptimizer, iterationHook_CG ) +{ + NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph()); + + Point2 x0(3,3); + Values c0; + c0.insert(X(1), x0); + + // Levenberg-Marquardt + NonlinearConjugateGradientOptimizer::Parameters cgParams; + size_t lastIterCalled = 0; + cgParams.iterationHook = [&](size_t iteration, double oldError, double newError) + { + // Tests: + lastIterCalled = iteration; + EXPECT(newError " << newError <<"\n"; + }; + NonlinearConjugateGradientOptimizer(fg, c0, cgParams).optimize(); + + EXPECT(lastIterCalled>5); +} + + /* ************************************************************************* */ //// Minimal traits example struct MyType : public Vector3 { From 426ad93812b03c8988a4dc8f2e70026c4a43bb38 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 8 Nov 2020 14:47:47 -0500 Subject: [PATCH 07/43] Use new env files for github actions --- .github/workflows/build-linux.yml | 12 ++++++------ .github/workflows/build-macos.yml | 8 ++++---- .github/workflows/build-python.yml | 18 +++++++++--------- .github/workflows/build-special.yml | 26 +++++++++++++------------- .github/workflows/build-windows.yml | 14 +++++++------- 5 files changed, 39 insertions(+), 39 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 87a0430f8..62b81bdf5 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -63,17 +63,17 @@ jobs: sudo apt install cmake build-essential pkg-config libpython-dev python-numpy - echo "::set-env name=BOOST_ROOT::$(echo $BOOST_ROOT_1_69_0)" - echo "::set-env name=LD_LIBRARY_PATH::$(echo $BOOST_ROOT_1_69_0/lib)" + echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_69_0)" >> $GITHUB_ENV + echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_69_0/lib)" >> $GITHUB_ENV if [ "${{ matrix.compiler }}" = "gcc" ]; then sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib - echo "::set-env name=CC::gcc-${{ matrix.version }}" - echo "::set-env name=CXX::g++-${{ matrix.version }}" + echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV + echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV else sudo apt-get install -y clang-${{ matrix.version }} g++-multilib - echo "::set-env name=CC::clang-${{ matrix.version }}" - echo "::set-env name=CXX::clang++-${{ matrix.version }}" + echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV + echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV fi - name: Check Boost version if: runner.os == 'Linux' diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 3ce98055a..69873980a 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -40,12 +40,12 @@ jobs: brew install ProfFan/robotics/boost if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} - echo "::set-env name=CC::gcc-${{ matrix.version }}" - echo "::set-env name=CXX::g++-${{ matrix.version }}" + echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV + echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV else sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app - echo "::set-env name=CC::clang" - echo "::set-env name=CXX::clang++" + echo "CC=clang" >> $GITHUB_ENV + echo "CXX=clang++" >> $GITHUB_ENV fi - name: Build and Test (macOS) if: runner.os == 'macOS' diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 8255fb8ab..97b68f6b7 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -77,12 +77,12 @@ jobs: if [ "${{ matrix.compiler }}" = "gcc" ]; then sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib - echo "::set-env name=CC::gcc-${{ matrix.version }}" - echo "::set-env name=CXX::g++-${{ matrix.version }}" + echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV + echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV else sudo apt-get install -y clang-${{ matrix.version }} g++-multilib - echo "::set-env name=CC::clang-${{ matrix.version }}" - echo "::set-env name=CXX::clang++-${{ matrix.version }}" + echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV + echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV fi - name: Install (macOS) if: runner.os == 'macOS' @@ -92,17 +92,17 @@ jobs: brew install ProfFan/robotics/boost if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} - echo "::set-env name=CC::gcc-${{ matrix.version }}" - echo "::set-env name=CXX::g++-${{ matrix.version }}" + echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV + echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV else sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app - echo "::set-env name=CC::clang" - echo "::set-env name=CXX::clang++" + echo "CC=clang" >> $GITHUB_ENV + echo "CXX=clang++" >> $GITHUB_ENV fi - name: Set GTSAM_WITH_TBB Flag if: matrix.flag == 'tbb' run: | - echo "::set-env name=GTSAM_WITH_TBB::ON" + echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV echo "GTSAM Uses TBB" - name: Build (Linux) if: runner.os == 'Linux' diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index c314acb16..70f3f40ec 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -64,17 +64,17 @@ jobs: sudo apt install cmake build-essential pkg-config libpython-dev python-numpy - echo "::set-env name=BOOST_ROOT::$(echo $BOOST_ROOT_1_69_0)" - echo "::set-env name=LD_LIBRARY_PATH::$(echo $BOOST_ROOT_1_69_0/lib)" + echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_69_0)" >> $GITHUB_ENV + echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_69_0/lib)" >> $GITHUB_ENV if [ "${{ matrix.compiler }}" = "gcc" ]; then sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib - echo "::set-env name=CC::gcc-${{ matrix.version }}" - echo "::set-env name=CXX::g++-${{ matrix.version }}" + echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV + echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV else sudo apt-get install -y clang-${{ matrix.version }} g++-multilib - echo "::set-env name=CC::clang-${{ matrix.version }}" - echo "::set-env name=CXX::clang++-${{ matrix.version }}" + echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV + echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV fi - name: Install (macOS) @@ -83,30 +83,30 @@ jobs: brew install cmake ninja boost if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} - echo "::set-env name=CC::gcc-${{ matrix.version }}" - echo "::set-env name=CXX::g++-${{ matrix.version }}" + echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV + echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV else sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app - echo "::set-env name=CC::clang" - echo "::set-env name=CXX::clang++" + echo "CC=clang" >> $GITHUB_ENV + echo "CXX=clang++" >> $GITHUB_ENV fi - name: Set Allow Deprecated Flag if: matrix.flag == 'deprecated' run: | - echo "::set-env name=GTSAM_ALLOW_DEPRECATED_SINCE_V41::ON" + echo "GTSAM_ALLOW_DEPRECATED_SINCE_V41=ON" >> $GITHUB_ENV echo "Allow deprecated since version 4.1" - name: Set Use Quaternions Flag if: matrix.flag == 'quaternions' run: | - echo "::set-env name=GTSAM_USE_QUATERNIONS::ON" + echo "GTSAM_USE_QUATERNIONS=ON" >> $GITHUB_ENV echo "Use Quaternions for rotations" - name: Set GTSAM_WITH_TBB Flag if: matrix.flag == 'tbb' run: | - echo "::set-env name=GTSAM_WITH_TBB::ON" + echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV echo "GTSAM Uses TBB" - name: Build & Test diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index b3150a751..432f7490d 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -50,17 +50,17 @@ jobs: # See: https://github.com/DaanDeMeyer/doctest/runs/231595515 # See: https://github.community/t5/GitHub-Actions/Something-is-wrong-with-the-chocolatey-installed-version-of-gcc/td-p/32413 scoop install gcc --global - echo "::set-env name=CC::gcc" - echo "::set-env name=CXX::g++" + echo "CC=gcc" >> $GITHUB_ENV + echo "CXX=g++" >> $GITHUB_ENV } elseif ("${{ matrix.compiler }}" -eq "clang") { - echo "::set-env name=CC::clang" - echo "::set-env name=CXX::clang++" + echo "CC=clang" >> $GITHUB_ENV + echo "CXX=clang++" >> $GITHUB_ENV } else { - echo "::set-env name=CC::${{ matrix.compiler }}" - echo "::set-env name=CXX::${{ matrix.compiler }}" + echo "CC=${{ matrix.compiler }}" >> $GITHUB_ENV + echo "CXX=${{ matrix.compiler }}" >> $GITHUB_ENV } # Scoop modifies the PATH so we make the modified PATH global. - echo "::set-env name=PATH::$env:PATH" + echo "$env" >> $GITHUB_PATH - name: Build (Windows) if: runner.os == 'Windows' run: | From 86cea10b558f7911563656b5fefaadf3c1dc2cff Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Nov 2020 17:21:49 -0500 Subject: [PATCH 08/43] ubuntu with gcc-9 in CI --- .github/workflows/build-python.yml | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 8255fb8ab..6cedf0079 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -19,7 +19,7 @@ jobs: # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ ubuntu-18.04-gcc-5, - # ubuntu-18.04-gcc-9, # TODO Disabled for now because of timeouts + ubuntu-18.04-gcc-9, ubuntu-18.04-clang-9, macOS-10.15-xcode-11.3.1, ubuntu-18.04-gcc-5-tbb, @@ -33,11 +33,10 @@ jobs: compiler: gcc version: "5" - # TODO Disabled for now because of timeouts - # - name: ubuntu-18.04-gcc-9 - # os: ubuntu-18.04 - # compiler: gcc - # version: "9" + - name: ubuntu-18.04-gcc-9 + os: ubuntu-18.04 + compiler: gcc + version: "9" - name: ubuntu-18.04-clang-9 os: ubuntu-18.04 From a04cd2cea1b257264784bfd5462c505d59f87aa3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Nov 2020 19:33:39 -0500 Subject: [PATCH 09/43] fix path setting for Windows --- .github/workflows/build-windows.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 432f7490d..920613745 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -60,7 +60,7 @@ jobs: echo "CXX=${{ matrix.compiler }}" >> $GITHUB_ENV } # Scoop modifies the PATH so we make the modified PATH global. - echo "$env" >> $GITHUB_PATH + echo "$env:PATH" >> $GITHUB_PATH - name: Build (Windows) if: runner.os == 'Windows' run: | From 2315df694aff7e648d2e22a478685946e7de4f24 Mon Sep 17 00:00:00 2001 From: Martin Vonheim Larsen Date: Wed, 11 Nov 2020 12:42:19 +0100 Subject: [PATCH 10/43] Docs: Use https for mathjax --- doc/Doxyfile.in | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/Doxyfile.in b/doc/Doxyfile.in index d5c969a8a..fd7f4e5f6 100644 --- a/doc/Doxyfile.in +++ b/doc/Doxyfile.in @@ -1188,7 +1188,7 @@ USE_MATHJAX = YES # MathJax, but it is strongly recommended to install a local copy of MathJax # before deployment. -MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest +MATHJAX_RELPATH = https://cdn.mathjax.org/mathjax/latest # The MATHJAX_EXTENSIONS tag can be used to specify one or MathJax extension # names that should be enabled during MathJax rendering. From c2455082dbb342567181d9126b37ed41214e7a52 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 11 Nov 2020 13:34:40 -0500 Subject: [PATCH 11/43] Force EXPMAP option for both if either POSE3 or ROT3 is set --- cmake/HandleGeneralOptions.cmake | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index 27d73bd86..4bc74804c 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -31,6 +31,13 @@ if(NOT MSVC AND NOT XCODE_VERSION) option(GTSAM_BUILD_WITH_CCACHE "Use ccache compiler cache" ON) endif() +# Enable GTSAM_ROT3_EXPMAP if GTSAM_POSE3_EXPMAP is enabled, and vice versa. +if(GTSAM_POSE3_EXPMAP) + set(GTSAM_ROT3_EXPMAP 1 CACHE BOOL "" FORCE) +elseif(GTSAM_ROT3_EXPMAP) + set(GTSAM_POSE3_EXPMAP 1 CACHE BOOL "" FORCE) +endif() + # Options relating to MATLAB wrapper # TODO: Check for matlab mex binary before handling building of binaries option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) From ddc0d136002c6f3c060691dc4dbdeddf48535bef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 11 Nov 2020 13:34:53 -0500 Subject: [PATCH 12/43] uncomment tests --- gtsam/geometry/tests/testPose3.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 9639ffbcf..8fafcb25f 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -906,9 +906,9 @@ TEST(Pose3 , ChartDerivatives) { Pose3 id; if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) { CHECK_CHART_DERIVATIVES(id,id); -// CHECK_CHART_DERIVATIVES(id,T2); -// CHECK_CHART_DERIVATIVES(T2,id); -// CHECK_CHART_DERIVATIVES(T2,T3); + CHECK_CHART_DERIVATIVES(id,T2); + CHECK_CHART_DERIVATIVES(T2,id); + CHECK_CHART_DERIVATIVES(T2,T3); } } From b5284e4b3fb6933c4443212ba695d1c151961e74 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 11 Nov 2020 22:46:16 -0500 Subject: [PATCH 13/43] Improved CayleyChart Local --- gtsam/geometry/Rot3M.cpp | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 02e5b771f..eb1bd6461 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -176,17 +176,13 @@ Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) { if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative"); // Create a fixed-size matrix Matrix3 A = R.matrix(); - // Mathematica closed form optimization (procrastination?) gone wild: - const double a = A(0, 0), b = A(0, 1), c = A(0, 2); - const double d = A(1, 0), e = A(1, 1), f = A(1, 2); - const double g = A(2, 0), h = A(2, 1), i = A(2, 2); - const double di = d * i, ce = c * e, cd = c * d, fg = f * g; - const double M = 1 + e - f * h + i + e * i; - const double K = -4.0 / (cd * h + M + a * M - g * (c + ce) - b * (d + di - fg)); - const double x = a * f - cd + f; - const double y = b * f - ce - c; - const double z = fg - di - d; - return K * Vector3(x, y, z); + const Matrix3 P = A + I_3x3; + // Check if (A+I) is invertible. Same as checking for -1 eigenvalue. + if (P.determinant() == 0.0) { + throw std::runtime_error("Rot3::CayleyChart::Local Invalid Rotation"); + } + Matrix3 Pinv = (A + I_3x3).inverse(); + return SO3::Vee(Pinv * (A - I_3x3)) * 2; } /* ************************************************************************* */ From 098e7045110d64db1da2424c847c23a62ba056ce Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 11 Nov 2020 22:46:39 -0500 Subject: [PATCH 14/43] Similarity3 test only for Rot3 Expmap --- gtsam_unstable/geometry/tests/testSimilarity3.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index b985eb374..937761f02 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -361,8 +361,12 @@ TEST(Similarity3, AlignPose3) { vector correspondences{bTa1, bTa2}; + // Cayley transform cannot accommodate 180 degree rotations, + // hence we only test for Expmap +#ifdef GTSAM_ROT3_EXPMAP Similarity3 actual_aSb = Similarity3::Align(correspondences); EXPECT(assert_equal(expected_aSb, actual_aSb)); +#endif } //****************************************************************************** From 40b5f31cbdf23a199309b725018b5635b8ddbd2c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Nov 2020 05:42:52 -0500 Subject: [PATCH 15/43] comment out Windows build which is timing out --- .github/workflows/build-windows.yml | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 920613745..74dbbed19 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -25,9 +25,11 @@ jobs: build_type: [Debug, Release] build_unstable: [ON] include: - - name: windows-2016-cl - os: windows-2016 - compiler: cl + + #TODO This build keeps timing out, need to understand why. + # - name: windows-2016-cl + # os: windows-2016 + # compiler: cl - name: windows-2019-cl os: windows-2019 From 9d261626d7a63b8394c9c7feab92d56fcb6c9057 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Nov 2020 06:12:29 -0500 Subject: [PATCH 16/43] Update Boost since 1.69.0 has been deprecated in CI images --- .github/workflows/build-linux.yml | 4 ++-- .github/workflows/build-special.yml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 62b81bdf5..d80a7f4ba 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -63,8 +63,8 @@ jobs: sudo apt install cmake build-essential pkg-config libpython-dev python-numpy - echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_69_0)" >> $GITHUB_ENV - echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_69_0/lib)" >> $GITHUB_ENV + echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_72_0)" >> $GITHUB_ENV + echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_72_0/lib)" >> $GITHUB_ENV if [ "${{ matrix.compiler }}" = "gcc" ]; then sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 70f3f40ec..715f25733 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -64,8 +64,8 @@ jobs: sudo apt install cmake build-essential pkg-config libpython-dev python-numpy - echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_69_0)" >> $GITHUB_ENV - echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_69_0/lib)" >> $GITHUB_ENV + echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_72_0)" >> $GITHUB_ENV + echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_72_0/lib)" >> $GITHUB_ENV if [ "${{ matrix.compiler }}" = "gcc" ]; then sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib From f2b6a0446a173fdbca525dcdca7004bc1129351a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Nov 2020 07:14:30 -0500 Subject: [PATCH 17/43] Comment out Windows 2016 since it times out --- .github/workflows/build-windows.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 74dbbed19..0a55de880 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -18,7 +18,8 @@ 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: [ - windows-2016-cl, + #TODO This build keeps timing out, need to understand why. + # windows-2016-cl, windows-2019-cl, ] From 6bf410c0d8a9180c715d1a87789c0e20ab41bd07 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Nov 2020 10:57:40 -0500 Subject: [PATCH 18/43] Revert "Improved CayleyChart Local" This reverts commit b5284e4b3fb6933c4443212ba695d1c151961e74. --- gtsam/geometry/Rot3M.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index eb1bd6461..02e5b771f 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -176,13 +176,17 @@ Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) { if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative"); // Create a fixed-size matrix Matrix3 A = R.matrix(); - const Matrix3 P = A + I_3x3; - // Check if (A+I) is invertible. Same as checking for -1 eigenvalue. - if (P.determinant() == 0.0) { - throw std::runtime_error("Rot3::CayleyChart::Local Invalid Rotation"); - } - Matrix3 Pinv = (A + I_3x3).inverse(); - return SO3::Vee(Pinv * (A - I_3x3)) * 2; + // Mathematica closed form optimization (procrastination?) gone wild: + const double a = A(0, 0), b = A(0, 1), c = A(0, 2); + const double d = A(1, 0), e = A(1, 1), f = A(1, 2); + const double g = A(2, 0), h = A(2, 1), i = A(2, 2); + const double di = d * i, ce = c * e, cd = c * d, fg = f * g; + const double M = 1 + e - f * h + i + e * i; + const double K = -4.0 / (cd * h + M + a * M - g * (c + ce) - b * (d + di - fg)); + const double x = a * f - cd + f; + const double y = b * f - ce - c; + const double z = fg - di - d; + return K * Vector3(x, y, z); } /* ************************************************************************* */ From 14ef7db636bfd1fc5a16f3dff41fcc7bb0df4a62 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Nov 2020 11:14:54 -0500 Subject: [PATCH 19/43] Use older and faster Cayley transform but add det check and docs --- gtsam/geometry/Rot3M.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 02e5b771f..67b774a74 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -176,7 +176,17 @@ Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) { if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative"); // Create a fixed-size matrix Matrix3 A = R.matrix(); - // Mathematica closed form optimization (procrastination?) gone wild: + + // Check if (A+I) is invertible. Same as checking for -1 eigenvalue. + if ((A + I_3x3).determinant() == 0.0) { + throw std::runtime_error("Rot3::CayleyChart::Local Invalid Rotation"); + } + + // Mathematica closed form optimization. + // The following are the essential computations for the following algorithm + // 1. Compute the inverse of P = (A+I), using a closed-form formula since P is 3x3 + // 2. Compute the Cayley transform C = P^{-1} * (A-I) + // 3. C is skew-symmetric, so we pick out the computations corresponding only to x, y, and z. const double a = A(0, 0), b = A(0, 1), c = A(0, 2); const double d = A(1, 0), e = A(1, 1), f = A(1, 2); const double g = A(2, 0), h = A(2, 1), i = A(2, 2); From a94b6dacaf9309dfc125a9b44f5eb71911d0da83 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Nov 2020 11:15:50 -0500 Subject: [PATCH 20/43] Print message when either Pose3 or Rot3 expmap is ON --- cmake/HandleGeneralOptions.cmake | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index 4bc74804c..85a529e49 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -33,8 +33,10 @@ endif() # Enable GTSAM_ROT3_EXPMAP if GTSAM_POSE3_EXPMAP is enabled, and vice versa. if(GTSAM_POSE3_EXPMAP) + message(STATUS "GTSAM_POSE3_EXPMAP=ON, enabling GTSAM_ROT3_EXPMAP as well") set(GTSAM_ROT3_EXPMAP 1 CACHE BOOL "" FORCE) elseif(GTSAM_ROT3_EXPMAP) + message(STATUS "GTSAM_ROT3_EXPMAP=ON, enabling GTSAM_POSE3_EXPMAP as well") set(GTSAM_POSE3_EXPMAP 1 CACHE BOOL "" FORCE) endif() From 4e3ac1b8398a66e1824947d7a1a25ae21821d362 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Nov 2020 11:16:50 -0500 Subject: [PATCH 21/43] CI path for Cayley transform --- .github/scripts/unix.sh | 2 ++ .github/workflows/build-special.yml | 14 ++++++++++++++ 2 files changed, 16 insertions(+) diff --git a/.github/scripts/unix.sh b/.github/scripts/unix.sh index f85e67dc1..55a8ac372 100644 --- a/.github/scripts/unix.sh +++ b/.github/scripts/unix.sh @@ -66,6 +66,8 @@ function configure() -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V41=${GTSAM_ALLOW_DEPRECATED_SINCE_V41:-OFF} \ -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \ + -DGTSAM_ROT3_EXPMAP=${GTSAM_ROT3_EXPMAP:-ON} \ + -DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \ -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ -DBOOST_ROOT=$BOOST_ROOT \ -DBoost_NO_SYSTEM_PATHS=ON \ diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 715f25733..532f917c1 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -24,6 +24,7 @@ jobs: ubuntu-gcc-deprecated, ubuntu-gcc-quaternions, ubuntu-gcc-tbb, + ubuntu-cayleymap, ] build_type: [Debug, Release] @@ -47,6 +48,12 @@ jobs: version: "9" flag: tbb + - name: ubuntu-cayleymap + os: ubuntu-18.04 + compiler: gcc + version: "9" + flag: cayley + steps: - name: Checkout uses: actions/checkout@master @@ -109,6 +116,13 @@ jobs: echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV echo "GTSAM Uses TBB" + - name: Use Cayley Transform for Rot3 + if: matrix.flag == 'cayley' + run: | + echo "GTSAM_POSE3_EXPMAP=OFF" >> $GITHUB_ENV + echo "GTSAM_ROT3_EXPMAP=OFF" >> $GITHUB_ENV + echo "GTSAM Uses Cayley map for Rot3" + - name: Build & Test run: | bash .github/scripts/unix.sh -t From 9e421c407bac3307ed8d552f01352fa7a3a23211 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 12 Nov 2020 18:17:16 -0500 Subject: [PATCH 22/43] small doc fix --- gtsam/geometry/Rot3M.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 67b774a74..07cc7302a 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -185,7 +185,7 @@ Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) { // Mathematica closed form optimization. // The following are the essential computations for the following algorithm // 1. Compute the inverse of P = (A+I), using a closed-form formula since P is 3x3 - // 2. Compute the Cayley transform C = P^{-1} * (A-I) + // 2. Compute the Cayley transform C = 2 * P^{-1} * (A-I) // 3. C is skew-symmetric, so we pick out the computations corresponding only to x, y, and z. const double a = A(0, 0), b = A(0, 1), c = A(0, 2); const double d = A(1, 0), e = A(1, 1), f = A(1, 2); From c86010d8740fb3af052be32a11e025ba7261f361 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 15 Nov 2020 18:15:52 -0500 Subject: [PATCH 23/43] Add new assertions and update tests --- gtsam/base/TestableAssertions.h | 41 +++++++++++++++++++ gtsam/geometry/tests/testCal3_S2.cpp | 11 +++++ gtsam/geometry/tests/testPose3.cpp | 26 ++---------- .../nonlinear/tests/testFunctorizedFactor.cpp | 13 +----- gtsam/nonlinear/tests/testValues.cpp | 10 +---- 5 files changed, 59 insertions(+), 42 deletions(-) diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 1a31aa284..88dd619e5 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -23,6 +23,7 @@ #include #include #include +#include #include namespace gtsam { @@ -349,4 +350,44 @@ bool assert_inequal(const V& expected, const V& actual, double tol = 1e-9) { return false; } +/** + * Capture std out via cout stream and compare against string. + */ +template +bool assert_stdout_equal(const std::string& expected, const V& actual) { + // Redirect output to buffer so we can compare + std::stringstream buffer; + // Save the original output stream so we can reset later + std::streambuf* old = std::cout.rdbuf(buffer.rdbuf()); + + // We test against actual std::cout for faithful reproduction + std::cout << actual; + + // Get output string and reset stdout + std::string actual_ = buffer.str(); + std::cout.rdbuf(old); + + return assert_equal(expected, actual_); +} + +/** + * Capture print function output and compare against string. + */ +template +bool assert_print_equal(const std::string& expected, const V& actual) { + // Redirect output to buffer so we can compare + std::stringstream buffer; + // Save the original output stream so we can reset later + std::streambuf* old = std::cout.rdbuf(buffer.rdbuf()); + + // We test against actual std::cout for faithful reproduction + actual.print(); + + // Get output string and reset stdout + std::string actual_ = buffer.str(); + std::cout.rdbuf(old); + + return assert_equal(expected, actual_); +} + } // \namespace gtsam diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index ccecb09c3..55ea32e32 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -127,6 +128,16 @@ TEST(Cal3_S2, between) { } +/* ************************************************************************* */ +TEST(Cal3_S2, Print) { + Cal3_S2 cal(5, 5, 5, 5, 5); + std::stringstream os; + os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew() << ", px:" << cal.px() + << ", py:" << cal.py() << "}"; + + EXPECT(assert_stdout_equal(os.str(), cal)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 8fafcb25f..6de2c0a33 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include // for operator += using namespace boost::assign; @@ -1028,32 +1029,13 @@ TEST(Pose3, Create) { } /* ************************************************************************* */ -TEST(Pose3, print) { - std::stringstream redirectStream; - std::streambuf* ssbuf = redirectStream.rdbuf(); - std::streambuf* oldbuf = std::cout.rdbuf(); - // redirect cout to redirectStream - std::cout.rdbuf(ssbuf); - +TEST(Pose3, Print) { Pose3 pose(Rot3::identity(), Point3(1, 2, 3)); - // output is captured to redirectStream - pose.print(); // Generate the expected output - std::stringstream expected; - Point3 translation(1, 2, 3); + std::string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 1 2 3\n"; - // Add expected rotation - expected << "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n"; - expected << "t: 1 2 3\n"; - - // reset cout to the original stream - std::cout.rdbuf(oldbuf); - - // Get substring corresponding to translation part - std::string actual = redirectStream.str(); - - CHECK_EQUAL(expected.str(), actual); + EXPECT(assert_print_equal(expected, pose)); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp index 48ab73ad0..cb91320cf 100644 --- a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp +++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -115,16 +116,6 @@ TEST(FunctorizedFactor, Print) { auto factor = MakeFunctorizedFactor(key, X, model, MultiplyFunctor(multiplier)); - // redirect output to buffer so we can compare - stringstream buffer; - streambuf *old = cout.rdbuf(buffer.rdbuf()); - - factor.print(); - - // get output string and reset stdout - string actual = buffer.str(); - cout.rdbuf(old); - string expected = " keys = { X0 }\n" " noise model: unit (9) \n" @@ -135,7 +126,7 @@ TEST(FunctorizedFactor, Print) { "]\n" " noise model sigmas: 1 1 1 1 1 1 1 1 1\n"; - CHECK_EQUAL(expected, actual); + EXPECT(assert_print_equal(expected, factor)); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 09b358efb..88cfb666f 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -595,15 +595,7 @@ TEST(Values, Demangle) { values.insert(key1, v); string expected = "Values with 1 values:\nValue v1: (Eigen::Matrix)\n[\n 5, 6, 7\n]\n\n"; - stringstream buffer; - streambuf * old = cout.rdbuf(buffer.rdbuf()); - - values.print(); - - string actual = buffer.str(); - cout.rdbuf(old); - - EXPECT(assert_equal(expected, actual)); + EXPECT(assert_print_equal(expected, values)); } /* ************************************************************************* */ From f2bfdbd317b02dfc75143c6c34c91b7f16a72f6f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 15 Nov 2020 18:40:02 -0500 Subject: [PATCH 24/43] upload build directory after workflow completes --- .github/workflows/build-linux.yml | 7 ++++++- .github/workflows/build-macos.yml | 5 +++++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index d80a7f4ba..07397c999 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -82,4 +82,9 @@ jobs: - name: Build and Test (Linux) if: runner.os == 'Linux' run: | - bash .github/scripts/unix.sh -t \ No newline at end of file + bash .github/scripts/unix.sh -t + - name: Upload build directory + uses: actions/upload-artifact@v2 + with: + name: gtsam-${{ matrix.name }} + path: $GITHUB_WORKSPACE/build/ diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 69873980a..524cef18a 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -51,3 +51,8 @@ jobs: if: runner.os == 'macOS' run: | bash .github/scripts/unix.sh -t + - name: Upload build directory + uses: actions/upload-artifact@v2 + with: + name: gtsam-${{ matrix.name }} + path: $GITHUB_WORKSPACE/build/ From 91d275c9c74c955bb1ff86ea9b213e0344b796de Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Mon, 16 Nov 2020 07:25:30 +0100 Subject: [PATCH 25/43] Add docs, fix ctor placement --- gtsam/nonlinear/NonlinearOptimizerParams.h | 40 ++++++++++++++++------ 1 file changed, 30 insertions(+), 10 deletions(-) diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 53805f5f0..92c800e0c 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -33,8 +33,6 @@ namespace gtsam { */ class GTSAM_EXPORT NonlinearOptimizerParams { public: - NonlinearOptimizerParams() = default; - /** See NonlinearOptimizerParams::verbosity */ enum Verbosity { SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR @@ -47,10 +45,6 @@ public: Verbosity verbosity = SILENT; ///< The printing verbosity during optimization (default SILENT) Ordering::OrderingType orderingType = Ordering::COLAMD; ///< The method of ordering use during variable elimination (default COLAMD) - virtual ~NonlinearOptimizerParams() { - } - virtual void print(const std::string& str = "") const; - size_t getMaxIterations() const { return maxIterations; } double getRelativeErrorTol() const { return relativeErrorTol; } double getAbsoluteErrorTol() const { return absoluteErrorTol; } @@ -68,13 +62,33 @@ public: static Verbosity verbosityTranslator(const std::string &s) ; static std::string verbosityTranslator(Verbosity value) ; - /** Type for an optional user-provided hook to be called after each - * internal optimizer iteration */ + /** Type for an optional user-provided hook to be called after each + * internal optimizer iteration. See iterationHook below. */ using IterationHook = std::function< void(size_t /*iteration*/, double/*errorBefore*/, double/*errorAfter*/)>; - /** Optional user-provided iteration hook to be called after each - * optimization iteration (Default: empty) */ + /** Optional user-provided iteration hook to be called after each + * optimization iteration (Default: none). + * Note that `IterationHook` is defined as a std::function<> with this + * signature: + * \code + * void(size_t iteration, double errorBefore, double errorAfter) + * \endcode + * which allows binding by means of a reference to a regular function: + * \code + * void foo(size_t iteration, double errorBefore, double errorAfter); + * // ... + * lmOpts.iterationHook = &foo; + * \endcode + * or to a C++11 lambda: + * \code + * lmOpts.iterationHook = [&](size_t iter, double oldError, double newError) + * { + * // ... + * }; + * \endcode + * or to the result of a properly-formed `std::bind` call. + */ IterationHook iterationHook; /** See NonlinearOptimizerParams::linearSolverType */ @@ -91,6 +105,12 @@ public: boost::optional ordering; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty) IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. + NonlinearOptimizerParams() = default; + virtual ~NonlinearOptimizerParams() { + } + + virtual void print(const std::string& str = "") const; + inline bool isMultifrontal() const { return (linearSolverType == MULTIFRONTAL_CHOLESKY) || (linearSolverType == MULTIFRONTAL_QR); From 9a4bd10e222918cfd984d793a04fad24741b7c21 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Mon, 16 Nov 2020 07:32:03 +0100 Subject: [PATCH 26/43] further extended docs --- gtsam/nonlinear/NonlinearOptimizerParams.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 92c800e0c..a7bc10a1f 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -80,7 +80,9 @@ public: * // ... * lmOpts.iterationHook = &foo; * \endcode - * or to a C++11 lambda: + * or to a C++11 lambda (preferred if you need to capture additional + * context variables, such that the optimizer object itself, the factor graph, + * etc.): * \code * lmOpts.iterationHook = [&](size_t iter, double oldError, double newError) * { From ac089ce6a37d6c6becded9ac4bee19f8cf1fe15c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Nov 2020 12:22:09 -0500 Subject: [PATCH 27/43] use KeyVector to allow proper wrapping with TBB --- gtsam/sfm/MFAS.cpp | 6 +++--- gtsam/sfm/MFAS.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp index 4cd983ecd..913752d8a 100644 --- a/gtsam/sfm/MFAS.cpp +++ b/gtsam/sfm/MFAS.cpp @@ -121,8 +121,8 @@ MFAS::MFAS(const TranslationEdges& relativeTranslations, } } -vector MFAS::computeOrdering() const { - vector ordering; // Nodes in MFAS order (result). +KeyVector MFAS::computeOrdering() const { + KeyVector ordering; // Nodes in MFAS order (result). // A graph is an unordered map from keys to nodes. Each node contains a list // of its adjacent nodes. Create the graph from the edgeWeights. @@ -140,7 +140,7 @@ vector MFAS::computeOrdering() const { map MFAS::computeOutlierWeights() const { // Find the ordering. - vector ordering = computeOrdering(); + KeyVector ordering = computeOrdering(); // Create a map from the node key to its position in the ordering. This makes // it easier to lookup positions of different nodes. diff --git a/gtsam/sfm/MFAS.h b/gtsam/sfm/MFAS.h index 3b01122a9..decfbed0f 100644 --- a/gtsam/sfm/MFAS.h +++ b/gtsam/sfm/MFAS.h @@ -84,7 +84,7 @@ class MFAS { * @brief Computes the 1D MFAS ordering of nodes in the graph * @return orderedNodes: vector of nodes in the obtained order */ - std::vector computeOrdering() const; + KeyVector computeOrdering() const; /** * @brief Computes the outlier weights of the graph. We define the outlier From 283582cd5d88e3a10ae8c9a619fb25b379029927 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Nov 2020 12:31:44 -0500 Subject: [PATCH 28/43] update MFAS tests --- gtsam/sfm/tests/testMFAS.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/sfm/tests/testMFAS.cpp b/gtsam/sfm/tests/testMFAS.cpp index 362027d5d..a1f6dfa5f 100644 --- a/gtsam/sfm/tests/testMFAS.cpp +++ b/gtsam/sfm/tests/testMFAS.cpp @@ -25,7 +25,7 @@ using namespace gtsam; vector edges = {make_pair(3, 2), make_pair(0, 1), make_pair(3, 1), make_pair(1, 2), make_pair(0, 2), make_pair(3, 0)}; // nodes in the graph -vector nodes = {Key(0), Key(1), Key(2), Key(3)}; +KeyVector nodes = {Key(0), Key(1), Key(2), Key(3)}; // weights from projecting in direction-1 (bad direction, outlier accepted) vector weights1 = {2, 1.5, 0.5, 0.25, 1, 0.75}; // weights from projecting in direction-2 (good direction, outlier rejected) @@ -47,10 +47,10 @@ map getEdgeWeights(const vector &edges, TEST(MFAS, OrderingWeights2) { MFAS mfas_obj(getEdgeWeights(edges, weights2)); - vector ordered_nodes = mfas_obj.computeOrdering(); + KeyVector ordered_nodes = mfas_obj.computeOrdering(); // ground truth (expected) ordering in this example - vector gt_ordered_nodes = {0, 1, 3, 2}; + KeyVector gt_ordered_nodes = {0, 1, 3, 2}; // check if the expected ordering is obtained for (size_t i = 0; i < ordered_nodes.size(); i++) { @@ -77,10 +77,10 @@ TEST(MFAS, OrderingWeights2) { TEST(MFAS, OrderingWeights1) { MFAS mfas_obj(getEdgeWeights(edges, weights1)); - vector ordered_nodes = mfas_obj.computeOrdering(); + KeyVector ordered_nodes = mfas_obj.computeOrdering(); // "ground truth" expected ordering in this example - vector gt_ordered_nodes = {3, 0, 1, 2}; + KeyVector gt_ordered_nodes = {3, 0, 1, 2}; // check if the expected ordering is obtained for (size_t i = 0; i < ordered_nodes.size(); i++) { From 5c94b5ccc70d374a49f024dcb24bcec94382c817 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Nov 2020 13:11:10 -0500 Subject: [PATCH 29/43] fix indentation and add upload for Windows --- .github/workflows/build-linux.yml | 6 +++--- .github/workflows/build-macos.yml | 6 +++--- .github/workflows/build-windows.yml | 7 ++++++- 3 files changed, 12 insertions(+), 7 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 07397c999..514f7c6d5 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -85,6 +85,6 @@ jobs: bash .github/scripts/unix.sh -t - name: Upload build directory uses: actions/upload-artifact@v2 - with: - name: gtsam-${{ matrix.name }} - path: $GITHUB_WORKSPACE/build/ + with: + name: gtsam-${{ matrix.name }} + path: $GITHUB_WORKSPACE/build/ diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 524cef18a..298fc316b 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -53,6 +53,6 @@ jobs: bash .github/scripts/unix.sh -t - name: Upload build directory uses: actions/upload-artifact@v2 - with: - name: gtsam-${{ matrix.name }} - path: $GITHUB_WORKSPACE/build/ + with: + name: gtsam-${{ matrix.name }} + path: $GITHUB_WORKSPACE/build/ diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 0a55de880..446e708a6 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -75,4 +75,9 @@ jobs: cmake --build build --config ${{ matrix.build_type }} --target wrap cmake --build build --config ${{ matrix.build_type }} --target check.base cmake --build build --config ${{ matrix.build_type }} --target check.base_unstable - cmake --build build --config ${{ matrix.build_type }} --target check.linear \ No newline at end of file + cmake --build build --config ${{ matrix.build_type }} --target check.linear + - name: Upload build directory + uses: actions/upload-artifact@v2 + with: + name: gtsam-${{ matrix.name }} + path: $GITHUB_WORKSPACE/build/ From aa2d9225dcc52848342d68435b28dc5995698700 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Nov 2020 14:42:17 -0500 Subject: [PATCH 30/43] correct form for workspace env variable --- .github/workflows/build-linux.yml | 2 +- .github/workflows/build-macos.yml | 2 +- .github/workflows/build-windows.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 514f7c6d5..1778286d9 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -87,4 +87,4 @@ jobs: uses: actions/upload-artifact@v2 with: name: gtsam-${{ matrix.name }} - path: $GITHUB_WORKSPACE/build/ + path: ${{ github.workspace }}/build/ diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 298fc316b..2a3af6b9e 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -55,4 +55,4 @@ jobs: uses: actions/upload-artifact@v2 with: name: gtsam-${{ matrix.name }} - path: $GITHUB_WORKSPACE/build/ + path: ${{ github.workspace }}/build/ diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 446e708a6..1de0d0493 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -80,4 +80,4 @@ jobs: uses: actions/upload-artifact@v2 with: name: gtsam-${{ matrix.name }} - path: $GITHUB_WORKSPACE/build/ + path: ${{ github.workspace }}/build/ From d5a09aad12ef04392cacb8b8bfe1589a66e42fca Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Nov 2020 17:47:42 -0500 Subject: [PATCH 31/43] differentiate between Release and Debug builds --- .github/workflows/build-linux.yml | 2 +- .github/workflows/build-macos.yml | 2 +- .github/workflows/build-windows.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 1778286d9..cd4933cd9 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -86,5 +86,5 @@ jobs: - name: Upload build directory uses: actions/upload-artifact@v2 with: - name: gtsam-${{ matrix.name }} + name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 2a3af6b9e..5e6f8ca04 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -54,5 +54,5 @@ jobs: - name: Upload build directory uses: actions/upload-artifact@v2 with: - name: gtsam-${{ matrix.name }} + name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 1de0d0493..63c276020 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -79,5 +79,5 @@ jobs: - name: Upload build directory uses: actions/upload-artifact@v2 with: - name: gtsam-${{ matrix.name }} + name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ From 1fd0e57fb0a2a9cd24accd3d8dc04d91328dccf6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Nov 2020 14:48:05 -0500 Subject: [PATCH 32/43] Better fkag naming, and more docs --- gtsam/base/Matrix.h | 2 +- gtsam/base/Vector.cpp | 14 +++++++------- gtsam/base/Vector.h | 5 ++++- gtsam/base/tests/testMatrix.cpp | 10 +++++----- 4 files changed, 17 insertions(+), 14 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 9adf4e1c1..a3a14c6c3 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -90,7 +90,7 @@ bool equal_with_abs_tol(const Eigen::DenseBase& A, const Eigen::DenseBas for(size_t i=0; i abs(a)) ? abs(b) : abs(a); // handle NaNs - if(std::isnan(a) || isnan(b)) { + if(isnan(a) || isnan(b)) { return isnan(a) && isnan(b); } // handle inf @@ -60,15 +60,15 @@ bool fpEqual(double a, double b, double tol, bool absolute) { else if(a == 0 || b == 0 || (abs(a) + abs(b)) < DOUBLE_MIN_NORMAL) { return abs(a-b) <= tol * DOUBLE_MIN_NORMAL; } - // Check if the numbers are really close - // Needed when comparing numbers near zero or tol is in vicinity - else if(abs(a-b) <= tol) { + // Check if the numbers are really close. + // Needed when comparing numbers near zero or tol is in vicinity. + else if (abs(a - b) <= tol) { return true; } - // Use relative error + // Check for relative error else if (abs(a - b) <= tol * min(larger, std::numeric_limits::max()) && - !absolute) { + check_relative) { return true; } diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index b0fc74f26..49b7e6d9d 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -85,10 +85,13 @@ static_assert( * respectively for the comparison to be true. * If one is NaN/Inf and the other is not, returns false. * + * The `check_relative` flag toggles checking for relative error as well. By + * default, the flag is true. + * * Return true if two numbers are close wrt tol. */ GTSAM_EXPORT bool fpEqual(double a, double b, double tol, - bool absolute = false); + bool check_relative = true); /** * print without optional string, must specify cout yourself diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index d22ffc0db..a7c218705 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -1164,15 +1164,15 @@ TEST(Matrix , IsVectorSpace) { } TEST(Matrix, AbsoluteError) { - double a = 2000, b = 1997, tol=1e-1; + double a = 2000, b = 1997, tol = 1e-1; bool isEqual; - // Test absolute error - isEqual = fpEqual(a, b, tol, true); + // Test only absolute error + isEqual = fpEqual(a, b, tol, false); EXPECT(!isEqual); - // Test relative error - isEqual = fpEqual(a, b, tol, false); + // Test relative error as well + isEqual = fpEqual(a, b, tol); EXPECT(isEqual); } From 30afc95936ffb360d99012b08fb623e7d6f8ebc2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Nov 2020 16:05:13 -0500 Subject: [PATCH 33/43] placed MEX check to new cmake file --- cmake/HandleGeneralOptions.cmake | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index 85a529e49..fb1e4e8d2 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -46,10 +46,17 @@ option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of Python to build the wrappers against.") # Check / set dependent variables for MATLAB wrapper -if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_TYPE_POSTFIXES) - set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX}) -endif() +if(GTSAM_INSTALL_MATLAB_TOOLBOX) + find_package(Matlab COMPONENTS MEX_COMPILER REQUIRED) + if(NOT Matlab_MEX_COMPILER) + message(FATAL_ERROR "Cannot find MEX compiler binary. Please check your Matlab installation and ensure MEX in installed as well.") + endif() -if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS) - message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.") + if(GTSAM_BUILD_TYPE_POSTFIXES) + set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX}) + endif() + + if(NOT BUILD_SHARED_LIBS) + message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.") + endif() endif() From 812240b056abfafe74cb3120c244068c495442c4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Nov 2020 16:08:06 -0500 Subject: [PATCH 34/43] restructuring --- cmake/HandleGeneralOptions.cmake | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index fb1e4e8d2..469d4bba6 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -24,6 +24,7 @@ option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF) option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module with pybind11" OFF) +option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON) option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) @@ -40,9 +41,7 @@ elseif(GTSAM_ROT3_EXPMAP) set(GTSAM_POSE3_EXPMAP 1 CACHE BOOL "" FORCE) endif() -# Options relating to MATLAB wrapper -# TODO: Check for matlab mex binary before handling building of binaries -option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) +# Set the default Python version. This is later updated in HandlePython.cmake. set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of Python to build the wrappers against.") # Check / set dependent variables for MATLAB wrapper From 628ae264968866dd1281a3c3710c90e6e667c8ea Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Nov 2020 16:29:53 -0500 Subject: [PATCH 35/43] encapsulated and updated all the CMake related to Matlab --- cmake/GtsamMatlabWrap.cmake | 77 +++++++++++++++++++------------- cmake/HandleGeneralOptions.cmake | 16 ------- 2 files changed, 45 insertions(+), 48 deletions(-) diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake index 4c44d2cb3..b17618f49 100644 --- a/cmake/GtsamMatlabWrap.cmake +++ b/cmake/GtsamMatlabWrap.cmake @@ -1,51 +1,64 @@ +# Check / set dependent variables for MATLAB wrapper +if(GTSAM_INSTALL_MATLAB_TOOLBOX) + find_package(Matlab COMPONENTS MEX_COMPILER REQUIRED) + if(NOT Matlab_MEX_COMPILER) + message(FATAL_ERROR "Cannot find MEX compiler binary. Please check your Matlab installation and ensure MEX in installed as well.") + endif() + + if(GTSAM_BUILD_TYPE_POSTFIXES) + set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX}) + endif() + + if(NOT BUILD_SHARED_LIBS) + message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.") + endif() +endif() + # Set up cache options option(GTSAM_MEX_BUILD_STATIC_MODULE "Build MATLAB wrapper statically (increases build time)" OFF) set(GTSAM_BUILD_MEX_BINARY_FLAGS "" CACHE STRING "Extra flags for running Matlab MEX compilation") set(GTSAM_TOOLBOX_INSTALL_PATH "" CACHE PATH "Matlab toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/gtsam_toolbox") if(NOT GTSAM_TOOLBOX_INSTALL_PATH) - set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/gtsam_toolbox") + set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/gtsam_toolbox") endif() # GTSAM_MEX_BUILD_STATIC_MODULE is not for Windows - on Windows any static # are already compiled into the library by the linker if(GTSAM_MEX_BUILD_STATIC_MODULE AND WIN32) - message(FATAL_ERROR "GTSAM_MEX_BUILD_STATIC_MODULE should not be set on Windows - the linker already automatically compiles in any dependent static libraries. To create a standalone toolbox pacakge, simply ensure that CMake finds the static versions of all dependent libraries (Boost, etc).") + message(FATAL_ERROR "GTSAM_MEX_BUILD_STATIC_MODULE should not be set on Windows - the linker already automatically compiles in any dependent static libraries. To create a standalone toolbox pacakge, simply ensure that CMake finds the static versions of all dependent libraries (Boost, etc).") endif() -# Try to automatically configure mex path -if(APPLE) - file(GLOB matlab_bin_directories "/Applications/MATLAB*/bin") - set(mex_program_name "mex") -elseif(WIN32) - file(GLOB matlab_bin_directories "C:/Program Files*/MATLAB/*/bin") - set(mex_program_name "mex.bat") -else() - file(GLOB matlab_bin_directories "/usr/local/MATLAB/*/bin") - set(mex_program_name "mex") -endif() +set(MEX_COMMAND ${Matlab_MEX_COMPILER} CACHE PATH "Path to MATLAB MEX compiler") +set(MATLAB_ROOT ${Matlab_ROOT_DIR} CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)") +# Try to automatically configure mex path from provided custom `bin` path. if(GTSAM_CUSTOM_MATLAB_PATH) - set(matlab_bin_directories ${GTSAM_CUSTOM_MATLAB_PATH}) -endif() + set(matlab_bin_directory ${GTSAM_CUSTOM_MATLAB_PATH}) -# Run find_program explicitly putting $PATH after our predefined program -# directories using 'ENV PATH' and 'NO_SYSTEM_ENVIRONMENT_PATH' - this prevents -# finding the LaTeX mex program (totally unrelated to MATLAB Mex) when LaTeX is -# on the system path. -list(REVERSE matlab_bin_directories) # Reverse list so the highest version (sorted alphabetically) is preferred -find_program(MEX_COMMAND ${mex_program_name} - PATHS ${matlab_bin_directories} ENV PATH - NO_DEFAULT_PATH) -mark_as_advanced(FORCE MEX_COMMAND) -# Now that we have mex, trace back to find the Matlab installation root -get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH) -get_filename_component(mex_path "${MEX_COMMAND}" PATH) -if(mex_path MATCHES ".*/win64$") - get_filename_component(MATLAB_ROOT "${mex_path}/../.." ABSOLUTE) -else() - get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE) + if(WIN32) + set(mex_program_name "mex.bat") + else() + set(mex_program_name "mex") + endif() + + # Run find_program explicitly putting $PATH after our predefined program + # directories using 'ENV PATH' and 'NO_SYSTEM_ENVIRONMENT_PATH' - this prevents + # finding the LaTeX mex program (totally unrelated to MATLAB Mex) when LaTeX is + # on the system path. + find_program(MEX_COMMAND ${mex_program_name} + PATHS ${matlab_bin_directory} ENV PATH + NO_DEFAULT_PATH) + + mark_as_advanced(FORCE MEX_COMMAND) + # Now that we have mex, trace back to find the Matlab installation root + get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH) + get_filename_component(mex_path "${MEX_COMMAND}" PATH) + if(mex_path MATCHES ".*/win64$") + get_filename_component(MATLAB_ROOT "${mex_path}/../.." ABSOLUTE) + else() + get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE) + endif() endif() -set(MATLAB_ROOT "${MATLAB_ROOT}" CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)") # User-friendly wrapping function. Builds a mex module from the provided diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index 469d4bba6..ee86066a2 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -43,19 +43,3 @@ endif() # Set the default Python version. This is later updated in HandlePython.cmake. set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of Python to build the wrappers against.") - -# Check / set dependent variables for MATLAB wrapper -if(GTSAM_INSTALL_MATLAB_TOOLBOX) - find_package(Matlab COMPONENTS MEX_COMPILER REQUIRED) - if(NOT Matlab_MEX_COMPILER) - message(FATAL_ERROR "Cannot find MEX compiler binary. Please check your Matlab installation and ensure MEX in installed as well.") - endif() - - if(GTSAM_BUILD_TYPE_POSTFIXES) - set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX}) - endif() - - if(NOT BUILD_SHARED_LIBS) - message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.") - endif() -endif() From 0737a4594a1c2a5eb814b6da060ffcab8749d1f6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Nov 2020 07:44:42 -0500 Subject: [PATCH 36/43] better flag name and docs --- gtsam/base/Vector.cpp | 4 ++-- gtsam/base/Vector.h | 8 +++++--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index d2f3ae868..658ab9a0d 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -39,7 +39,7 @@ namespace gtsam { * 1. https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/ * 2. https://floating-point-gui.de/errors/comparison/ * ************************************************************************* */ -bool fpEqual(double a, double b, double tol, bool check_relative) { +bool fpEqual(double a, double b, double tol, bool check_relative_also) { using std::abs; using std::isnan; using std::isinf; @@ -68,7 +68,7 @@ bool fpEqual(double a, double b, double tol, bool check_relative) { // Check for relative error else if (abs(a - b) <= tol * min(larger, std::numeric_limits::max()) && - check_relative) { + check_relative_also) { return true; } diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 49b7e6d9d..ed90a7126 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -85,13 +85,15 @@ static_assert( * respectively for the comparison to be true. * If one is NaN/Inf and the other is not, returns false. * - * The `check_relative` flag toggles checking for relative error as well. By - * default, the flag is true. + * @param check_relative_also is a flag which toggles additional checking for + * relative error. This means that if either the absolute error or the relative + * error is within the tolerance, the result will be true. + * By default, the flag is true. * * Return true if two numbers are close wrt tol. */ GTSAM_EXPORT bool fpEqual(double a, double b, double tol, - bool check_relative = true); + bool check_relative_also = true); /** * print without optional string, must specify cout yourself From 9e2007562a8448065224a30c72240ea4aea1b643 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Nov 2020 08:04:30 -0500 Subject: [PATCH 37/43] only upload release builds --- .github/workflows/build-linux.yml | 1 + .github/workflows/build-macos.yml | 1 + .github/workflows/build-windows.yml | 1 + 3 files changed, 3 insertions(+) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index cd4933cd9..940e34b82 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -85,6 +85,7 @@ jobs: bash .github/scripts/unix.sh -t - name: Upload build directory uses: actions/upload-artifact@v2 + if: ${{ matrix.build_type }} == "Release" with: name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 5e6f8ca04..cccfd7b5b 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -53,6 +53,7 @@ jobs: bash .github/scripts/unix.sh -t - name: Upload build directory uses: actions/upload-artifact@v2 + if: ${{ matrix.build_type }} == "Release" with: name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 63c276020..a14d7de5e 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -78,6 +78,7 @@ jobs: cmake --build build --config ${{ matrix.build_type }} --target check.linear - name: Upload build directory uses: actions/upload-artifact@v2 + if: ${{ matrix.build_type }} == "Release" with: name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ From 802580eec79d5c39995af170e40a7c0ba1eb54e6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Nov 2020 12:40:38 -0500 Subject: [PATCH 38/43] enforce constant term in quadratic to be 0 --- gtsam_unstable/linear/QPSParser.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/gtsam_unstable/linear/QPSParser.cpp b/gtsam_unstable/linear/QPSParser.cpp index 3039185f2..df21c0132 100644 --- a/gtsam_unstable/linear/QPSParser.cpp +++ b/gtsam_unstable/linear/QPSParser.cpp @@ -81,7 +81,7 @@ class QPSVisitor { varname_to_key; // Variable QPS string name to key std::unordered_map> H; // H from hessian - double f; // Constant term of quadratic cost + double f = 0; // Constant term of quadratic cost std::string obj_name; // the objective function has a name in the QPS std::string name_; // the quadratic program has a name in the QPS std::unordered_map @@ -175,10 +175,11 @@ class QPSVisitor { string var_ = fromChars<1>(vars); string row_ = fromChars<3>(vars); double coefficient = at_c<5>(vars); - if (row_ == obj_name) + if (row_ == obj_name) { f = -coefficient; - else + } else { b[row_] = coefficient; + } if (debug) { cout << "Added RHS for Var: " << var_ << " Row: " << row_ @@ -194,15 +195,17 @@ class QPSVisitor { string row2_ = fromChars<7>(vars); double coefficient1 = at_c<5>(vars); double coefficient2 = at_c<9>(vars); - if (row1_ == obj_name) + if (row1_ == obj_name) { f = -coefficient1; - else + } else { b[row1_] = coefficient1; + } - if (row2_ == obj_name) + if (row2_ == obj_name) { f = -coefficient2; - else + } else { b[row2_] = coefficient2; + } if (debug) { cout << "Added RHS for Var: " << var_ << " Row: " << row1_ From 99e3a111e1d7d0d16cc05e8b12fed6390bbbdf28 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Nov 2020 14:58:50 -0500 Subject: [PATCH 39/43] correct conditional syntax --- .github/workflows/build-linux.yml | 2 +- .github/workflows/build-macos.yml | 2 +- .github/workflows/build-windows.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 940e34b82..4039711f2 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -85,7 +85,7 @@ jobs: bash .github/scripts/unix.sh -t - name: Upload build directory uses: actions/upload-artifact@v2 - if: ${{ matrix.build_type }} == "Release" + if: ${{ matrix.build_type == "Release" }} with: name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index cccfd7b5b..1d36e048f 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -53,7 +53,7 @@ jobs: bash .github/scripts/unix.sh -t - name: Upload build directory uses: actions/upload-artifact@v2 - if: ${{ matrix.build_type }} == "Release" + if: ${{ matrix.build_type == "Release" }} with: name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index a14d7de5e..8e469d49f 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -78,7 +78,7 @@ jobs: cmake --build build --config ${{ matrix.build_type }} --target check.linear - name: Upload build directory uses: actions/upload-artifact@v2 - if: ${{ matrix.build_type }} == "Release" + if: ${{ matrix.build_type == "Release" }} with: name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ From c206ed9c2fcfff0802951ae7cabea5c8ba7ecd5e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Nov 2020 15:35:09 -0500 Subject: [PATCH 40/43] syntax update --- .github/workflows/build-linux.yml | 2 +- .github/workflows/build-macos.yml | 2 +- .github/workflows/build-windows.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 4039711f2..32c3bd8aa 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -85,7 +85,7 @@ jobs: bash .github/scripts/unix.sh -t - name: Upload build directory uses: actions/upload-artifact@v2 - if: ${{ matrix.build_type == "Release" }} + if: matrix.build_type == 'Release' with: name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 1d36e048f..cf1a474e3 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -53,7 +53,7 @@ jobs: bash .github/scripts/unix.sh -t - name: Upload build directory uses: actions/upload-artifact@v2 - if: ${{ matrix.build_type == "Release" }} + if: matrix.build_type == 'Release' with: name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 8e469d49f..7eb908c94 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -78,7 +78,7 @@ jobs: cmake --build build --config ${{ matrix.build_type }} --target check.linear - name: Upload build directory uses: actions/upload-artifact@v2 - if: ${{ matrix.build_type == "Release" }} + if: matrix.build_type == 'Release' with: name: gtsam-${{ matrix.name }}-${{ matrix.build_type }} path: ${{ github.workspace }}/build/ From e4c738dabf9b603cdbd26e2214611bd211bbcde9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 30 Nov 2020 06:40:54 -0500 Subject: [PATCH 41/43] Deprecate SimpleCamera properly --- gtsam/geometry/SimpleCamera.cpp | 2 ++ gtsam/geometry/SimpleCamera.h | 17 ++++++++-- gtsam/gtsam.i | 59 +++++---------------------------- 3 files changed, 25 insertions(+), 53 deletions(-) diff --git a/gtsam/geometry/SimpleCamera.cpp b/gtsam/geometry/SimpleCamera.cpp index 6134ae3d4..d1a5ed330 100644 --- a/gtsam/geometry/SimpleCamera.cpp +++ b/gtsam/geometry/SimpleCamera.cpp @@ -21,6 +21,7 @@ namespace gtsam { +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 SimpleCamera simpleCamera(const Matrix34& P) { // P = [A|a] = s K cRw [I|-T], with s the unknown scale @@ -45,5 +46,6 @@ namespace gtsam { return SimpleCamera(Pose3(wRc, T), Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2))); } +#endif } diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index 82f26aee2..04746ba6f 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -19,14 +19,23 @@ #pragma once #include -#include +#include +#include +#include #include +#include namespace gtsam { - /// A simple camera class with a Cal3_S2 calibration -typedef gtsam::PinholeCamera PinholeCameraCal3_S2; + /// Convenient aliases for Pinhole camera classes with different calibrations. + /// Also needed as forward declarations in the wrapper. + using PinholeCameraCal3_S2 = gtsam::PinholeCamera; + using PinholeCameraCal3Bundler = gtsam::PinholeCamera; + //TODO Need to fix issue 621 for this to work with wrapper + // using PinholeCameraCal3DS2 = gtsam::PinholeCamera; + // using PinholeCameraCal3Unified = gtsam::PinholeCamera; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** * @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x * Use PinholeCameraCal3_S2 instead @@ -140,4 +149,6 @@ struct traits : public internal::Manifold {}; template struct Range : HasRange {}; +#endif + } // namespace gtsam diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 1b4d976da..eb36e73a3 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -329,7 +329,7 @@ virtual class Value { }; #include -template +template virtual class GenericValue : gtsam::Value { void serializable() const; }; @@ -1059,52 +1059,12 @@ class PinholeCamera { void serialize() const; }; +// Forward declaration of PinholeCameraCalX is defined here. #include -virtual class SimpleCamera { - // Standard Constructors and Named Constructors - SimpleCamera(); - SimpleCamera(const gtsam::Pose3& pose); - SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K); - static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, const gtsam::Pose2& pose, double height); - static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height); - static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, - const gtsam::Point3& upVector, const gtsam::Cal3_S2& K); - static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, - const gtsam::Point3& upVector); - - // Testable - void print(string s) const; - bool equals(const gtsam::SimpleCamera& camera, double tol) const; - - // Standard Interface - gtsam::Pose3 pose() const; - gtsam::Cal3_S2 calibration() const; - - // Manifold - gtsam::SimpleCamera retract(Vector d) const; - Vector localCoordinates(const gtsam::SimpleCamera& T2) const; - size_t dim() const; - static size_t Dim(); - - // Transformations and measurement functions - static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); - pair projectSafe(const gtsam::Point3& pw) const; - gtsam::Point2 project(const gtsam::Point3& point); - gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& pose); - - // enabling serialization functionality - void serialize() const; - -}; - -gtsam::SimpleCamera simpleCamera(const Matrix& P); - // Some typedefs for common camera types // PinholeCameraCal3_S2 is the same as SimpleCamera above typedef gtsam::PinholeCamera PinholeCameraCal3_S2; -//TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified +//TODO (Issue 621) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified //typedef gtsam::PinholeCamera PinholeCameraCal3DS2; //typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; @@ -1150,7 +1110,7 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize); - + //************************************************************************* // Symbolic //************************************************************************* @@ -2069,7 +2029,7 @@ class NonlinearFactorGraph { gtsam::KeySet keys() const; gtsam::KeyVector keyVector() const; - template, gtsam::imuBias::ConstantBias}> + template, gtsam::imuBias::ConstantBias}> void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); // NonlinearFactorGraph @@ -2493,7 +2453,7 @@ class ISAM2 { template , + gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera, Vector, Matrix}> VALUE calculateEstimate(size_t key) const; gtsam::Values calculateBestEstimate() const; @@ -2527,12 +2487,11 @@ class NonlinearISAM { //************************************************************************* // Nonlinear factor types //************************************************************************* -#include #include #include #include -template}> +template}> virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2556,7 +2515,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { template virtual class NonlinearEquality : gtsam::NoiseModelFactor { // Constructor - forces exact evaluation @@ -2675,7 +2634,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { gtsam::Point2 measured() const; }; typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; -//TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2 +//TODO (Issue 621) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2 //typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Bundler; From 2703307a430cf7dd6ea58dd63e95d8b6795ba30c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 30 Nov 2020 06:44:45 -0500 Subject: [PATCH 42/43] deprecate SimpleCamera tests --- gtsam/geometry/tests/testSimpleCamera.cpp | 4 ++++ gtsam_unstable/slam/serialization.cpp | 27 ++++++++++++++------- tests/testSerializationSLAM.cpp | 29 +++++++++++------------ 3 files changed, 36 insertions(+), 24 deletions(-) diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index edf122d3c..18a25c553 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -26,6 +26,8 @@ using namespace std; using namespace gtsam; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 + static const Cal3_S2 K(625, 625, 0, 0, 0); static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), @@ -149,6 +151,8 @@ TEST( SimpleCamera, simpleCamera) CHECK(assert_equal(expected, actual,1e-1)); } +#endif + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index 8a661f2ef..803e4353a 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -43,7 +43,6 @@ typedef PriorFactor PriorFactorPose3; typedef PriorFactor PriorFactorCal3_S2; typedef PriorFactor PriorFactorCal3DS2; typedef PriorFactor PriorFactorCalibratedCamera; -typedef PriorFactor PriorFactorSimpleCamera; typedef PriorFactor PriorFactorPinholeCameraCal3_S2; typedef PriorFactor PriorFactorStereoCamera; @@ -68,7 +67,6 @@ typedef NonlinearEquality NonlinearEqualityPose3; typedef NonlinearEquality NonlinearEqualityCal3_S2; typedef NonlinearEquality NonlinearEqualityCal3DS2; typedef NonlinearEquality NonlinearEqualityCalibratedCamera; -typedef NonlinearEquality NonlinearEqualitySimpleCamera; typedef NonlinearEquality NonlinearEqualityPinholeCameraCal3_S2; typedef NonlinearEquality NonlinearEqualityStereoCamera; @@ -77,10 +75,8 @@ typedef RangeFactor RangeFactor3D; typedef RangeFactor RangeFactorPose2; typedef RangeFactor RangeFactorPose3; typedef RangeFactor RangeFactorCalibratedCameraPoint; -typedef RangeFactor RangeFactorSimpleCameraPoint; typedef RangeFactor RangeFactorPinholeCameraCal3_S2Point; typedef RangeFactor RangeFactorCalibratedCamera; -typedef RangeFactor RangeFactorSimpleCamera; typedef RangeFactor RangeFactorPinholeCameraCal3_S2; typedef BearingRangeFactor BearingRangeFactor2D; @@ -90,6 +86,7 @@ typedef GenericProjectionFactor GenericProjectionFactorC typedef GenericProjectionFactor GenericProjectionFactorCal3DS2; typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +//TODO fix issue 621 //typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; typedef gtsam::GeneralSFMFactor2 GeneralSFMFactor2Cal3_S2; @@ -129,7 +126,6 @@ GTSAM_VALUE_EXPORT(gtsam::Cal3_S2); GTSAM_VALUE_EXPORT(gtsam::Cal3DS2); GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo); GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera); -GTSAM_VALUE_EXPORT(gtsam::SimpleCamera); GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2); GTSAM_VALUE_EXPORT(gtsam::StereoCamera); @@ -150,7 +146,6 @@ BOOST_CLASS_EXPORT_GUID(PriorFactorPose3, "gtsam::PriorFactorPose3"); BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2"); BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2"); BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera"); -BOOST_CLASS_EXPORT_GUID(PriorFactorSimpleCamera, "gtsam::PriorFactorSimpleCamera"); BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera"); BOOST_CLASS_EXPORT_GUID(BetweenFactorLieVector, "gtsam::BetweenFactorLieVector"); @@ -174,7 +169,6 @@ BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose3, "gtsam::NonlinearEqualityPose3") BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera"); BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D"); @@ -182,9 +176,7 @@ BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D"); BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2"); BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3"); BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint"); -BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleCameraPoint"); BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera"); -BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera"); BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D"); @@ -192,12 +184,29 @@ BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectio BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2"); BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2"); +//TODO Fix issue 621 //BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2"); BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2"); BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D"); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 + +typedef PriorFactor PriorFactorSimpleCamera; +typedef NonlinearEquality NonlinearEqualitySimpleCamera; +typedef RangeFactor RangeFactorSimpleCameraPoint; +typedef RangeFactor RangeFactorSimpleCamera; + +GTSAM_VALUE_EXPORT(gtsam::SimpleCamera); +BOOST_CLASS_EXPORT_GUID(PriorFactorSimpleCamera, "gtsam::PriorFactorSimpleCamera"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera"); +BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleCameraPoint"); +BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera"); + +#endif + + /* ************************************************************************* */ // Actual implementations of functions /* ************************************************************************* */ diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 84e521156..53086e921 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -89,10 +89,8 @@ typedef RangeFactor RangeFactor3D; typedef RangeFactor RangeFactorPose2; typedef RangeFactor RangeFactorPose3; typedef RangeFactor RangeFactorCalibratedCameraPoint; -typedef RangeFactor RangeFactorSimpleCameraPoint; typedef RangeFactor RangeFactorPinholeCameraCal3_S2Point; typedef RangeFactor RangeFactorCalibratedCamera; -typedef RangeFactor RangeFactorSimpleCamera; typedef RangeFactor RangeFactorPinholeCameraCal3_S2; typedef BearingRangeFactor BearingRangeFactor2D; @@ -102,6 +100,7 @@ typedef GenericProjectionFactor GenericProjectionFactorC typedef GenericProjectionFactor GenericProjectionFactorCal3DS2; typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +//TODO Fix issue 621 for this to work //typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; typedef gtsam::GeneralSFMFactor2 GeneralSFMFactor2Cal3_S2; @@ -145,7 +144,6 @@ GTSAM_VALUE_EXPORT(gtsam::Cal3_S2); GTSAM_VALUE_EXPORT(gtsam::Cal3DS2); GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo); GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera); -GTSAM_VALUE_EXPORT(gtsam::SimpleCamera); GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2); GTSAM_VALUE_EXPORT(gtsam::StereoCamera); @@ -190,9 +188,9 @@ BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D"); BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2"); BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3"); BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint"); -BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleCameraPoint"); +BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2Point, "gtsam::RangeFactorPinholeCameraCal3_S2Point"); BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera"); -BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera"); +BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2, "gtsam::RangeFactorPinholeCameraCal3_S2"); BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D"); @@ -200,6 +198,7 @@ BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectio BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2"); BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2"); +//TODO fix issue 621 //BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2"); BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2"); @@ -352,9 +351,9 @@ TEST (testSerializationSLAM, factors) { RangeFactorPose2 rangeFactorPose2(a08, b08, 2.0, model1); RangeFactorPose3 rangeFactorPose3(a09, b09, 2.0, model1); RangeFactorCalibratedCameraPoint rangeFactorCalibratedCameraPoint(a12, a05, 2.0, model1); - RangeFactorSimpleCameraPoint rangeFactorSimpleCameraPoint(a13, a05, 2.0, model1); + RangeFactorPinholeCameraCal3_S2Point rangeFactorPinholeCameraCal3_S2Point(a13, a05, 2.0, model1); RangeFactorCalibratedCamera rangeFactorCalibratedCamera(a12, b12, 2.0, model1); - RangeFactorSimpleCamera rangeFactorSimpleCamera(a13, b13, 2.0, model1); + RangeFactorPinholeCameraCal3_S2 rangeFactorPinholeCameraCal3_S2(a13, b13, 2.0, model1); BearingRangeFactor2D bearingRangeFactor2D(a08, a03, rot2, 2.0, model2); @@ -405,9 +404,9 @@ TEST (testSerializationSLAM, factors) { graph += rangeFactorPose2; graph += rangeFactorPose3; graph += rangeFactorCalibratedCameraPoint; - graph += rangeFactorSimpleCameraPoint; + graph += rangeFactorPinholeCameraCal3_S2Point; graph += rangeFactorCalibratedCamera; - graph += rangeFactorSimpleCamera; + graph += rangeFactorPinholeCameraCal3_S2; graph += bearingRangeFactor2D; @@ -463,9 +462,9 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsObj(rangeFactorPose2)); EXPECT(equalsObj(rangeFactorPose3)); EXPECT(equalsObj(rangeFactorCalibratedCameraPoint)); - EXPECT(equalsObj(rangeFactorSimpleCameraPoint)); + EXPECT(equalsObj(rangeFactorPinholeCameraCal3_S2Point)); EXPECT(equalsObj(rangeFactorCalibratedCamera)); - EXPECT(equalsObj(rangeFactorSimpleCamera)); + EXPECT(equalsObj(rangeFactorPinholeCameraCal3_S2)); EXPECT(equalsObj(bearingRangeFactor2D)); @@ -521,9 +520,9 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsXML(rangeFactorPose2)); EXPECT(equalsXML(rangeFactorPose3)); EXPECT(equalsXML(rangeFactorCalibratedCameraPoint)); - EXPECT(equalsXML(rangeFactorSimpleCameraPoint)); + EXPECT(equalsXML(rangeFactorPinholeCameraCal3_S2Point)); EXPECT(equalsXML(rangeFactorCalibratedCamera)); - EXPECT(equalsXML(rangeFactorSimpleCamera)); + EXPECT(equalsXML(rangeFactorPinholeCameraCal3_S2)); EXPECT(equalsXML(bearingRangeFactor2D)); @@ -579,9 +578,9 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsBinary(rangeFactorPose2)); EXPECT(equalsBinary(rangeFactorPose3)); EXPECT(equalsBinary(rangeFactorCalibratedCameraPoint)); - EXPECT(equalsBinary(rangeFactorSimpleCameraPoint)); + EXPECT(equalsBinary(rangeFactorPinholeCameraCal3_S2Point)); EXPECT(equalsBinary(rangeFactorCalibratedCamera)); - EXPECT(equalsBinary(rangeFactorSimpleCamera)); + EXPECT(equalsBinary(rangeFactorPinholeCameraCal3_S2)); EXPECT(equalsBinary(bearingRangeFactor2D)); From d9018a9593a7dc1009986ceb3d01ebf0362d5a93 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 30 Nov 2020 10:40:39 -0500 Subject: [PATCH 43/43] update Python test --- python/gtsam/tests/test_SimpleCamera.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/python/gtsam/tests/test_SimpleCamera.py b/python/gtsam/tests/test_SimpleCamera.py index efdfec561..358eb1f48 100644 --- a/python/gtsam/tests/test_SimpleCamera.py +++ b/python/gtsam/tests/test_SimpleCamera.py @@ -14,11 +14,12 @@ import unittest import numpy as np import gtsam -from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, SimpleCamera +from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, PinholeCameraCal3_S2 as SimpleCamera from gtsam.utils.test_case import GtsamTestCase K = Cal3_S2(625, 625, 0, 0, 0) + class TestSimpleCamera(GtsamTestCase): def test_constructor(self): @@ -29,15 +30,15 @@ class TestSimpleCamera(GtsamTestCase): def test_level2(self): # Create a level camera, looking in Y-direction - pose2 = Pose2(0.4,0.3,math.pi/2.0) + pose2 = Pose2(0.4, 0.3, math.pi/2.0) camera = SimpleCamera.Level(K, pose2, 0.1) # expected - x = Point3(1,0,0) - y = Point3(0,0,-1) - z = Point3(0,1,0) - wRc = Rot3(x,y,z) - expected = Pose3(wRc,Point3(0.4,0.3,0.1)) + x = Point3(1, 0, 0) + y = Point3(0, 0, -1) + z = Point3(0, 1, 0) + wRc = Rot3(x, y, z) + expected = Pose3(wRc, Point3(0.4, 0.3, 0.1)) self.gtsamAssertEquals(camera.pose(), expected, 1e-9)