diff --git a/.github/scripts/boost.sh b/.github/scripts/boost.sh index 647a84628..3c7e01274 100644 --- a/.github/scripts/boost.sh +++ b/.github/scripts/boost.sh @@ -9,10 +9,10 @@ tar -zxf ${BOOST_FOLDER}.tar.gz # Bootstrap cd ${BOOST_FOLDER}/ -./bootstrap.sh +./bootstrap.sh --with-libraries=serialization,filesystem,thread,system,atomic,date_time,timer,chrono,program_options,regex # Build and install -sudo ./b2 install +sudo ./b2 -j$(nproc) install # Rebuild ld cache sudo ldconfig diff --git a/.github/scripts/unix.sh b/.github/scripts/unix.sh index 55a8ac372..6abbb5596 100644 --- a/.github/scripts/unix.sh +++ b/.github/scripts/unix.sh @@ -92,7 +92,11 @@ function build () configure - make -j2 + if [ "$(uname)" == "Linux" ]; then + make -j$(nproc) + elif [ "$(uname)" == "Darwin" ]; then + make -j$(sysctl -n hw.physicalcpu) + fi finish } @@ -105,8 +109,12 @@ function test () configure - # Actual build: - make -j2 check + # Actual testing + if [ "$(uname)" == "Linux" ]; then + make -j$(nproc) + elif [ "$(uname)" == "Darwin" ]; then + make -j$(sysctl -n hw.physicalcpu) + fi finish } diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 5483c32cf..f52e5eec3 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -47,8 +47,7 @@ jobs: - name: Checkout uses: actions/checkout@v2 - - name: Install (Linux) - if: runner.os == 'Linux' + - name: Install Dependencies run: | # LLVM (clang) 9 is not in Bionic's repositories so we add the official LLVM repository. if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then @@ -63,7 +62,7 @@ jobs: fi sudo apt-get -y update - sudo apt-get install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev + sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev if [ "${{ matrix.compiler }}" = "gcc" ]; then sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib @@ -79,7 +78,5 @@ jobs: run: | bash .github/scripts/boost.sh - - name: Build and Test (Linux) - if: runner.os == 'Linux' - run: | - bash .github/scripts/unix.sh -t + - name: Build and Test + run: bash .github/scripts/unix.sh -t diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index ed8f8563b..462723222 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -34,8 +34,7 @@ jobs: - name: Checkout uses: actions/checkout@v2 - - name: Install (macOS) - if: runner.os == 'macOS' + - name: Install Dependencies run: | brew install cmake ninja brew install boost @@ -48,7 +47,5 @@ jobs: echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV fi - - name: Build and Test (macOS) - if: runner.os == 'macOS' - run: | - bash .github/scripts/unix.sh -t + - name: Build and Test + run: bash .github/scripts/unix.sh -t diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index addde8c64..3fc2d662f 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -81,7 +81,7 @@ jobs: fi sudo apt-get -y update - sudo apt install cmake build-essential pkg-config libpython-dev python-numpy libboost-all-dev + sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libboost-all-dev 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 6427e13bc..3bb3fa298 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -70,7 +70,7 @@ jobs: fi sudo apt-get -y update - sudo apt install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev + sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev if [ "${{ matrix.compiler }}" = "gcc" ]; then sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index a564dade9..5dfdcd013 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -12,42 +12,46 @@ jobs: CTEST_PARALLEL_LEVEL: 2 CMAKE_BUILD_TYPE: ${{ matrix.build_type }} GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }} + BOOST_VERSION: 1.72.0 + BOOST_EXE: boost_1_72_0-msvc-14.2 + strategy: fail-fast: false matrix: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ - #TODO This build keeps timing out, need to understand why. - # windows-2016-cl, - windows-2019-cl, - ] + #TODO This build fails, need to understand why. + # windows-2016-cl, + windows-2019-cl, + ] build_type: [Debug, Release] build_unstable: [ON] include: - - #TODO This build keeps timing out, need to understand why. + #TODO This build fails, need to understand why. # - name: windows-2016-cl # os: windows-2016 # compiler: cl + # platform: 32 - name: windows-2019-cl os: windows-2019 compiler: cl + platform: 64 steps: - - name: Checkout - uses: actions/checkout@v2 - - name: Install (Windows) - if: runner.os == 'Windows' + - name: Install Dependencies shell: powershell run: | Invoke-Expression (New-Object System.Net.WebClient).DownloadString('https://get.scoop.sh') + scoop install cmake --global # So we don't get issues with CMP0074 policy scoop install ninja --global + if ("${{ matrix.compiler }}".StartsWith("clang")) { scoop install llvm --global } + if ("${{ matrix.compiler }}" -eq "gcc") { # Chocolatey GCC is broken on the windows-2019 image. # See: https://github.com/DaanDeMeyer/doctest/runs/231595515 @@ -55,27 +59,38 @@ jobs: scoop install gcc --global echo "CC=gcc" >> $GITHUB_ENV echo "CXX=g++" >> $GITHUB_ENV + } elseif ("${{ matrix.compiler }}" -eq "clang") { echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV + } else { - echo "CC=${{ matrix.compiler }}" >> $GITHUB_ENV - echo "CXX=${{ matrix.compiler }}" >> $GITHUB_ENV + echo "CC=${{ matrix.compiler }}" >> $env:GITHUB_ENV + echo "CXX=${{ matrix.compiler }}" >> $env:GITHUB_ENV + } + # Scoop modifies the PATH so we make the modified PATH global. - echo "$env:PATH" >> $GITHUB_PATH + echo "$env:PATH" >> $env:GITHUB_PATH - - name: Download and install Boost - uses: MarkusJx/install-boost@v1.0.1 - id: install-boost - with: - boost_version: 1.72.0 - toolset: msvc14.2 + - name: Install Boost + shell: powershell + run: | + # Snippet from: https://github.com/actions/virtual-environments/issues/2667 + $BOOST_PATH = "C:\hostedtoolcache\windows\Boost\$env:BOOST_VERSION\x86_64" - - name: Build (Windows) - if: runner.os == 'Windows' - env: - BOOST_ROOT: ${{ steps.install-boost.outputs.BOOST_ROOT }} + # Use the prebuilt binary for Windows + $Url = "https://sourceforge.net/projects/boost/files/boost-binaries/$env:BOOST_VERSION/$env:BOOST_EXE-${{matrix.platform}}.exe" + (New-Object System.Net.WebClient).DownloadFile($Url, "$env:TEMP\boost.exe") + Start-Process -Wait -FilePath "$env:TEMP\boost.exe" "/SILENT","/SP-","/SUPPRESSMSGBOXES","/DIR=$BOOST_PATH" + + # Set the BOOST_ROOT variable + echo "BOOST_ROOT=$BOOST_PATH" >> $env:GITHUB_ENV + + - name: Checkout + uses: actions/checkout@v2 + + - name: Build run: | cmake -E remove_directory build cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib" diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 8ac67a5c3..c1f0b6b3f 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -30,7 +30,7 @@ namespace gtsam { * \nosubgrouping */ template -class PinholeCamera: public PinholeBaseK { +class GTSAM_EXPORT PinholeCamera: public PinholeBaseK { public: diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 949caaa27..cc6435a58 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -31,7 +31,7 @@ namespace gtsam { * \nosubgrouping */ template -class PinholeBaseK: public PinholeBase { +class GTSAM_EXPORT PinholeBaseK: public PinholeBase { private: diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 4e56347d3..0e303cbcd 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -982,6 +982,24 @@ gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize); +gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, + gtsam::Cal3_S2* sharedCal, + const gtsam::Point2Vector& measurements, + const gtsam::Point3& initialEstimate); +gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, + gtsam::Cal3DS2* sharedCal, + const gtsam::Point2Vector& measurements, + const gtsam::Point3& initialEstimate); +gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, + gtsam::Cal3Bundler* sharedCal, + const gtsam::Point2Vector& measurements, + const gtsam::Point3& initialEstimate); +gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3_S2& cameras, + const gtsam::Point2Vector& measurements, + const gtsam::Point3& initialEstimate); +gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Bundler& cameras, + const gtsam::Point2Vector& measurements, + const gtsam::Point3& initialEstimate); #include template diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index f2ba3e31c..47438ecd6 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -182,6 +182,16 @@ protected: return item->second; } +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(index_); + ar & BOOST_SERIALIZATION_NVP(nFactors_); + ar & BOOST_SERIALIZATION_NVP(nEntries_); + } + /// @} }; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 9f33e757f..92c2142a7 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -315,6 +315,26 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { void removeVariables(const KeySet& unusedKeys); void updateDelta(bool forceFullSolve = false) const; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & boost::serialization::base_object >(*this); + ar & BOOST_SERIALIZATION_NVP(theta_); + ar & BOOST_SERIALIZATION_NVP(variableIndex_); + ar & BOOST_SERIALIZATION_NVP(delta_); + ar & BOOST_SERIALIZATION_NVP(deltaNewton_); + ar & BOOST_SERIALIZATION_NVP(RgProd_); + ar & BOOST_SERIALIZATION_NVP(deltaReplacedMask_); + ar & BOOST_SERIALIZATION_NVP(nonlinearFactors_); + ar & BOOST_SERIALIZATION_NVP(linearFactors_); + ar & BOOST_SERIALIZATION_NVP(doglegDelta_); + ar & BOOST_SERIALIZATION_NVP(fixedVariables_); + ar & BOOST_SERIALIZATION_NVP(update_count_); + } + }; // ISAM2 /// traits diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 7bb802186..80262ae3f 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -58,22 +58,23 @@ TEST(Expression, Constant) { /* ************************************************************************* */ // Leaf TEST(Expression, Leaf) { - Rot3_ R(100); + const Key key = 100; + Rot3_ R(key); Values values; - values.insert(100, someR); + values.insert(key, someR); Rot3 actual2 = R.value(values); EXPECT(assert_equal(someR, actual2)); } /* ************************************************************************* */ -// Many Leaves +// Test the function `createUnknowns` to create many leaves at once. TEST(Expression, Leaves) { Values values; - Point3 somePoint(1, 2, 3); + const Point3 somePoint(1, 2, 3); values.insert(Symbol('p', 10), somePoint); - std::vector points = createUnknowns(10, 'p', 1); - EXPECT(assert_equal(somePoint, points.back().value(values))); + std::vector pointExpressions = createUnknowns(10, 'p', 1); + EXPECT(assert_equal(somePoint, pointExpressions.back().value(values))); } /* ************************************************************************* */ @@ -88,29 +89,34 @@ double f2(const Point3& p, OptionalJacobian<1, 3> H) { Vector f3(const Point3& p, OptionalJacobian H) { return p; } -Point3_ p(1); +Point3_ pointExpression(1); set expected = list_of(1); } // namespace unary +// Create a unary expression that takes another expression as a single argument. TEST(Expression, Unary1) { using namespace unary; - Expression e(f1, p); - EXPECT(expected == e.keys()); -} -TEST(Expression, Unary2) { - using namespace unary; - Double_ e(f2, p); - EXPECT(expected == e.keys()); + Expression unaryExpression(f1, pointExpression); + EXPECT(expected == unaryExpression.keys()); +} + +// Check that also works with a scalar return value. +TEST(Expression, Unary2) { + using namespace unary; + Double_ unaryExpression(f2, pointExpression); + EXPECT(expected == unaryExpression.keys()); } -/* ************************************************************************* */ // Unary(Leaf), dynamic TEST(Expression, Unary3) { using namespace unary; - // Expression e(f3, p); + // TODO(yetongumich): dynamic output arguments do not work yet! + // Expression unaryExpression(f3, pointExpression); + // EXPECT(expected == unaryExpression.keys()); } /* ************************************************************************* */ +// Simple test class that implements the `VectorSpace` protocol. class Class : public Point3 { public: enum {dimension = 3}; @@ -133,16 +139,20 @@ template<> struct traits : public internal::VectorSpace {}; // Nullary Method TEST(Expression, NullaryMethod) { // Create expression - Expression p(67); - Expression norm_(p, &Class::norm); + const Key key(67); + Expression classExpression(key); + + // Make expression from a class method, note how it differs from the function + // expressions by leading with the class expression in the constructor. + Expression norm_(classExpression, &Class::norm); // Create Values Values values; - values.insert(67, Class(3, 4, 5)); + values.insert(key, Class(3, 4, 5)); // Check dims as map std::map map; - norm_.dims(map); + norm_.dims(map); // TODO(yetongumich): Change to google style pointer convention. LONGS_EQUAL(1, map.size()); // Get value and Jacobians @@ -150,9 +160,10 @@ TEST(Expression, NullaryMethod) { double actual = norm_.value(values, H); // Check all - EXPECT(actual == sqrt(50)); + const double norm = sqrt(3*3 + 4*4 + 5*5); + EXPECT(actual == norm); Matrix expected(1, 3); - expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0); + expected << 3.0 / norm, 4.0 / norm, 5.0 / norm; EXPECT(assert_equal(expected, H[0])); } @@ -170,21 +181,21 @@ Point3_ p_cam(x, &Pose3::transformTo, p); } /* ************************************************************************* */ -// Check that creating an expression to double compiles +// Check that creating an expression to double compiles. TEST(Expression, BinaryToDouble) { using namespace binary; Double_ p_cam(doubleF, x, p); } /* ************************************************************************* */ -// keys +// Check keys of an expression created from class method. TEST(Expression, BinaryKeys) { set expected = list_of(1)(2); EXPECT(expected == binary::p_cam.keys()); } /* ************************************************************************* */ -// dimensions +// Check dimensions by calling `dims` method. TEST(Expression, BinaryDimensions) { map actual, expected = map_list_of(1, 6)(2, 3); binary::p_cam.dims(actual); @@ -192,7 +203,7 @@ TEST(Expression, BinaryDimensions) { } /* ************************************************************************* */ -// dimensions +// Check dimensions of execution trace. TEST(Expression, BinaryTraceSize) { typedef internal::BinaryExpression Binary; size_t expectedTraceSize = sizeof(Binary::Record); @@ -247,6 +258,7 @@ TEST(Expression, TreeTraceSize) { } /* ************************************************************************* */ +// Test compose operation with * operator. TEST(Expression, compose1) { // Create expression Rot3_ R1(1), R2(2); @@ -258,7 +270,7 @@ TEST(Expression, compose1) { } /* ************************************************************************* */ -// Test compose with arguments referring to the same rotation +// Test compose with arguments referring to the same rotation. TEST(Expression, compose2) { // Create expression Rot3_ R1(1), R2(1); @@ -270,7 +282,7 @@ TEST(Expression, compose2) { } /* ************************************************************************* */ -// Test compose with one arguments referring to constant rotation +// Test compose with one arguments referring to constant rotation. TEST(Expression, compose3) { // Create expression Rot3_ R1(Rot3::identity()), R2(3); @@ -282,7 +294,7 @@ TEST(Expression, compose3) { } /* ************************************************************************* */ -// Test with ternary function +// Test with ternary function. Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) @@ -306,6 +318,7 @@ TEST(Expression, ternary) { } /* ************************************************************************* */ +// Test scalar multiplication with * operator. TEST(Expression, ScalarMultiply) { const Key key(67); const Point3_ expr = 23 * Point3_(key); @@ -336,6 +349,7 @@ TEST(Expression, ScalarMultiply) { } /* ************************************************************************* */ +// Test sum with + operator. TEST(Expression, BinarySum) { const Key key(67); const Point3_ sum_ = Point3_(key) + Point3_(Point3(1, 1, 1)); @@ -366,6 +380,7 @@ TEST(Expression, BinarySum) { } /* ************************************************************************* */ +// Test sum of 3 variables with + operator. TEST(Expression, TripleSum) { const Key key(67); const Point3_ p1_(Point3(1, 1, 1)), p2_(key); @@ -387,6 +402,7 @@ TEST(Expression, TripleSum) { } /* ************************************************************************* */ +// Test sum with += operator. TEST(Expression, PlusEqual) { const Key key(67); const Point3_ p1_(Point3(1, 1, 1)), p2_(key); @@ -461,11 +477,12 @@ TEST(Expression, WeightedSum) { EXPECT(actual_dims == expected_dims); Values values; - values.insert(key1, Point3(1, 0, 0)); - values.insert(key2, Point3(0, 1, 0)); + const Point3 point1(1, 0, 0), point2(0, 1, 0); + values.insert(key1, point1); + values.insert(key2, point2); // Check value - const Point3 expected = 17 * Point3(1, 0, 0) + 23 * Point3(0, 1, 0); + const Point3 expected = 17 * point1 + 23 * point2; EXPECT(assert_equal(expected, weighted_sum_.value(values))); // Check value + Jacobians diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index 90ebcbbba..f4bb5f4f6 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -17,6 +17,7 @@ */ #include +#include #include #include #include @@ -31,6 +32,30 @@ using namespace std; using namespace gtsam; using namespace gtsam::serializationTestHelpers; + +/* ************************************************************************* */ +// Create GUIDs for Noisemodels +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,"gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +/* ************************************************************************* */ +// Create GUIDs for factors +BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor, "gtsam::PriorFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional"); + + /* ************************************************************************* */ // Export all classes derived from Value GTSAM_VALUE_EXPORT(gtsam::Cal3_S2); @@ -82,6 +107,61 @@ TEST (Serialization, TemplatedValues) { EXPECT(equalsBinary(values)); } +TEST(Serialization, ISAM2) { + + gtsam::ISAM2Params parameters; + gtsam::ISAM2 solver(parameters); + gtsam::NonlinearFactorGraph graph; + gtsam::Values initialValues; + initialValues.clear(); + + gtsam::Vector6 temp6; + for (int i = 0; i < 6; ++i) { + temp6[i] = 0.0001; + } + gtsam::noiseModel::Diagonal::shared_ptr noiseModel = gtsam::noiseModel::Diagonal::Sigmas(temp6); + + gtsam::Pose3 pose0(gtsam::Rot3(), gtsam::Point3(0, 0, 0)); + gtsam::Symbol symbol0('x', 0); + graph.add(gtsam::PriorFactor(symbol0, pose0, noiseModel)); + initialValues.insert(symbol0, pose0); + + solver.update(graph, initialValues, + gtsam::FastVector()); + + std::string binaryPath = "saved_solver.dat"; + try { + std::ofstream outputStream(binaryPath); + boost::archive::binary_oarchive outputArchive(outputStream); + outputArchive << solver; + } catch(...) { + EXPECT(false); + } + + gtsam::ISAM2 solverFromDisk; + try { + std::ifstream ifs(binaryPath); + boost::archive::binary_iarchive inputArchive(ifs); + inputArchive >> solverFromDisk; + } catch(...) { + EXPECT(false); + } + + gtsam::Pose3 p1, p2; + try { + p1 = solver.calculateEstimate(symbol0); + } catch(std::exception &e) { + EXPECT(false); + } + + try { + p2 = solverFromDisk.calculateEstimate(symbol0); + } catch(std::exception &e) { + EXPECT(false); + } + EXPECT(assert_equal(p1, p2)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 76fd1bfc7..58e98ebfa 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -955,7 +955,9 @@ static BinaryMeasurement convertPose2ToBinaryMeasurementRot2( "parseMeasurements can only convert Pose2 measurements " "with Gaussian noise models."); const Matrix3 M = gaussian->covariance(); - auto model = noiseModel::Gaussian::Covariance(M.block<1, 1>(2, 2)); + // the (2,2) entry of Pose2's covariance corresponds to Rot2's covariance + // because the tangent space of Pose2 is ordered as (vx, vy, w) + auto model = noiseModel::Isotropic::Variance(1, M(2, 2)); return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), model); } @@ -1001,7 +1003,9 @@ static BinaryMeasurement convert( "parseMeasurements can only convert Pose3 measurements " "with Gaussian noise models."); const Matrix6 M = gaussian->covariance(); - auto model = noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3)); + // the upper-left 3x3 sub-block of Pose3's covariance corresponds to Rot3's covariance + // because the tangent space of Pose3 is ordered as (w,T) where w and T are both Vector3's + auto model = noiseModel::Gaussian::Covariance(M.block<3, 3>(0, 0)); return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), model); } diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 67100a0ac..ada304f27 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -164,10 +164,15 @@ namespace gtsam { } /** return the calibration object */ - inline const boost::shared_ptr calibration() const { + const boost::shared_ptr calibration() const { return K_; } + /** return the (optional) sensor pose with respect to the vehicle frame */ + const boost::optional& body_P_sensor() const { + return body_P_sensor_; + } + /** return verbosity */ inline bool verboseCheirality() const { return verboseCheirality_; } diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index e9eac22eb..66dbed1eb 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -58,40 +58,42 @@ Point2_ p(2); TEST(ExpressionFactor, Leaf) { using namespace leaf; - // Create old-style factor to create expected value and derivatives + // Create old-style factor to create expected value and derivatives. PriorFactor old(2, Point2(0, 0), model); - // Concise version + // Create the equivalent factor with expression. ExpressionFactor f(model, Point2(0, 0), p); + + // Check values and derivatives. EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f.dim()); boost::shared_ptr gf2 = f.linearize(values); - EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); + EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9)); } /* ************************************************************************* */ -// non-zero noise model +// Test leaf expression with noise model of different variance. TEST(ExpressionFactor, Model) { using namespace leaf; SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01)); - // Create old-style factor to create expected value and derivatives + // Create old-style factor to create expected value and derivatives. PriorFactor old(2, Point2(0, 0), model); - // Concise version + // Create the equivalent factor with expression. ExpressionFactor f(model, Point2(0, 0), p); - // Check values and derivatives + // Check values and derivatives. EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f.dim()); boost::shared_ptr gf2 = f.linearize(values); - EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); + EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9)); EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5); // another way } /* ************************************************************************* */ -// Constrained noise model +// Test leaf expression with constrained noise model. TEST(ExpressionFactor, Constrained) { using namespace leaf; @@ -105,7 +107,7 @@ TEST(ExpressionFactor, Constrained) { EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f.dim()); boost::shared_ptr gf2 = f.linearize(values); - EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); + EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9)); } /* ************************************************************************* */ @@ -129,7 +131,7 @@ TEST(ExpressionFactor, Unary) { boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf, 1e-9)); + EXPECT(assert_equal(expected, *jf, 1e-9)); } /* ************************************************************************* */ @@ -142,11 +144,13 @@ Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) { if (H) *H << I_3x3, I_3x3, I_3x3; return v; } + typedef Eigen::Matrix Matrix9; Vector9 id9(const Vector9& v, OptionalJacobian<9,9> H) { if (H) *H = Matrix9::Identity(); return v; } + TEST(ExpressionFactor, Wide) { // Create some values Values values; @@ -207,6 +211,7 @@ TEST(ExpressionFactor, Binary) { EXPECT(assert_equal(expected25, (Matrix ) (*r)->dTdA1, 1e-9)); EXPECT(assert_equal(expected22, (Matrix ) (*r)->dTdA2, 1e-9)); } + /* ************************************************************************* */ // Unary(Binary(Leaf,Leaf)) TEST(ExpressionFactor, Shallow) { @@ -263,7 +268,7 @@ TEST(ExpressionFactor, Shallow) { EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f2.dim()); boost::shared_ptr gf2 = f2.linearize(values); - EXPECT( assert_equal(*expected, *gf2, 1e-9)); + EXPECT(assert_equal(*expected, *gf2, 1e-9)); } /* ************************************************************************* */ @@ -296,7 +301,7 @@ TEST(ExpressionFactor, tree) { EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f.dim()); boost::shared_ptr gf = f.linearize(values); - EXPECT( assert_equal(*expected, *gf, 1e-9)); + EXPECT(assert_equal(*expected, *gf, 1e-9)); // Concise version ExpressionFactor f2(model, measured, @@ -304,14 +309,14 @@ TEST(ExpressionFactor, tree) { EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f2.dim()); boost::shared_ptr gf2 = f2.linearize(values); - EXPECT( assert_equal(*expected, *gf2, 1e-9)); + EXPECT(assert_equal(*expected, *gf2, 1e-9)); // Try ternary version ExpressionFactor f3(model, measured, project3(x, p, K)); EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f3.dim()); boost::shared_ptr gf3 = f3.linearize(values); - EXPECT( assert_equal(*expected, *gf3, 1e-9)); + EXPECT(assert_equal(*expected, *gf3, 1e-9)); } /* ************************************************************************* */ @@ -332,15 +337,15 @@ TEST(ExpressionFactor, Compose1) { // Check unwhitenedError std::vector H(2); Vector actual = f.unwhitenedError(values, H); - EXPECT( assert_equal(I_3x3, H[0],1e-9)); - EXPECT( assert_equal(I_3x3, H[1],1e-9)); + EXPECT(assert_equal(I_3x3, H[0],1e-9)); + EXPECT(assert_equal(I_3x3, H[1],1e-9)); // Check linearization JacobianFactor expected(1, I_3x3, 2, I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); + EXPECT(assert_equal(expected, *jf,1e-9)); } /* ************************************************************************* */ @@ -362,14 +367,14 @@ TEST(ExpressionFactor, compose2) { std::vector H(1); Vector actual = f.unwhitenedError(values, H); EXPECT_LONGS_EQUAL(1, H.size()); - EXPECT( assert_equal(2*I_3x3, H[0],1e-9)); + EXPECT(assert_equal(2*I_3x3, H[0],1e-9)); // Check linearization JacobianFactor expected(1, 2 * I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); + EXPECT(assert_equal(expected, *jf,1e-9)); } /* ************************************************************************* */ @@ -391,14 +396,14 @@ TEST(ExpressionFactor, compose3) { std::vector H(1); Vector actual = f.unwhitenedError(values, H); EXPECT_LONGS_EQUAL(1, H.size()); - EXPECT( assert_equal(I_3x3, H[0],1e-9)); + EXPECT(assert_equal(I_3x3, H[0],1e-9)); // Check linearization JacobianFactor expected(3, I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); + EXPECT(assert_equal(expected, *jf,1e-9)); } /* ************************************************************************* */ @@ -434,16 +439,16 @@ TEST(ExpressionFactor, composeTernary) { std::vector H(3); Vector actual = f.unwhitenedError(values, H); EXPECT_LONGS_EQUAL(3, H.size()); - EXPECT( assert_equal(I_3x3, H[0],1e-9)); - EXPECT( assert_equal(I_3x3, H[1],1e-9)); - EXPECT( assert_equal(I_3x3, H[2],1e-9)); + EXPECT(assert_equal(I_3x3, H[0],1e-9)); + EXPECT(assert_equal(I_3x3, H[1],1e-9)); + EXPECT(assert_equal(I_3x3, H[2],1e-9)); // Check linearization JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); + EXPECT(assert_equal(expected, *jf,1e-9)); } TEST(ExpressionFactor, tree_finite_differences) { @@ -636,7 +641,7 @@ TEST(ExpressionFactor, MultiplyWithInverseFunction) { class TestNaryFactor : public gtsam::ExpressionFactorN { + gtsam::Rot3, gtsam::Point3> { private: using This = TestNaryFactor; using Base = diff --git a/wrap/gtwrap/interface_parser/classes.py b/wrap/gtwrap/interface_parser/classes.py index 3e6a0fc3c..11317962d 100644 --- a/wrap/gtwrap/interface_parser/classes.py +++ b/wrap/gtwrap/interface_parser/classes.py @@ -10,7 +10,7 @@ Parser classes and rules for parsing C++ classes. Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert """ -from typing import Iterable, List, Union +from typing import Any, Iterable, List, Union from pyparsing import Literal, Optional, ZeroOrMore # type: ignore @@ -48,12 +48,12 @@ class Method: args_list, t.is_const)) def __init__(self, - template: str, + template: Union[Template, Any], name: str, return_type: ReturnType, args: ArgumentList, is_const: str, - parent: Union[str, "Class"] = ''): + parent: Union["Class", Any] = ''): self.template = template self.name = name self.return_type = return_type @@ -98,7 +98,7 @@ class StaticMethod: name: str, return_type: ReturnType, args: ArgumentList, - parent: Union[str, "Class"] = ''): + parent: Union["Class", Any] = ''): self.name = name self.return_type = return_type self.args = args @@ -129,7 +129,7 @@ class Constructor: def __init__(self, name: str, args: ArgumentList, - parent: Union["Class", str] = ''): + parent: Union["Class", Any] = ''): self.name = name self.args = args @@ -167,7 +167,7 @@ class Operator: return_type: ReturnType, args: ArgumentList, is_const: str, - parent: Union[str, "Class"] = ''): + parent: Union["Class", Any] = ''): self.name = name self.operator = operator self.return_type = return_type @@ -284,7 +284,7 @@ class Class: properties: List[Variable], operators: List[Operator], enums: List[Enum], - parent: str = '', + parent: Any = '', ): self.template = template self.is_virtual = is_virtual diff --git a/wrap/gtwrap/interface_parser/function.py b/wrap/gtwrap/interface_parser/function.py index 995aba10e..9fe1f56f0 100644 --- a/wrap/gtwrap/interface_parser/function.py +++ b/wrap/gtwrap/interface_parser/function.py @@ -169,7 +169,7 @@ class GlobalFunction: return_type: ReturnType, args_list: ArgumentList, template: Template, - parent: str = ''): + parent: Any = ''): self.name = name self.return_type = return_type self.args = args_list diff --git a/wrap/gtwrap/interface_parser/type.py b/wrap/gtwrap/interface_parser/type.py index 0b9be6501..49315cc56 100644 --- a/wrap/gtwrap/interface_parser/type.py +++ b/wrap/gtwrap/interface_parser/type.py @@ -12,7 +12,7 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae # pylint: disable=unnecessary-lambda, expression-not-assigned -from typing import Iterable, List, Union +from typing import List, Sequence, Union from pyparsing import (Forward, Optional, Or, ParseResults, # type: ignore delimitedList) @@ -49,12 +49,12 @@ class Typename: def __init__(self, t: ParseResults, - instantiations: Iterable[ParseResults] = ()): + instantiations: Sequence[ParseResults] = ()): self.name = t[-1] # the name is the last element in this list self.namespaces = t[:-1] if instantiations: - if isinstance(instantiations, Iterable): + if isinstance(instantiations, Sequence): self.instantiations = instantiations # type: ignore else: self.instantiations = instantiations.asList() diff --git a/wrap/gtwrap/matlab_wrapper/mixins.py b/wrap/gtwrap/matlab_wrapper/mixins.py index 061cea283..217801ff3 100644 --- a/wrap/gtwrap/matlab_wrapper/mixins.py +++ b/wrap/gtwrap/matlab_wrapper/mixins.py @@ -1,5 +1,7 @@ """Mixins for reducing the amount of boilerplate in the main wrapper class.""" +from typing import Any, Tuple, Union + import gtwrap.interface_parser as parser import gtwrap.template_instantiator as instantiator @@ -7,13 +9,14 @@ import gtwrap.template_instantiator as instantiator class CheckMixin: """Mixin to provide various checks.""" # Data types that are primitive types - not_ptr_type = ['int', 'double', 'bool', 'char', 'unsigned char', 'size_t'] + not_ptr_type: Tuple = ('int', 'double', 'bool', 'char', 'unsigned char', + 'size_t') # Ignore the namespace for these datatypes - ignore_namespace = ['Matrix', 'Vector', 'Point2', 'Point3'] + ignore_namespace: Tuple = ('Matrix', 'Vector', 'Point2', 'Point3') # Methods that should be ignored - ignore_methods = ['pickle'] + ignore_methods: Tuple = ('pickle', ) # Methods that should not be wrapped directly - whitelist = ['serializable', 'serialize'] + whitelist: Tuple = ('serializable', 'serialize') # Datatypes that do not need to be checked in methods not_check_type: list = [] @@ -23,7 +26,7 @@ class CheckMixin: return True return False - def is_shared_ptr(self, arg_type): + def is_shared_ptr(self, arg_type: parser.Type): """ Determine if the `interface_parser.Type` should be treated as a shared pointer in the wrapper. @@ -33,7 +36,7 @@ class CheckMixin: and arg_type.typename.name not in self.ignore_namespace and arg_type.typename.name != 'string') - def is_ptr(self, arg_type): + def is_ptr(self, arg_type: parser.Type): """ Determine if the `interface_parser.Type` should be treated as a raw pointer in the wrapper. @@ -43,7 +46,7 @@ class CheckMixin: and arg_type.typename.name not in self.ignore_namespace and arg_type.typename.name != 'string') - def is_ref(self, arg_type): + def is_ref(self, arg_type: parser.Type): """ Determine if the `interface_parser.Type` should be treated as a reference in the wrapper. @@ -55,7 +58,14 @@ class CheckMixin: class FormatMixin: """Mixin to provide formatting utilities.""" - def _clean_class_name(self, instantiated_class): + + ignore_namespace: tuple + data_type: Any + data_type_param: Any + _return_count: Any + + def _clean_class_name(self, + instantiated_class: instantiator.InstantiatedClass): """Reformatted the C++ class name to fit Matlab defined naming standards """ @@ -65,23 +75,23 @@ class FormatMixin: return instantiated_class.name def _format_type_name(self, - type_name, - separator='::', - include_namespace=True, - constructor=False, - method=False): + type_name: parser.Typename, + separator: str = '::', + include_namespace: bool = True, + is_constructor: bool = False, + is_method: bool = False): """ Args: type_name: an interface_parser.Typename to reformat separator: the statement to add between namespaces and typename include_namespace: whether to include namespaces when reformatting - constructor: if the typename will be in a constructor - method: if the typename will be in a method + is_constructor: if the typename will be in a constructor + is_method: if the typename will be in a method Raises: constructor and method cannot both be true """ - if constructor and method: + if is_constructor and is_method: raise ValueError( 'Constructor and method parameters cannot both be True') @@ -93,9 +103,9 @@ class FormatMixin: if name not in self.ignore_namespace and namespace != '': formatted_type_name += namespace + separator - if constructor: + if is_constructor: formatted_type_name += self.data_type.get(name) or name - elif method: + elif is_method: formatted_type_name += self.data_type_param.get(name) or name else: formatted_type_name += name @@ -106,8 +116,8 @@ class FormatMixin: template = '{}'.format( self._format_type_name(type_name.instantiations[idx], include_namespace=include_namespace, - constructor=constructor, - method=method)) + is_constructor=is_constructor, + is_method=is_method)) templates.append(template) if len(templates) > 0: # If there are no templates @@ -119,15 +129,15 @@ class FormatMixin: self._format_type_name(type_name.instantiations[idx], separator=separator, include_namespace=False, - constructor=constructor, - method=method)) + is_constructor=is_constructor, + is_method=is_method)) return formatted_type_name def _format_return_type(self, - return_type, - include_namespace=False, - separator="::"): + return_type: parser.function.ReturnType, + include_namespace: bool = False, + separator: str = "::"): """Format return_type. Args: @@ -154,18 +164,15 @@ class FormatMixin: return return_wrap - def _format_class_name(self, instantiated_class, separator=''): + def _format_class_name(self, + instantiated_class: instantiator.InstantiatedClass, + separator: str = ''): """Format a template_instantiator.InstantiatedClass name.""" if instantiated_class.parent == '': parent_full_ns = [''] else: parent_full_ns = instantiated_class.parent.full_namespaces() - # class_name = instantiated_class.parent.name - # - # if class_name != '': - # class_name += separator - # - # class_name += instantiated_class.name + parentname = "".join([separator + x for x in parent_full_ns]) + separator @@ -175,10 +182,12 @@ class FormatMixin: return class_name - def _format_static_method(self, static_method, separator=''): - """Example: - - gtsamPoint3.staticFunction + def _format_static_method(self, + static_method: parser.StaticMethod, + separator: str = ''): + """ + Example: + gtsam.Point3.staticFunction() """ method = '' @@ -188,35 +197,17 @@ class FormatMixin: return method[2 * len(separator):] - def _format_instance_method(self, instance_method, separator=''): + def _format_global_function(self, + function: Union[parser.GlobalFunction, Any], + separator: str = ''): """Example: gtsamPoint3.staticFunction """ method = '' - if isinstance(instance_method, instantiator.InstantiatedMethod): - method_list = [ - separator + x - for x in instance_method.parent.parent.full_namespaces() - ] - method += "".join(method_list) + separator - - method += instance_method.parent.name + separator - method += instance_method.original.name - method += "<" + instance_method.instantiations.to_cpp() + ">" - - return method[2 * len(separator):] - - def _format_global_method(self, static_method, separator=''): - """Example: - - gtsamPoint3.staticFunction - """ - method = '' - - if isinstance(static_method, parser.GlobalFunction): - method += "".join([separator + x for x in static_method.parent.full_namespaces()]) + \ + if isinstance(function, parser.GlobalFunction): + method += "".join([separator + x for x in function.parent.full_namespaces()]) + \ separator return method[2 * len(separator):] diff --git a/wrap/gtwrap/matlab_wrapper/wrapper.py b/wrap/gtwrap/matlab_wrapper/wrapper.py index b040d2731..97945f73a 100755 --- a/wrap/gtwrap/matlab_wrapper/wrapper.py +++ b/wrap/gtwrap/matlab_wrapper/wrapper.py @@ -11,8 +11,6 @@ import textwrap from functools import partial, reduce from typing import Dict, Iterable, List, Union -from loguru import logger - import gtwrap.interface_parser as parser import gtwrap.template_instantiator as instantiator from gtwrap.matlab_wrapper.mixins import CheckMixin, FormatMixin @@ -200,7 +198,7 @@ class MatlabWrapper(CheckMixin, FormatMixin): check_type = self._format_type_name( arg.ctype.typename, separator='.', - constructor=not wrap_datatypes) + is_constructor=not wrap_datatypes) var_arg_wrap += " && isa(varargin{{{num}}},'{data_type}')".format( num=i, data_type=check_type) @@ -1090,11 +1088,10 @@ class MatlabWrapper(CheckMixin, FormatMixin): if method.instantiations: # method_name += '<{}>'.format( # self._format_type_name(method.instantiations)) - # method_name = self._format_instance_method(method, '::') method = method.to_cpp() elif isinstance(method, parser.GlobalFunction): - method_name = self._format_global_method(method, '::') + method_name = self._format_global_function(method, '::') method_name += method.name else: diff --git a/wrap/gtwrap/template_instantiator.py b/wrap/gtwrap/template_instantiator.py index 87729cfa6..0cda93d5d 100644 --- a/wrap/gtwrap/template_instantiator.py +++ b/wrap/gtwrap/template_instantiator.py @@ -4,7 +4,7 @@ import itertools from copy import deepcopy -from typing import Iterable, List +from typing import Any, Iterable, List, Sequence import gtwrap.interface_parser as parser @@ -214,17 +214,17 @@ class InstantiatedMethod(parser.Method): } """ def __init__(self, - original, + original: parser.Method, instantiations: Iterable[parser.Typename] = ()): self.original = original self.instantiations = instantiations - self.template = '' + self.template: Any = '' self.is_const = original.is_const self.parent = original.parent # Check for typenames if templated. # This way, we can gracefully handle both templated and non-templated methods. - typenames = self.original.template.typenames if self.original.template else [] + typenames: Sequence = self.original.template.typenames if self.original.template else [] self.name = instantiate_name(original.name, self.instantiations) self.return_type = instantiate_return_type( original.return_type, @@ -348,13 +348,12 @@ class InstantiatedClass(parser.Class): return "{virtual}Class {cpp_class} : {parent_class}\n"\ "{ctors}\n{static_methods}\n{methods}\n{operators}".format( virtual="virtual " if self.is_virtual else '', - name=self.name, cpp_class=self.to_cpp(), parent_class=self.parent, ctors="\n".join([repr(ctor) for ctor in self.ctors]), - methods="\n".join([repr(m) for m in self.methods]), static_methods="\n".join([repr(m) for m in self.static_methods]), + methods="\n".join([repr(m) for m in self.methods]), operators="\n".join([repr(op) for op in self.operators]) ) diff --git a/wrap/requirements.txt b/wrap/requirements.txt index dd24ea4ed..a7181b271 100644 --- a/wrap/requirements.txt +++ b/wrap/requirements.txt @@ -1,3 +1,2 @@ pyparsing pytest -loguru \ No newline at end of file diff --git a/wrap/tests/test_matlab_wrapper.py b/wrap/tests/test_matlab_wrapper.py index fad4de16a..112750721 100644 --- a/wrap/tests/test_matlab_wrapper.py +++ b/wrap/tests/test_matlab_wrapper.py @@ -11,8 +11,6 @@ import os.path as osp import sys import unittest -from loguru import logger - sys.path.append(osp.dirname(osp.dirname(osp.abspath(__file__)))) from gtwrap.matlab_wrapper import MatlabWrapper @@ -44,10 +42,6 @@ class TestWrap(unittest.TestCase): # Create the `actual/matlab` directory os.makedirs(self.MATLAB_ACTUAL_DIR, exist_ok=True) - # set the log level to INFO by default - logger.remove() # remove the default sink - logger.add(sys.stderr, format="{time} {level} {message}", level="INFO") - def compare_and_diff(self, file): """ Compute the comparison between the expected and actual file,