diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 5dfdcd013..a9e794b3f 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -26,7 +26,11 @@ jobs: windows-2019-cl, ] - build_type: [Debug, Release] + build_type: [ + Debug, + #TODO(Varun) The release build takes over 2.5 hours, need to figure out why. + # Release + ] build_unstable: [ON] include: #TODO This build fails, need to understand why. @@ -90,13 +94,18 @@ jobs: - name: Checkout uses: actions/checkout@v2 - - name: Build + - name: Configuration 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" - cmake --build build --config ${{ matrix.build_type }} --target gtsam - cmake --build build --config ${{ matrix.build_type }} --target gtsam_unstable - 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 + + - name: Build + run: | + # Since Visual Studio is a multi-generator, we need to use --config + # https://stackoverflow.com/a/24470998/1236990 + cmake --build build -j 4 --config ${{ matrix.build_type }} --target gtsam + cmake --build build -j 4 --config ${{ matrix.build_type }} --target gtsam_unstable + cmake --build build -j 4 --config ${{ matrix.build_type }} --target wrap + cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base + cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base_unstable + cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.linear diff --git a/CMakeLists.txt b/CMakeLists.txt index b8480867e..5fd5d521c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -87,6 +87,13 @@ if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX) CACHE STRING "The Python version to use for wrapping") # Set the include directory for matlab.h set(GTWRAP_INCLUDE_NAME "wrap") + + # Copy matlab.h to the correct folder. + configure_file(${PROJECT_SOURCE_DIR}/wrap/matlab.h + ${PROJECT_BINARY_DIR}/wrap/matlab.h COPYONLY) + # Add the include directories so that matlab.h can be found + include_directories("${PROJECT_BINARY_DIR}" "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}") + add_subdirectory(wrap) list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake") endif() diff --git a/gtsam/base/base.i b/gtsam/base/base.i index d9c51fbe8..9838f97d3 100644 --- a/gtsam/base/base.i +++ b/gtsam/base/base.i @@ -38,7 +38,7 @@ class DSFMap { DSFMap(); KEY find(const KEY& key) const; void merge(const KEY& x, const KEY& y); - std::map sets(); + std::map sets(); }; class IndexPairSet { diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i index 8f06fd2e1..c9c027438 100644 --- a/gtsam/basis/basis.i +++ b/gtsam/basis/basis.i @@ -140,7 +140,7 @@ class FitBasis { static gtsam::GaussianFactorGraph::shared_ptr LinearGraph( const std::map& sequence, const gtsam::noiseModel::Base* model, size_t N); - Parameters parameters() const; + This::Parameters parameters() const; }; } // namespace gtsam diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 67c3278a3..6ac63c93f 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -165,6 +165,7 @@ gtsam::Values allPose2s(gtsam::Values& values); Matrix extractPose2(const gtsam::Values& values); gtsam::Values allPose3s(gtsam::Values& values); Matrix extractPose3(const gtsam::Values& values); +Matrix extractVectors(const gtsam::Values& values, char c); void perturbPoint2(gtsam::Values& values, double sigma, int seed = 42u); void perturbPose2(gtsam::Values& values, double sigmaT, double sigmaR, int seed = 42u); diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 7fafd95df..38d831e15 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -282,7 +282,7 @@ public: * which are objects in non-linear manifolds (Lie groups). */ template -class NoiseModelFactor1: public NoiseModelFactor { +class GTSAM_EXPORT NoiseModelFactor1: public NoiseModelFactor { public: @@ -366,7 +366,7 @@ private: /** A convenient base class for creating your own NoiseModelFactor with 2 * variables. To derive from this class, implement evaluateError(). */ template -class NoiseModelFactor2: public NoiseModelFactor { +class GTSAM_EXPORT NoiseModelFactor2: public NoiseModelFactor { public: @@ -441,7 +441,7 @@ private: /** A convenient base class for creating your own NoiseModelFactor with 3 * variables. To derive from this class, implement evaluateError(). */ template -class NoiseModelFactor3: public NoiseModelFactor { +class GTSAM_EXPORT NoiseModelFactor3: public NoiseModelFactor { public: @@ -518,7 +518,7 @@ private: /** A convenient base class for creating your own NoiseModelFactor with 4 * variables. To derive from this class, implement evaluateError(). */ template -class NoiseModelFactor4: public NoiseModelFactor { +class GTSAM_EXPORT NoiseModelFactor4: public NoiseModelFactor { public: @@ -599,7 +599,7 @@ private: /** A convenient base class for creating your own NoiseModelFactor with 5 * variables. To derive from this class, implement evaluateError(). */ template -class NoiseModelFactor5: public NoiseModelFactor { +class GTSAM_EXPORT NoiseModelFactor5: public NoiseModelFactor { public: @@ -684,7 +684,7 @@ private: /** A convenient base class for creating your own NoiseModelFactor with 6 * variables. To derive from this class, implement evaluateError(). */ template -class NoiseModelFactor6: public NoiseModelFactor { +class GTSAM_EXPORT NoiseModelFactor6: public NoiseModelFactor { public: diff --git a/gtsam/geometry/tests/testUtilities.cpp b/gtsam/nonlinear/tests/testUtilities.cpp similarity index 68% rename from gtsam/geometry/tests/testUtilities.cpp rename to gtsam/nonlinear/tests/testUtilities.cpp index 25ac3acc8..55a7fdb13 100644 --- a/gtsam/geometry/tests/testUtilities.cpp +++ b/gtsam/nonlinear/tests/testUtilities.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include using namespace gtsam; @@ -55,6 +54,26 @@ TEST(Utilities, ExtractPoint3) { EXPECT_LONGS_EQUAL(2, all_points.rows()); } +/* ************************************************************************* */ +TEST(Utilities, ExtractVector) { + // Test normal case with 3 vectors and 1 non-vector (ignore non-vector) + auto values = Values(); + values.insert(X(0), (Vector(4) << 1, 2, 3, 4).finished()); + values.insert(X(2), (Vector(4) << 13, 14, 15, 16).finished()); + values.insert(X(1), (Vector(4) << 6, 7, 8, 9).finished()); + values.insert(X(3), Pose3()); + auto actual = utilities::extractVectors(values, 'x'); + auto expected = + (Matrix(3, 4) << 1, 2, 3, 4, 6, 7, 8, 9, 13, 14, 15, 16).finished(); + EXPECT(assert_equal(expected, actual)); + + // Check that mis-sized vectors fail + values.insert(X(4), (Vector(2) << 1, 2).finished()); + THROWS_EXCEPTION(utilities::extractVectors(values, 'x')); + values.update(X(4), (Vector(6) << 1, 2, 3, 4, 5, 6).finished()); + THROWS_EXCEPTION(utilities::extractVectors(values, 'x')); +} + /* ************************************************************************* */ int main() { srand(time(nullptr)); diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index fdc1da2c4..d2b38d374 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -162,6 +163,34 @@ Matrix extractPose3(const Values& values) { return result; } +/// Extract all Vector values with a given symbol character into an mxn matrix, +/// where m is the number of symbols that match the character and n is the +/// dimension of the variables. If not all variables have dimension n, then a +/// runtime error will be thrown. The order of returned values are sorted by +/// the symbol. +/// For example, calling extractVector(values, 'x'), where values contains 200 +/// variables x1, x2, ..., x200 of type Vector each 5-dimensional, will return a +/// 200x5 matrix with row i containing xi. +Matrix extractVectors(const Values& values, char c) { + Values::ConstFiltered vectors = + values.filter(Symbol::ChrTest(c)); + if (vectors.size() == 0) { + return Matrix(); + } + auto dim = vectors.begin()->value.size(); + Matrix result(vectors.size(), dim); + Eigen::Index rowi = 0; + for (const auto& kv : vectors) { + if (kv.value.size() != dim) { + throw std::runtime_error( + "Tried to extract different-sized vectors into a single matrix"); + } + result.row(rowi) = kv.value; + ++rowi; + } + return result; +} + /// Perturb all Point2 values using normally distributed noise void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { noiseModel::Isotropic::shared_ptr model = diff --git a/gtsam/sfm/MFAS.h b/gtsam/sfm/MFAS.h index decfbed0f..151b318ad 100644 --- a/gtsam/sfm/MFAS.h +++ b/gtsam/sfm/MFAS.h @@ -48,7 +48,7 @@ namespace gtsam { unit translations in a projection direction. @addtogroup SFM */ -class MFAS { +class GTSAM_EXPORT MFAS { public: // used to represent edges between two nodes in the graph. When used in // translation averaging for global SfM diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 787efac51..5997ad224 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -1,7 +1,20 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2014, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + /* - * @file EssentialMatrixFactor.cpp + * @file EssentialMatrixFactor.h * @brief EssentialMatrixFactor class * @author Frank Dellaert + * @author Ayush Baid + * @author Akshay Krishnan * @date December 17, 2013 */ diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index ddf56b289..209c1196d 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -47,7 +47,7 @@ namespace gtsam { * @tparam CAMERA should behave like a PinholeCamera. */ template -class SmartFactorBase: public NonlinearFactor { +class GTSAM_EXPORT SmartFactorBase: public NonlinearFactor { private: typedef NonlinearFactor Base; diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index c7b1d5424..3cd69c46f 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -41,11 +41,10 @@ namespace gtsam { * If the calibration should be optimized, as well, use SmartProjectionFactor instead! * @addtogroup SLAM */ -template -class SmartProjectionPoseFactor: public SmartProjectionFactor< - PinholePose > { - -private: +template +class GTSAM_EXPORT SmartProjectionPoseFactor + : public SmartProjectionFactor > { + private: typedef PinholePose Camera; typedef SmartProjectionFactor Base; typedef SmartProjectionPoseFactor This; @@ -156,7 +155,6 @@ public: ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(K_); } - }; // end of class declaration diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 527e838b2..da1c197cb 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -168,7 +168,7 @@ template virtual class PoseTranslationPrior : gtsam::NoiseModelFactor { PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); - POSE measured() const; + POSE::Translation measured() const; // enabling serialization functionality void serialize() const; @@ -185,7 +185,7 @@ template virtual class PoseRotationPrior : gtsam::NoiseModelFactor { PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); - POSE measured() const; + POSE::Rotation measured() const; }; typedef gtsam::PoseRotationPrior PoseRotationPrior2D; @@ -196,6 +196,21 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, const gtsam::Point2& pB, const gtsam::noiseModel::Base* noiseModel); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::EssentialMatrixFactor& other, double tol) const; + Vector evaluateError(const gtsam::EssentialMatrix& E) const; +}; + +#include +virtual class EssentialMatrixConstraint : gtsam::NoiseModelFactor { + EssentialMatrixConstraint(size_t key1, size_t key2, const gtsam::EssentialMatrix &measuredE, + const gtsam::noiseModel::Base *model); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::EssentialMatrixConstraint& other, double tol) const; + Vector evaluateError(const gtsam::Pose3& p1, const gtsam::Pose3& p2) const; + const gtsam::EssentialMatrix& measured() const; }; #include diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index 080239b35..2faac24d1 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testEssentialMatrixConstraint.cpp + * @file TestEssentialMatrixConstraint.cpp * @brief Unit tests for EssentialMatrixConstraint Class * @author Frank Dellaert * @author Pablo Alcantarilla diff --git a/gtsam_unstable/slam/LocalOrientedPlane3Factor.h b/gtsam_unstable/slam/LocalOrientedPlane3Factor.h index 5264c8f4b..7f71b282b 100644 --- a/gtsam_unstable/slam/LocalOrientedPlane3Factor.h +++ b/gtsam_unstable/slam/LocalOrientedPlane3Factor.h @@ -9,6 +9,8 @@ #include #include +#include + #include namespace gtsam { @@ -32,9 +34,9 @@ namespace gtsam { * a local linearisation point for the plane. The plane is representated and * optimized in x1 frame in the optimization. */ -class LocalOrientedPlane3Factor: public NoiseModelFactor3 { -protected: +class GTSAM_UNSTABLE_EXPORT LocalOrientedPlane3Factor + : public NoiseModelFactor3 { + protected: OrientedPlane3 measured_p_; typedef NoiseModelFactor3 Base; public: diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h index ec7da22ef..cab48e506 100644 --- a/gtsam_unstable/slam/PoseToPointFactor.h +++ b/gtsam_unstable/slam/PoseToPointFactor.h @@ -1,11 +1,14 @@ /** * @file PoseToPointFactor.hpp - * @brief This factor can be used to track a 3D landmark over time by - *providing local measurements of its location. + * @brief This factor can be used to model relative position measurements + * from a (2D or 3D) pose to a landmark * @author David Wisth + * @author Luca Carlone **/ #pragma once +#include +#include #include #include #include @@ -17,12 +20,13 @@ namespace gtsam { * A class for a measurement between a pose and a point. * @addtogroup SLAM */ -class PoseToPointFactor : public NoiseModelFactor2 { +template +class PoseToPointFactor : public NoiseModelFactor2 { private: typedef PoseToPointFactor This; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactor2 Base; - Point3 measured_; /** the point measurement in local coordinates */ + POINT measured_; /** the point measurement in local coordinates */ public: // shorthand for a smart pointer to a factor @@ -32,7 +36,7 @@ class PoseToPointFactor : public NoiseModelFactor2 { PoseToPointFactor() {} /** Constructor */ - PoseToPointFactor(Key key1, Key key2, const Point3& measured, + PoseToPointFactor(Key key1, Key key2, const POINT& measured, const SharedNoiseModel& model) : Base(model, key1, key2), measured_(measured) {} @@ -41,8 +45,8 @@ class PoseToPointFactor : public NoiseModelFactor2 { /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { + void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override { std::cout << s << "PoseToPointFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n" << " measured: " << measured_.transpose() << std::endl; @@ -50,30 +54,31 @@ class PoseToPointFactor : public NoiseModelFactor2 { } /** equals */ - virtual bool equals(const NonlinearFactor& expected, - double tol = 1e-9) const { + bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const override { const This* e = dynamic_cast(&expected); return e != nullptr && Base::equals(*e, tol) && - traits::Equals(this->measured_, e->measured_, tol); + traits::Equals(this->measured_, e->measured_, tol); } /** implement functions needed to derive from Factor */ /** vector of errors - * @brief Error = wTwi.inverse()*wPwp - measured_ - * @param wTwi The pose of the sensor in world coordinates - * @param wPwp The estimated point location in world coordinates + * @brief Error = w_T_b.inverse()*w_P - measured_ + * @param w_T_b The pose of the body in world coordinates + * @param w_P The estimated point location in world coordinates * * Note: measured_ and the error are in local coordiantes. */ - Vector evaluateError(const Pose3& wTwi, const Point3& wPwp, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { - return wTwi.transformTo(wPwp, H1, H2) - measured_; + Vector evaluateError( + const POSE& w_T_b, const POINT& w_P, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override { + return w_T_b.transformTo(w_P, H1, H2) - measured_; } /** return the measured */ - const Point3& measured() const { return measured_; } + const POINT& measured() const { return measured_; } private: /** Serialization function */ diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index fbc11503c..53860efdc 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -18,9 +18,11 @@ #pragma once -#include -#include #include +#include +#include +#include + #include namespace gtsam { @@ -30,28 +32,27 @@ namespace gtsam { * estimates the body pose, body-camera transform, 3D landmark, and calibration. * @addtogroup SLAM */ - template - class ProjectionFactorPPPC: public NoiseModelFactor4 { - protected: +template +class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC + : public NoiseModelFactor4 { + protected: + Point2 measured_; ///< 2D measurement - Point2 measured_; ///< 2D measurement + // verbosity handling for Cheirality Exceptions + bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) + bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) - // verbosity handling for Cheirality Exceptions - bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) - bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) + public: + /// shorthand for base class type + typedef NoiseModelFactor4 Base; - public: + /// shorthand for this class + typedef ProjectionFactorPPPC This; - /// shorthand for base class type - typedef NoiseModelFactor4 Base; + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; - /// shorthand for this class - typedef ProjectionFactorPPPC This; - - /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; - - /// Default constructor + /// Default constructor ProjectionFactorPPPC() : measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) { } @@ -168,7 +169,7 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(throwCheirality_); ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); } - }; +}; /// traits template diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index c92653c13..2aeaa4824 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -21,6 +21,7 @@ #include #include #include +#include #include @@ -40,7 +41,7 @@ namespace gtsam { * @addtogroup SLAM */ -class ProjectionFactorRollingShutter +class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter : public NoiseModelFactor3 { protected: // Keep a copy of measurement and calibration for I/O diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 7578b2a83..ff84fcd16 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -20,6 +20,7 @@ #include #include +#include namespace gtsam { /** @@ -41,7 +42,7 @@ namespace gtsam { * @addtogroup SLAM */ template -class SmartProjectionPoseFactorRollingShutter +class GTSAM_UNSTABLE_EXPORT SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor { private: typedef SmartProjectionFactor Base; diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 88e112998..5cdfb2ab7 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -20,18 +20,18 @@ #pragma once -#include -#include - -#include #include #include -#include +#include #include +#include +#include +#include #include +#include -#include #include +#include #include namespace gtsam { @@ -49,8 +49,9 @@ typedef SmartProjectionParams SmartStereoProjectionParams; * If you'd like to store poses in values instead of cameras, use * SmartStereoProjectionPoseFactor instead */ -class SmartStereoProjectionFactor: public SmartFactorBase { -private: +class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactor + : public SmartFactorBase { + private: typedef SmartFactorBase Base; diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index ce6df15cb..e20241a0e 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -40,7 +40,8 @@ namespace gtsam { * are Pose3 variables). * @addtogroup SLAM */ -class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { +class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP + : public SmartStereoProjectionFactor { protected: /// shared pointer to calibration object (one for each camera) std::vector> K_all_; @@ -294,7 +295,6 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(K_all_); } - }; // end of class declaration diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 2a8180ac5..a46000a68 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -43,7 +43,8 @@ namespace gtsam { * This factor requires that values contains the involved poses (Pose3). * @addtogroup SLAM */ -class SmartStereoProjectionPoseFactor : public SmartStereoProjectionFactor { +class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor + : public SmartStereoProjectionFactor { protected: /// shared pointer to calibration object (one for each camera) std::vector> K_all_; diff --git a/gtsam_unstable/slam/tests/testPoseToPointFactor.cpp b/gtsam_unstable/slam/tests/testPoseToPointFactor.cpp new file mode 100644 index 000000000..5aaaaec53 --- /dev/null +++ b/gtsam_unstable/slam/tests/testPoseToPointFactor.cpp @@ -0,0 +1,161 @@ +/** + * @file testPoseToPointFactor.cpp + * @brief + * @author David Wisth + * @author Luca Carlone + * @date June 20, 2020 + */ + +#include +#include +#include + +using namespace gtsam; +using namespace gtsam::noiseModel; + +/* ************************************************************************* */ +// Verify zero error when there is no noise +TEST(PoseToPointFactor, errorNoiseless_2D) { + Pose2 pose = Pose2::identity(); + Point2 point(1.0, 2.0); + Point2 noise(0.0, 0.0); + Point2 measured = point + noise; + + Key pose_key(1); + Key point_key(2); + PoseToPointFactor factor(pose_key, point_key, measured, + Isotropic::Sigma(2, 0.05)); + Vector expectedError = Vector2(0.0, 0.0); + Vector actualError = factor.evaluateError(pose, point); + EXPECT(assert_equal(expectedError, actualError, 1E-5)); +} + +/* ************************************************************************* */ +// Verify expected error in test scenario +TEST(PoseToPointFactor, errorNoise_2D) { + Pose2 pose = Pose2::identity(); + Point2 point(1.0, 2.0); + Point2 noise(-1.0, 0.5); + Point2 measured = point + noise; + + Key pose_key(1); + Key point_key(2); + PoseToPointFactor factor(pose_key, point_key, measured, + Isotropic::Sigma(2, 0.05)); + Vector expectedError = -noise; + Vector actualError = factor.evaluateError(pose, point); + EXPECT(assert_equal(expectedError, actualError, 1E-5)); +} + +/* ************************************************************************* */ +// Check Jacobians are correct +TEST(PoseToPointFactor, jacobian_2D) { + // Measurement + gtsam::Point2 l_meas(1, 2); + + // Linearisation point + gtsam::Point2 p_t(-5, 12); + gtsam::Rot2 p_R(1.5 * M_PI); + Pose2 p(p_R, p_t); + + gtsam::Point2 l(3, 0); + + // Factor + Key pose_key(1); + Key point_key(2); + SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); + PoseToPointFactor factor(pose_key, point_key, l_meas, noise); + + // Calculate numerical derivatives + auto f = std::bind(&PoseToPointFactor::evaluateError, factor, + std::placeholders::_1, std::placeholders::_2, boost::none, + boost::none); + Matrix numerical_H1 = numericalDerivative21(f, p, l); + Matrix numerical_H2 = numericalDerivative22(f, p, l); + + // Use the factor to calculate the derivative + Matrix actual_H1; + Matrix actual_H2; + factor.evaluateError(p, l, actual_H1, actual_H2); + + // Verify we get the expected error + EXPECT(assert_equal(numerical_H1, actual_H1, 1e-8)); + EXPECT(assert_equal(numerical_H2, actual_H2, 1e-8)); +} + +/* ************************************************************************* */ +// Verify zero error when there is no noise +TEST(PoseToPointFactor, errorNoiseless_3D) { + Pose3 pose = Pose3::identity(); + Point3 point(1.0, 2.0, 3.0); + Point3 noise(0.0, 0.0, 0.0); + Point3 measured = point + noise; + + Key pose_key(1); + Key point_key(2); + PoseToPointFactor factor(pose_key, point_key, measured, + Isotropic::Sigma(3, 0.05)); + Vector expectedError = Vector3(0.0, 0.0, 0.0); + Vector actualError = factor.evaluateError(pose, point); + EXPECT(assert_equal(expectedError, actualError, 1E-5)); +} + +/* ************************************************************************* */ +// Verify expected error in test scenario +TEST(PoseToPointFactor, errorNoise_3D) { + Pose3 pose = Pose3::identity(); + Point3 point(1.0, 2.0, 3.0); + Point3 noise(-1.0, 0.5, 0.3); + Point3 measured = point + noise; + + Key pose_key(1); + Key point_key(2); + PoseToPointFactor factor(pose_key, point_key, measured, + Isotropic::Sigma(3, 0.05)); + Vector expectedError = -noise; + Vector actualError = factor.evaluateError(pose, point); + EXPECT(assert_equal(expectedError, actualError, 1E-5)); +} + +/* ************************************************************************* */ +// Check Jacobians are correct +TEST(PoseToPointFactor, jacobian_3D) { + // Measurement + gtsam::Point3 l_meas = gtsam::Point3(1, 2, 3); + + // Linearisation point + gtsam::Point3 p_t = gtsam::Point3(-5, 12, 2); + gtsam::Rot3 p_R = gtsam::Rot3::RzRyRx(1.5 * M_PI, -0.3 * M_PI, 0.4 * M_PI); + Pose3 p(p_R, p_t); + + gtsam::Point3 l = gtsam::Point3(3, 0, 5); + + // Factor + Key pose_key(1); + Key point_key(2); + SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); + PoseToPointFactor factor(pose_key, point_key, l_meas, noise); + + // Calculate numerical derivatives + auto f = std::bind(&PoseToPointFactor::evaluateError, factor, + std::placeholders::_1, std::placeholders::_2, boost::none, + boost::none); + Matrix numerical_H1 = numericalDerivative21(f, p, l); + Matrix numerical_H2 = numericalDerivative22(f, p, l); + + // Use the factor to calculate the derivative + Matrix actual_H1; + Matrix actual_H2; + factor.evaluateError(p, l, actual_H1, actual_H2); + + // Verify we get the expected error + EXPECT(assert_equal(numerical_H1, actual_H1, 1e-8)); + EXPECT(assert_equal(numerical_H2, actual_H2, 1e-8)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testPoseToPointFactor.h b/gtsam_unstable/slam/tests/testPoseToPointFactor.h deleted file mode 100644 index e0e5c4581..000000000 --- a/gtsam_unstable/slam/tests/testPoseToPointFactor.h +++ /dev/null @@ -1,86 +0,0 @@ -/** - * @file testPoseToPointFactor.cpp - * @brief - * @author David Wisth - * @date June 20, 2020 - */ - -#include -#include -#include - -using namespace gtsam; -using namespace gtsam::noiseModel; - -/// Verify zero error when there is no noise -TEST(PoseToPointFactor, errorNoiseless) { - Pose3 pose = Pose3::identity(); - Point3 point(1.0, 2.0, 3.0); - Point3 noise(0.0, 0.0, 0.0); - Point3 measured = t + noise; - - Key pose_key(1); - Key point_key(2); - PoseToPointFactor factor(pose_key, point_key, measured, - Isotropic::Sigma(3, 0.05)); - Vector expectedError = Vector3(0.0, 0.0, 0.0); - Vector actualError = factor.evaluateError(pose, point); - EXPECT(assert_equal(expectedError, actualError, 1E-5)); -} - -/// Verify expected error in test scenario -TEST(PoseToPointFactor, errorNoise) { - Pose3 pose = Pose3::identity(); - Point3 point(1.0, 2.0, 3.0); - Point3 noise(-1.0, 0.5, 0.3); - Point3 measured = t + noise; - - Key pose_key(1); - Key point_key(2); - PoseToPointFactor factor(pose_key, point_key, measured, - Isotropic::Sigma(3, 0.05)); - Vector expectedError = noise; - Vector actualError = factor.evaluateError(pose, point); - EXPECT(assert_equal(expectedError, actualError, 1E-5)); -} - -/// Check Jacobians are correct -TEST(PoseToPointFactor, jacobian) { - // Measurement - gtsam::Point3 l_meas = gtsam::Point3(1, 2, 3); - - // Linearisation point - gtsam::Point3 p_t = gtsam::Point3(-5, 12, 2); - gtsam::Rot3 p_R = gtsam::Rot3::RzRyRx(1.5 * M_PI, -0.3 * M_PI, 0.4 * M_PI); - Pose3 p(p_R, p_t); - - gtsam::Point3 l = gtsam::Point3(3, 0, 5); - - // Factor - Key pose_key(1); - Key point_key(2); - SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); - PoseToPointFactor factor(pose_key, point_key, l_meas, noise); - - // Calculate numerical derivatives - auto f = std::bind(&PoseToPointFactor::evaluateError, factor, _1, _2, - boost::none, boost::none); - Matrix numerical_H1 = numericalDerivative21(f, p, l); - Matrix numerical_H2 = numericalDerivative22(f, p, l); - - // Use the factor to calculate the derivative - Matrix actual_H1; - Matrix actual_H2; - factor.evaluateError(p, l, actual_H1, actual_H2); - - // Verify we get the expected error - EXPECT_TRUE(assert_equal(numerical_H1, actual_H1, 1e-8)); - EXPECT_TRUE(assert_equal(numerical_H2, actual_H2, 1e-8)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/matlab/+gtsam/VisualISAMInitialize.m b/matlab/+gtsam/VisualISAMInitialize.m index 29f8b3b46..9b834e3e1 100644 --- a/matlab/+gtsam/VisualISAMInitialize.m +++ b/matlab/+gtsam/VisualISAMInitialize.m @@ -12,11 +12,11 @@ end isam = ISAM2(params); %% Set Noise parameters -noiseModels.pose = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); +noiseModels.pose = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]', true); %noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); -noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.05 0.05 0.05 0.2 0.2 0.2]'); -noiseModels.point = noiseModel.Isotropic.Sigma(3, 0.1); -noiseModels.measurement = noiseModel.Isotropic.Sigma(2, 1.0); +noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.05 0.05 0.05 0.2 0.2 0.2]', true); +noiseModels.point = noiseModel.Isotropic.Sigma(3, 0.1, true); +noiseModels.measurement = noiseModel.Isotropic.Sigma(2, 1.0, true); %% Add constraints/priors % TODO: should not be from ground truth! diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index 28e7cce6e..749ad870a 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -64,8 +64,21 @@ set(ignore gtsam::Point3 gtsam::CustomFactor) +set(interface_files + ${GTSAM_SOURCE_DIR}/gtsam/gtsam.i + ${GTSAM_SOURCE_DIR}/gtsam/base/base.i + ${GTSAM_SOURCE_DIR}/gtsam/basis/basis.i + ${GTSAM_SOURCE_DIR}/gtsam/geometry/geometry.i + ${GTSAM_SOURCE_DIR}/gtsam/linear/linear.i + ${GTSAM_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i + ${GTSAM_SOURCE_DIR}/gtsam/symbolic/symbolic.i + ${GTSAM_SOURCE_DIR}/gtsam/sam/sam.i + ${GTSAM_SOURCE_DIR}/gtsam/slam/slam.i + ${GTSAM_SOURCE_DIR}/gtsam/sfm/sfm.i + ${GTSAM_SOURCE_DIR}/gtsam/navigation/navigation.i +) # Wrap -matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam/gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}" +matlab_wrap("${interface_files}" "gtsam" "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}" "${ignore}") # Wrap version for gtsam_unstable diff --git a/matlab/gtsam_tests/testJacobianFactor.m b/matlab/gtsam_tests/testJacobianFactor.m index 1c214c3bc..7a2d344b1 100644 --- a/matlab/gtsam_tests/testJacobianFactor.m +++ b/matlab/gtsam_tests/testJacobianFactor.m @@ -32,7 +32,7 @@ x1 = 3; % the RHS b2=[-1;1.5;2;-1]; sigmas = [1;1;1;1]; -model4 = noiseModel.Diagonal.Sigmas(sigmas); +model4 = noiseModel.Diagonal.Sigmas(sigmas, true); combined = JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4); % eliminate the first variable (x2) in the combined factor, destructive ! @@ -74,7 +74,7 @@ Bx1 = [ % the RHS b1= [0.0;0.894427]; -model2 = noiseModel.Diagonal.Sigmas([1;1]); +model2 = noiseModel.Diagonal.Sigmas([1;1], true); expectedLF = JacobianFactor(l1, Bl1, x1, Bx1, b1, model2); % check if the result matches the combined (reduced) factor diff --git a/matlab/gtsam_tests/testKalmanFilter.m b/matlab/gtsam_tests/testKalmanFilter.m index 3564c6b7a..46846b8f7 100644 --- a/matlab/gtsam_tests/testKalmanFilter.m +++ b/matlab/gtsam_tests/testKalmanFilter.m @@ -23,13 +23,13 @@ import gtsam.* F = eye(2,2); B = eye(2,2); u = [1.0; 0.0]; -modelQ = noiseModel.Diagonal.Sigmas([0.1;0.1]); +modelQ = noiseModel.Diagonal.Sigmas([0.1;0.1], true); Q = 0.01*eye(2,2); H = eye(2,2); z1 = [1.0, 0.0]'; z2 = [2.0, 0.0]'; z3 = [3.0, 0.0]'; -modelR = noiseModel.Diagonal.Sigmas([0.1;0.1]); +modelR = noiseModel.Diagonal.Sigmas([0.1;0.1], true); R = 0.01*eye(2,2); %% Create the set of expected output TestValues diff --git a/matlab/gtsam_tests/testLocalizationExample.m b/matlab/gtsam_tests/testLocalizationExample.m index ed091ea71..6c79eada5 100644 --- a/matlab/gtsam_tests/testLocalizationExample.m +++ b/matlab/gtsam_tests/testLocalizationExample.m @@ -17,7 +17,7 @@ graph = NonlinearFactorGraph; %% Add two odometry factors odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) -odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta +odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true); % 20cm std on x,y, 0.1 rad on theta graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)); graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)); @@ -27,7 +27,7 @@ groundTruth = Values; groundTruth.insert(1, Pose2(0.0, 0.0, 0.0)); groundTruth.insert(2, Pose2(2.0, 0.0, 0.0)); groundTruth.insert(3, Pose2(4.0, 0.0, 0.0)); -model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]); +model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10], true); for i=1:3 graph.add(PriorFactorPose2(i, groundTruth.atPose2(i), model)); end diff --git a/matlab/gtsam_tests/testOdometryExample.m b/matlab/gtsam_tests/testOdometryExample.m index 9bd4762a7..744d1af6e 100644 --- a/matlab/gtsam_tests/testOdometryExample.m +++ b/matlab/gtsam_tests/testOdometryExample.m @@ -17,12 +17,12 @@ graph = NonlinearFactorGraph; %% Add a Gaussian prior on pose x_1 priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin -priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta +priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1], true); % 30cm std on x,y, 0.1 rad on theta graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph %% Add two odometry factors odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) -odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta +odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true); % 20cm std on x,y, 0.1 rad on theta graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)); graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)); diff --git a/matlab/gtsam_tests/testPlanarSLAMExample.m b/matlab/gtsam_tests/testPlanarSLAMExample.m index e0b4dbfc8..8cb2826fd 100644 --- a/matlab/gtsam_tests/testPlanarSLAMExample.m +++ b/matlab/gtsam_tests/testPlanarSLAMExample.m @@ -30,18 +30,18 @@ graph = NonlinearFactorGraph; %% Add prior priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin -priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); +priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1], true); graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph %% Add odometry odometry = Pose2(2.0, 0.0, 0.0); -odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); +odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true); graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise)); graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); %% Add bearing/range measurement factors degrees = pi/180; -brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); +brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2], true); graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise)); graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise)); graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise)); diff --git a/matlab/gtsam_tests/testPose2SLAMExample.m b/matlab/gtsam_tests/testPose2SLAMExample.m index 72e5331f2..17374c71b 100644 --- a/matlab/gtsam_tests/testPose2SLAMExample.m +++ b/matlab/gtsam_tests/testPose2SLAMExample.m @@ -26,19 +26,19 @@ graph = NonlinearFactorGraph; %% Add prior % gaussian for prior priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin -priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); +priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1], true); graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph %% Add odometry % general noisemodel for odometry -odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); +odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true); graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise)); graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise)); graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise)); graph.add(BetweenFactorPose2(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise)); %% Add pose constraint -model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); +model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true); graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi/2), model)); %% Initialize to noisy points diff --git a/matlab/gtsam_tests/testPose3SLAMExample.m b/matlab/gtsam_tests/testPose3SLAMExample.m index 51ba69240..d5e23ee77 100644 --- a/matlab/gtsam_tests/testPose3SLAMExample.m +++ b/matlab/gtsam_tests/testPose3SLAMExample.m @@ -21,7 +21,7 @@ p1 = hexagon.atPose3(1); fg = NonlinearFactorGraph; fg.add(NonlinearEqualityPose3(0, p0)); delta = p0.between(p1); -covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); +covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180], true); fg.add(BetweenFactorPose3(0,1, delta, covariance)); fg.add(BetweenFactorPose3(1,2, delta, covariance)); fg.add(BetweenFactorPose3(2,3, delta, covariance)); diff --git a/matlab/gtsam_tests/testSFMExample.m b/matlab/gtsam_tests/testSFMExample.m index a1f63c3a7..6ac41eedc 100644 --- a/matlab/gtsam_tests/testSFMExample.m +++ b/matlab/gtsam_tests/testSFMExample.m @@ -25,7 +25,7 @@ poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; graph = NonlinearFactorGraph; %% Add factors for all measurements -measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma); +measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma, true); for i=1:length(data.Z) for k=1:length(data.Z{i}) j = data.J{i}{k}; @@ -33,9 +33,9 @@ for i=1:length(data.Z) end end -posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); +posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas, true); graph.add(PriorFactorPose3(symbol('x',1), truth.cameras{1}.pose, posePriorNoise)); -pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma); +pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma, true); graph.add(PriorFactorPoint3(symbol('p',1), truth.points{1}, pointPriorNoise)); %% Initial estimate diff --git a/matlab/gtsam_tests/testSerialization.m b/matlab/gtsam_tests/testSerialization.m index e55822c1c..2f1d116e8 100644 --- a/matlab/gtsam_tests/testSerialization.m +++ b/matlab/gtsam_tests/testSerialization.m @@ -45,30 +45,30 @@ graph = NonlinearFactorGraph; % Prior factor priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin -priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); +priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1], true); graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph % Between Factors odometry = Pose2(2.0, 0.0, 0.0); -odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); +odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true); graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise)); graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); % Range Factors -rNoise = noiseModel.Diagonal.Sigmas([0.2]); +rNoise = noiseModel.Diagonal.Sigmas([0.2], true); graph.add(RangeFactor2D(i1, j1, sqrt(4+4), rNoise)); graph.add(RangeFactor2D(i2, j1, 2, rNoise)); graph.add(RangeFactor2D(i3, j2, 2, rNoise)); % Bearing Factors degrees = pi/180; -bNoise = noiseModel.Diagonal.Sigmas([0.1]); +bNoise = noiseModel.Diagonal.Sigmas([0.1], true); graph.add(BearingFactor2D(i1, j1, Rot2(45*degrees), bNoise)); graph.add(BearingFactor2D(i2, j1, Rot2(90*degrees), bNoise)); graph.add(BearingFactor2D(i3, j2, Rot2(90*degrees), bNoise)); % BearingRange Factors -brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); +brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2], true); graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise)); graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise)); graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise)); diff --git a/matlab/gtsam_tests/testStereoVOExample.m b/matlab/gtsam_tests/testStereoVOExample.m index c721244ba..8edbb1553 100644 --- a/matlab/gtsam_tests/testStereoVOExample.m +++ b/matlab/gtsam_tests/testStereoVOExample.m @@ -33,7 +33,7 @@ graph.add(NonlinearEqualityPose3(x1, first_pose)); %% Create realistic calibration and measurement noise model % format: fx fy skew cx cy baseline K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2); -stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]); +stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0], true); %% Add measurements % pose 1 diff --git a/matlab/gtsam_tests/testUtilities.m b/matlab/gtsam_tests/testUtilities.m index da8dec789..2bfe81a83 100644 --- a/matlab/gtsam_tests/testUtilities.m +++ b/matlab/gtsam_tests/testUtilities.m @@ -45,3 +45,12 @@ CHECK('KeySet', isa(actual,'gtsam.KeySet')); CHECK('size==3', actual.size==3); CHECK('actual.count(x1)', actual.count(x1)); +% test extractVectors +values = Values(); +values.insert(symbol('x', 0), (1:6)'); +values.insert(symbol('x', 1), (7:12)'); +values.insert(symbol('x', 2), (13:18)'); +values.insert(symbol('x', 7), Pose3()); +actual = utilities.extractVectors(values, 'x'); +expected = reshape(1:18, 6, 3)'; +CHECK('extractVectors', all(actual == expected, 'all')); diff --git a/python/gtsam/tests/testEssentialMatrixConstraint.py b/python/gtsam/tests/testEssentialMatrixConstraint.py new file mode 100644 index 000000000..8439ad2e9 --- /dev/null +++ b/python/gtsam/tests/testEssentialMatrixConstraint.py @@ -0,0 +1,47 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +visual_isam unit tests. +Author: Frank Dellaert & Pablo Alcantarilla +""" + +import unittest + +import gtsam +import numpy as np +from gtsam import (EssentialMatrix, EssentialMatrixConstraint, Point3, Pose3, + Rot3, Unit3, symbol) +from gtsam.utils.test_case import GtsamTestCase + + +class TestVisualISAMExample(GtsamTestCase): + def test_VisualISAMExample(self): + + # Create a factor + poseKey1 = symbol('x', 1) + poseKey2 = symbol('x', 2) + trueRotation = Rot3.RzRyRx(0.15, 0.15, -0.20) + trueTranslation = Point3(+0.5, -1.0, +1.0) + trueDirection = Unit3(trueTranslation) + E = EssentialMatrix(trueRotation, trueDirection) + model = gtsam.noiseModel.Isotropic.Sigma(5, 0.25) + factor = EssentialMatrixConstraint(poseKey1, poseKey2, E, model) + + # Create a linearization point at the zero-error point + pose1 = Pose3(Rot3.RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0)) + pose2 = Pose3( + Rot3.RzRyRx(0.179693265735950, 0.002945368776519, + 0.102274823253840), + Point3(-3.37493895, 6.14660244, -8.93650986)) + + expected = np.zeros((5, 1)) + actual = factor.evaluateError(pose1, pose2) + self.gtsamAssertEquals(actual, expected, 1e-8) + + +if __name__ == "__main__": + unittest.main() diff --git a/wrap/cmake/MatlabWrap.cmake b/wrap/cmake/MatlabWrap.cmake index 083b88566..3cb058102 100644 --- a/wrap/cmake/MatlabWrap.cmake +++ b/wrap/cmake/MatlabWrap.cmake @@ -62,10 +62,10 @@ macro(find_and_configure_matlab) endmacro() # Consistent and user-friendly wrap function -function(matlab_wrap interfaceHeader linkLibraries +function(matlab_wrap interfaceHeader moduleName linkLibraries extraIncludeDirs extraMexFlags ignore_classes) find_and_configure_matlab() - wrap_and_install_library("${interfaceHeader}" "${linkLibraries}" + wrap_and_install_library("${interfaceHeader}" "${moduleName}" "${linkLibraries}" "${extraIncludeDirs}" "${extraMexFlags}" "${ignore_classes}") endfunction() @@ -77,6 +77,7 @@ endfunction() # Arguments: # # interfaceHeader: The relative path to the wrapper interface definition file. +# moduleName: The name of the wrapped module, e.g. gtsam # linkLibraries: Any *additional* libraries to link. Your project library # (e.g. `lba`), libraries it depends on, and any necessary MATLAB libraries will # be linked automatically. So normally, leave this empty. @@ -85,15 +86,15 @@ endfunction() # extraMexFlags: Any *additional* flags to pass to the compiler when building # the wrap code. Normally, leave this empty. # ignore_classes: List of classes to ignore in the wrapping. -function(wrap_and_install_library interfaceHeader linkLibraries +function(wrap_and_install_library interfaceHeader moduleName linkLibraries extraIncludeDirs extraMexFlags ignore_classes) - wrap_library_internal("${interfaceHeader}" "${linkLibraries}" + wrap_library_internal("${interfaceHeader}" "${moduleName}" "${linkLibraries}" "${extraIncludeDirs}" "${mexFlags}") - install_wrapped_library_internal("${interfaceHeader}") + install_wrapped_library_internal("${moduleName}") endfunction() # Internal function that wraps a library and compiles the wrapper -function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs +function(wrap_library_internal interfaceHeader moduleName linkLibraries extraIncludeDirs extraMexFlags) if(UNIX AND NOT APPLE) if(CMAKE_SIZEOF_VOID_P EQUAL 8) @@ -120,7 +121,6 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs # Extract module name from interface header file name get_filename_component(interfaceHeader "${interfaceHeader}" ABSOLUTE) get_filename_component(modulePath "${interfaceHeader}" PATH) - get_filename_component(moduleName "${interfaceHeader}" NAME_WE) # Paths for generated files set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}") @@ -136,8 +136,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs # explicit link libraries list so that the next block of code can unpack any # static libraries set(automaticDependencies "") - foreach(lib ${moduleName} ${linkLibraries}) - # message("MODULE NAME: ${moduleName}") + foreach(lib ${module} ${linkLibraries}) if(TARGET "${lib}") get_target_property(dependentLibraries ${lib} INTERFACE_LINK_LIBRARIES) # message("DEPENDENT LIBRARIES: ${dependentLibraries}") @@ -176,7 +175,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs set(otherLibraryTargets "") set(otherLibraryNontargets "") set(otherSourcesAndObjects "") - foreach(lib ${moduleName} ${linkLibraries} ${automaticDependencies}) + foreach(lib ${module} ${linkLibraries} ${automaticDependencies}) if(TARGET "${lib}") if(WRAP_MEX_BUILD_STATIC_MODULE) get_target_property(target_sources ${lib} SOURCES) @@ -250,7 +249,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs COMMAND ${CMAKE_COMMAND} -E env "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}" - ${PYTHON_EXECUTABLE} ${MATLAB_WRAP_SCRIPT} --src ${interfaceHeader} + ${PYTHON_EXECUTABLE} ${MATLAB_WRAP_SCRIPT} --src "${interfaceHeader}" --module_name ${moduleName} --out ${generated_files_path} --top_module_namespaces ${moduleName} --ignore ${ignore_classes} VERBATIM @@ -324,8 +323,8 @@ endfunction() # Internal function that installs a wrap toolbox function(install_wrapped_library_internal interfaceHeader) - get_filename_component(moduleName "${interfaceHeader}" NAME_WE) - set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}") + get_filename_component(module "${interfaceHeader}" NAME_WE) + set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${module}") # NOTE: only installs .m and mex binary files (not .cpp) - the trailing slash # on the directory name here prevents creating the top-level module name diff --git a/wrap/gtwrap/interface_parser/type.py b/wrap/gtwrap/interface_parser/type.py index e94db4ff2..7aacf0b81 100644 --- a/wrap/gtwrap/interface_parser/type.py +++ b/wrap/gtwrap/interface_parser/type.py @@ -53,6 +53,10 @@ class Typename: self.name = t[-1] # the name is the last element in this list self.namespaces = t[:-1] + # If the first namespace is empty string, just get rid of it. + if self.namespaces and self.namespaces[0] == '': + self.namespaces.pop(0) + if instantiations: if isinstance(instantiations, Sequence): self.instantiations = instantiations # type: ignore @@ -92,8 +96,8 @@ class Typename: else: cpp_name = self.name return '{}{}{}'.format( - "::".join(self.namespaces[idx:]), - "::" if self.namespaces[idx:] else "", + "::".join(self.namespaces), + "::" if self.namespaces else "", cpp_name, ) diff --git a/wrap/gtwrap/matlab_wrapper/mixins.py b/wrap/gtwrap/matlab_wrapper/mixins.py index 2d7c75b39..f4a7988fd 100644 --- a/wrap/gtwrap/matlab_wrapper/mixins.py +++ b/wrap/gtwrap/matlab_wrapper/mixins.py @@ -108,7 +108,7 @@ class FormatMixin: elif is_method: formatted_type_name += self.data_type_param.get(name) or name else: - formatted_type_name += name + formatted_type_name += str(name) if separator == "::": # C++ templates = [] @@ -192,10 +192,9 @@ class FormatMixin: method = '' if isinstance(static_method, parser.StaticMethod): - method += "".join([separator + x for x in static_method.parent.namespaces()]) + \ - separator + static_method.parent.name + separator + method += static_method.parent.to_cpp() + separator - return method[2 * len(separator):] + return method def _format_global_function(self, function: Union[parser.GlobalFunction, Any], diff --git a/wrap/gtwrap/matlab_wrapper/wrapper.py b/wrap/gtwrap/matlab_wrapper/wrapper.py index 97945f73a..b87db23f3 100755 --- a/wrap/gtwrap/matlab_wrapper/wrapper.py +++ b/wrap/gtwrap/matlab_wrapper/wrapper.py @@ -239,18 +239,18 @@ class MatlabWrapper(CheckMixin, FormatMixin): return var_list_wrap - def _wrap_method_check_statement(self, args): + def _wrap_method_check_statement(self, args: parser.ArgumentList): """ Wrap the given arguments into either just a varargout call or a call in an if statement that checks if the parameters are accurate. + + TODO Update this method so that default arguments are supported. """ - check_statement = '' arg_id = 1 - if check_statement == '': - check_statement = \ - 'if length(varargin) == {param_count}'.format( - param_count=len(args.list())) + param_count = len(args) + check_statement = 'if length(varargin) == {param_count}'.format( + param_count=param_count) for _, arg in enumerate(args.list()): name = arg.ctype.typename.name @@ -809,7 +809,7 @@ class MatlabWrapper(CheckMixin, FormatMixin): for static_method in static_methods: format_name = list(static_method[0].name) - format_name[0] = format_name[0].upper() + format_name[0] = format_name[0] if static_method[0].name in self.ignore_methods: continue @@ -850,12 +850,13 @@ class MatlabWrapper(CheckMixin, FormatMixin): wrapper=self._wrapper_name(), id=self._update_wrapper_id( (namespace_name, instantiated_class, - static_overload.name, static_overload)), + static_overload.name, static_overload)), class_name=instantiated_class.name, end_statement=end_statement), - prefix=' ') + prefix=' ') - #TODO Figure out what is static_overload doing here. + # If the arguments don't match any of the checks above, + # throw an error with the class and method name. method_text += textwrap.indent(textwrap.dedent("""\ error('Arguments do not match any overload of function {class_name}.{method_name}'); """.format(class_name=class_name, @@ -1081,7 +1082,6 @@ class MatlabWrapper(CheckMixin, FormatMixin): obj_start = '' if isinstance(method, instantiator.InstantiatedMethod): - # method_name = method.original.name method_name = method.to_cpp() obj_start = 'obj->' @@ -1090,6 +1090,10 @@ class MatlabWrapper(CheckMixin, FormatMixin): # self._format_type_name(method.instantiations)) method = method.to_cpp() + elif isinstance(method, instantiator.InstantiatedStaticMethod): + method_name = self._format_static_method(method, '::') + method_name += method.original.name + elif isinstance(method, parser.GlobalFunction): method_name = self._format_global_function(method, '::') method_name += method.name @@ -1250,7 +1254,7 @@ class MatlabWrapper(CheckMixin, FormatMixin): method_name = '' if is_static_method: - method_name = self._format_static_method(extra) + '.' + method_name = self._format_static_method(extra, '.') method_name += extra.name @@ -1567,23 +1571,23 @@ class MatlabWrapper(CheckMixin, FormatMixin): def wrap(self, files, path): """High level function to wrap the project.""" + content = "" modules = {} for file in files: with open(file, 'r') as f: - content = f.read() + content += f.read() - # Parse the contents of the interface file - parsed_result = parser.Module.parseString(content) - # print(parsed_result) + # Parse the contents of the interface file + parsed_result = parser.Module.parseString(content) - # Instantiate the module - module = instantiator.instantiate_namespace(parsed_result) + # Instantiate the module + module = instantiator.instantiate_namespace(parsed_result) - if module.name in modules: - modules[module. - name].content[0].content += module.content[0].content - else: - modules[module.name] = module + if module.name in modules: + modules[ + module.name].content[0].content += module.content[0].content + else: + modules[module.name] = module for module in modules.values(): # Wrap the full namespace diff --git a/wrap/gtwrap/template_instantiator/helpers.py b/wrap/gtwrap/template_instantiator/helpers.py index b55515dba..194c6f686 100644 --- a/wrap/gtwrap/template_instantiator/helpers.py +++ b/wrap/gtwrap/template_instantiator/helpers.py @@ -55,16 +55,14 @@ def instantiate_type( # make a deep copy so that there is no overwriting of original template params ctype = deepcopy(ctype) - # Check if the return type has template parameters + # Check if the return type has template parameters as the typename's name if ctype.typename.instantiations: for idx, instantiation in enumerate(ctype.typename.instantiations): if instantiation.name in template_typenames: template_idx = template_typenames.index(instantiation.name) - ctype.typename.instantiations[ - idx] = instantiations[ # type: ignore - template_idx] + ctype.typename.instantiations[idx].name =\ + instantiations[template_idx] - return ctype str_arg_typename = str(ctype.typename) @@ -125,9 +123,18 @@ def instantiate_type( # Case when 'This' is present in the type namespace, e.g `This::Subclass`. elif 'This' in str_arg_typename: - # Simply get the index of `This` in the namespace and replace it with the instantiated name. - namespace_idx = ctype.typename.namespaces.index('This') - ctype.typename.namespaces[namespace_idx] = cpp_typename.name + # Check if `This` is in the namespaces + if 'This' in ctype.typename.namespaces: + # Simply get the index of `This` in the namespace and + # replace it with the instantiated name. + namespace_idx = ctype.typename.namespaces.index('This') + ctype.typename.namespaces[namespace_idx] = cpp_typename.name + # Else check if it is in the template namespace, e.g vector + else: + for idx, instantiation in enumerate(ctype.typename.instantiations): + if 'This' in instantiation.namespaces: + ctype.typename.instantiations[idx].namespaces = \ + cpp_typename.namespaces + [cpp_typename.name] return ctype else: diff --git a/wrap/tests/expected/matlab/+gtsam/GeneralSFMFactorCal3Bundler.m b/wrap/tests/expected/matlab/+gtsam/GeneralSFMFactorCal3Bundler.m new file mode 100644 index 000000000..0ce4051af --- /dev/null +++ b/wrap/tests/expected/matlab/+gtsam/GeneralSFMFactorCal3Bundler.m @@ -0,0 +1,31 @@ +%class GeneralSFMFactorCal3Bundler, see Doxygen page for details +%at https://gtsam.org/doxygen/ +% +classdef GeneralSFMFactorCal3Bundler < handle + properties + ptr_gtsamGeneralSFMFactorCal3Bundler = 0 + end + methods + function obj = GeneralSFMFactorCal3Bundler(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + special_cases_wrapper(7, my_ptr); + else + error('Arguments do not match any overload of gtsam.GeneralSFMFactorCal3Bundler constructor'); + end + obj.ptr_gtsamGeneralSFMFactorCal3Bundler = my_ptr; + end + + function delete(obj) + special_cases_wrapper(8, obj.ptr_gtsamGeneralSFMFactorCal3Bundler); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected/matlab/+gtsam/Point3.m b/wrap/tests/expected/matlab/+gtsam/Point3.m index 06d378ac2..b3290faf2 100644 --- a/wrap/tests/expected/matlab/+gtsam/Point3.m +++ b/wrap/tests/expected/matlab/+gtsam/Point3.m @@ -78,7 +78,7 @@ classdef Point3 < handle error('Arguments do not match any overload of function Point3.StaticFunctionRet'); end - function varargout = StaticFunction(varargin) + function varargout = staticFunction(varargin) % STATICFUNCTION usage: staticFunction() : returns double % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 diff --git a/wrap/tests/expected/matlab/+gtsam/SfmTrack.m b/wrap/tests/expected/matlab/+gtsam/SfmTrack.m new file mode 100644 index 000000000..428da2706 --- /dev/null +++ b/wrap/tests/expected/matlab/+gtsam/SfmTrack.m @@ -0,0 +1,31 @@ +%class SfmTrack, see Doxygen page for details +%at https://gtsam.org/doxygen/ +% +classdef SfmTrack < handle + properties + ptr_gtsamSfmTrack = 0 + end + methods + function obj = SfmTrack(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + special_cases_wrapper(3, my_ptr); + else + error('Arguments do not match any overload of gtsam.SfmTrack constructor'); + end + obj.ptr_gtsamSfmTrack = my_ptr; + end + + function delete(obj) + special_cases_wrapper(4, obj.ptr_gtsamSfmTrack); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected/matlab/+gtsam/Values.m b/wrap/tests/expected/matlab/+gtsam/Values.m new file mode 100644 index 000000000..d85b24b91 --- /dev/null +++ b/wrap/tests/expected/matlab/+gtsam/Values.m @@ -0,0 +1,59 @@ +%class Values, see Doxygen page for details +%at https://gtsam.org/doxygen/ +% +%-------Constructors------- +%Values() +%Values(Values other) +% +%-------Methods------- +%insert(size_t j, Vector vector) : returns void +%insert(size_t j, Matrix matrix) : returns void +% +classdef Values < handle + properties + ptr_gtsamValues = 0 + end + methods + function obj = Values(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + namespaces_wrapper(26, my_ptr); + elseif nargin == 0 + my_ptr = namespaces_wrapper(27); + elseif nargin == 1 && isa(varargin{1},'gtsam.Values') + my_ptr = namespaces_wrapper(28, varargin{1}); + else + error('Arguments do not match any overload of gtsam.Values constructor'); + end + obj.ptr_gtsamValues = my_ptr; + end + + function delete(obj) + namespaces_wrapper(29, obj.ptr_gtsamValues); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + function varargout = insert(this, varargin) + % INSERT usage: insert(size_t j, Vector vector) : returns void + % Doxygen can be found at https://gtsam.org/doxygen/ + if length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double') && size(varargin{2},2)==1 + namespaces_wrapper(30, this, varargin{:}); + return + end + % INSERT usage: insert(size_t j, Matrix matrix) : returns void + % Doxygen can be found at https://gtsam.org/doxygen/ + if length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double') + namespaces_wrapper(31, this, varargin{:}); + return + end + error('Arguments do not match any overload of function gtsam.Values.insert'); + end + + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected/matlab/+ns2/ClassA.m b/wrap/tests/expected/matlab/+ns2/ClassA.m index 4640e7cca..71718ccba 100644 --- a/wrap/tests/expected/matlab/+ns2/ClassA.m +++ b/wrap/tests/expected/matlab/+ns2/ClassA.m @@ -74,7 +74,7 @@ classdef ClassA < handle end methods(Static = true) - function varargout = Afunction(varargin) + function varargout = afunction(varargin) % AFUNCTION usage: afunction() : returns double % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 diff --git a/wrap/tests/expected/matlab/DefaultFuncInt.m b/wrap/tests/expected/matlab/DefaultFuncInt.m new file mode 100644 index 000000000..284aaa9f0 --- /dev/null +++ b/wrap/tests/expected/matlab/DefaultFuncInt.m @@ -0,0 +1,6 @@ +function varargout = DefaultFuncInt(varargin) + if length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') + functions_wrapper(8, varargin{:}); + else + error('Arguments do not match any overload of function DefaultFuncInt'); + end diff --git a/wrap/tests/expected/matlab/DefaultFuncObj.m b/wrap/tests/expected/matlab/DefaultFuncObj.m new file mode 100644 index 000000000..d2006dcad --- /dev/null +++ b/wrap/tests/expected/matlab/DefaultFuncObj.m @@ -0,0 +1,6 @@ +function varargout = DefaultFuncObj(varargin) + if length(varargin) == 1 && isa(varargin{1},'gtsam.KeyFormatter') + functions_wrapper(10, varargin{:}); + else + error('Arguments do not match any overload of function DefaultFuncObj'); + end diff --git a/wrap/tests/expected/matlab/DefaultFuncString.m b/wrap/tests/expected/matlab/DefaultFuncString.m new file mode 100644 index 000000000..86572ffbf --- /dev/null +++ b/wrap/tests/expected/matlab/DefaultFuncString.m @@ -0,0 +1,6 @@ +function varargout = DefaultFuncString(varargin) + if length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'char') + functions_wrapper(9, varargin{:}); + else + error('Arguments do not match any overload of function DefaultFuncString'); + end diff --git a/wrap/tests/expected/matlab/DefaultFuncVector.m b/wrap/tests/expected/matlab/DefaultFuncVector.m new file mode 100644 index 000000000..9e4db181d --- /dev/null +++ b/wrap/tests/expected/matlab/DefaultFuncVector.m @@ -0,0 +1,6 @@ +function varargout = DefaultFuncVector(varargin) + if length(varargin) == 2 && isa(varargin{1},'std.vectornumeric') && isa(varargin{2},'std.vectorchar') + functions_wrapper(12, varargin{:}); + else + error('Arguments do not match any overload of function DefaultFuncVector'); + end diff --git a/wrap/tests/expected/matlab/DefaultFuncZero.m b/wrap/tests/expected/matlab/DefaultFuncZero.m new file mode 100644 index 000000000..7d37dcfa7 --- /dev/null +++ b/wrap/tests/expected/matlab/DefaultFuncZero.m @@ -0,0 +1,6 @@ +function varargout = DefaultFuncZero(varargin) + if length(varargin) == 5 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'logical') && isa(varargin{5},'logical') + functions_wrapper(11, varargin{:}); + else + error('Arguments do not match any overload of function DefaultFuncZero'); + end diff --git a/wrap/tests/expected/matlab/ForwardKinematics.m b/wrap/tests/expected/matlab/ForwardKinematics.m new file mode 100644 index 000000000..e420dcc46 --- /dev/null +++ b/wrap/tests/expected/matlab/ForwardKinematics.m @@ -0,0 +1,36 @@ +%class ForwardKinematics, see Doxygen page for details +%at https://gtsam.org/doxygen/ +% +%-------Constructors------- +%ForwardKinematics(Robot robot, string start_link_name, string end_link_name, Values joint_angles, Pose3 l2Tp) +% +classdef ForwardKinematics < handle + properties + ptr_ForwardKinematics = 0 + end + methods + function obj = ForwardKinematics(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + class_wrapper(55, my_ptr); + elseif nargin == 5 && isa(varargin{1},'gtdynamics.Robot') && isa(varargin{2},'char') && isa(varargin{3},'char') && isa(varargin{4},'gtsam.Values') && isa(varargin{5},'gtsam.Pose3') + my_ptr = class_wrapper(56, varargin{1}, varargin{2}, varargin{3}, varargin{4}, varargin{5}); + else + error('Arguments do not match any overload of ForwardKinematics constructor'); + end + obj.ptr_ForwardKinematics = my_ptr; + end + + function delete(obj) + class_wrapper(57, obj.ptr_ForwardKinematics); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected/matlab/ForwardKinematicsFactor.m b/wrap/tests/expected/matlab/ForwardKinematicsFactor.m new file mode 100644 index 000000000..46aa41392 --- /dev/null +++ b/wrap/tests/expected/matlab/ForwardKinematicsFactor.m @@ -0,0 +1,36 @@ +%class ForwardKinematicsFactor, see Doxygen page for details +%at https://gtsam.org/doxygen/ +% +classdef ForwardKinematicsFactor < gtsam.BetweenFactor + properties + ptr_ForwardKinematicsFactor = 0 + end + methods + function obj = ForwardKinematicsFactor(varargin) + if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + if nargin == 2 + my_ptr = varargin{2}; + else + my_ptr = inheritance_wrapper(36, varargin{2}); + end + base_ptr = inheritance_wrapper(35, my_ptr); + else + error('Arguments do not match any overload of ForwardKinematicsFactor constructor'); + end + obj = obj@gtsam.BetweenFactorPose3(uint64(5139824614673773682), base_ptr); + obj.ptr_ForwardKinematicsFactor = my_ptr; + end + + function delete(obj) + inheritance_wrapper(37, obj.ptr_ForwardKinematicsFactor); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected/matlab/FunDouble.m b/wrap/tests/expected/matlab/FunDouble.m index 78609c7f6..5f432341b 100644 --- a/wrap/tests/expected/matlab/FunDouble.m +++ b/wrap/tests/expected/matlab/FunDouble.m @@ -3,6 +3,7 @@ % %-------Methods------- %multiTemplatedMethodStringSize_t(double d, string t, size_t u) : returns Fun +%sets() : returns std::map::double> %templatedMethodString(double d, string t) : returns Fun % %-------Static Methods------- @@ -46,11 +47,21 @@ classdef FunDouble < handle error('Arguments do not match any overload of function FunDouble.multiTemplatedMethodStringSize_t'); end + function varargout = sets(this, varargin) + % SETS usage: sets() : returns std.mapdoubledouble + % Doxygen can be found at https://gtsam.org/doxygen/ + if length(varargin) == 0 + varargout{1} = class_wrapper(8, this, varargin{:}); + return + end + error('Arguments do not match any overload of function FunDouble.sets'); + end + function varargout = templatedMethodString(this, varargin) % TEMPLATEDMETHODSTRING usage: templatedMethodString(double d, string t) : returns Fun % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 2 && isa(varargin{1},'double') && isa(varargin{2},'char') - varargout{1} = class_wrapper(8, this, varargin{:}); + varargout{1} = class_wrapper(9, this, varargin{:}); return end error('Arguments do not match any overload of function FunDouble.templatedMethodString'); @@ -59,22 +70,22 @@ classdef FunDouble < handle end methods(Static = true) - function varargout = StaticMethodWithThis(varargin) + function varargout = staticMethodWithThis(varargin) % STATICMETHODWITHTHIS usage: staticMethodWithThis() : returns Fundouble % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 - varargout{1} = class_wrapper(9, varargin{:}); + varargout{1} = class_wrapper(10, varargin{:}); return end error('Arguments do not match any overload of function FunDouble.staticMethodWithThis'); end - function varargout = TemplatedStaticMethodInt(varargin) + function varargout = templatedStaticMethodInt(varargin) % TEMPLATEDSTATICMETHODINT usage: templatedStaticMethodInt(int m) : returns double % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'numeric') - varargout{1} = class_wrapper(10, varargin{:}); + varargout{1} = class_wrapper(11, varargin{:}); return end diff --git a/wrap/tests/expected/matlab/FunRange.m b/wrap/tests/expected/matlab/FunRange.m index 1d1a6f7b8..52ee78aa2 100644 --- a/wrap/tests/expected/matlab/FunRange.m +++ b/wrap/tests/expected/matlab/FunRange.m @@ -52,7 +52,7 @@ classdef FunRange < handle end methods(Static = true) - function varargout = Create(varargin) + function varargout = create(varargin) % CREATE usage: create() : returns FunRange % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 diff --git a/wrap/tests/expected/matlab/MultipleTemplatesIntDouble.m b/wrap/tests/expected/matlab/MultipleTemplatesIntDouble.m index 863d30ee8..80e6eb6c5 100644 --- a/wrap/tests/expected/matlab/MultipleTemplatesIntDouble.m +++ b/wrap/tests/expected/matlab/MultipleTemplatesIntDouble.m @@ -9,7 +9,7 @@ classdef MultipleTemplatesIntDouble < handle function obj = MultipleTemplatesIntDouble(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(50, my_ptr); + class_wrapper(51, my_ptr); else error('Arguments do not match any overload of MultipleTemplatesIntDouble constructor'); end @@ -17,7 +17,7 @@ classdef MultipleTemplatesIntDouble < handle end function delete(obj) - class_wrapper(51, obj.ptr_MultipleTemplatesIntDouble); + class_wrapper(52, obj.ptr_MultipleTemplatesIntDouble); end function display(obj), obj.print(''); end diff --git a/wrap/tests/expected/matlab/MultipleTemplatesIntFloat.m b/wrap/tests/expected/matlab/MultipleTemplatesIntFloat.m index b7f1fdac5..5e9c3a8b4 100644 --- a/wrap/tests/expected/matlab/MultipleTemplatesIntFloat.m +++ b/wrap/tests/expected/matlab/MultipleTemplatesIntFloat.m @@ -9,7 +9,7 @@ classdef MultipleTemplatesIntFloat < handle function obj = MultipleTemplatesIntFloat(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(52, my_ptr); + class_wrapper(53, my_ptr); else error('Arguments do not match any overload of MultipleTemplatesIntFloat constructor'); end @@ -17,7 +17,7 @@ classdef MultipleTemplatesIntFloat < handle end function delete(obj) - class_wrapper(53, obj.ptr_MultipleTemplatesIntFloat); + class_wrapper(54, obj.ptr_MultipleTemplatesIntFloat); end function display(obj), obj.print(''); end diff --git a/wrap/tests/expected/matlab/MyFactorPosePoint2.m b/wrap/tests/expected/matlab/MyFactorPosePoint2.m index 7634ae2cb..56843ed0a 100644 --- a/wrap/tests/expected/matlab/MyFactorPosePoint2.m +++ b/wrap/tests/expected/matlab/MyFactorPosePoint2.m @@ -15,9 +15,9 @@ classdef MyFactorPosePoint2 < handle function obj = MyFactorPosePoint2(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(63, my_ptr); + class_wrapper(64, my_ptr); elseif nargin == 4 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'gtsam.noiseModel.Base') - my_ptr = class_wrapper(64, varargin{1}, varargin{2}, varargin{3}, varargin{4}); + my_ptr = class_wrapper(65, varargin{1}, varargin{2}, varargin{3}, varargin{4}); else error('Arguments do not match any overload of MyFactorPosePoint2 constructor'); end @@ -25,7 +25,7 @@ classdef MyFactorPosePoint2 < handle end function delete(obj) - class_wrapper(65, obj.ptr_MyFactorPosePoint2); + class_wrapper(66, obj.ptr_MyFactorPosePoint2); end function display(obj), obj.print(''); end @@ -36,7 +36,7 @@ classdef MyFactorPosePoint2 < handle % PRINT usage: print(string s, KeyFormatter keyFormatter) : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.KeyFormatter') - class_wrapper(66, this, varargin{:}); + class_wrapper(67, this, varargin{:}); return end error('Arguments do not match any overload of function MyFactorPosePoint2.print'); diff --git a/wrap/tests/expected/matlab/MyVector12.m b/wrap/tests/expected/matlab/MyVector12.m index 291d0d71b..09f5488c9 100644 --- a/wrap/tests/expected/matlab/MyVector12.m +++ b/wrap/tests/expected/matlab/MyVector12.m @@ -12,9 +12,9 @@ classdef MyVector12 < handle function obj = MyVector12(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(47, my_ptr); + class_wrapper(48, my_ptr); elseif nargin == 0 - my_ptr = class_wrapper(48); + my_ptr = class_wrapper(49); else error('Arguments do not match any overload of MyVector12 constructor'); end @@ -22,7 +22,7 @@ classdef MyVector12 < handle end function delete(obj) - class_wrapper(49, obj.ptr_MyVector12); + class_wrapper(50, obj.ptr_MyVector12); end function display(obj), obj.print(''); end diff --git a/wrap/tests/expected/matlab/MyVector3.m b/wrap/tests/expected/matlab/MyVector3.m index 3051dc2e2..1266ddef2 100644 --- a/wrap/tests/expected/matlab/MyVector3.m +++ b/wrap/tests/expected/matlab/MyVector3.m @@ -12,9 +12,9 @@ classdef MyVector3 < handle function obj = MyVector3(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(44, my_ptr); + class_wrapper(45, my_ptr); elseif nargin == 0 - my_ptr = class_wrapper(45); + my_ptr = class_wrapper(46); else error('Arguments do not match any overload of MyVector3 constructor'); end @@ -22,7 +22,7 @@ classdef MyVector3 < handle end function delete(obj) - class_wrapper(46, obj.ptr_MyVector3); + class_wrapper(47, obj.ptr_MyVector3); end function display(obj), obj.print(''); end diff --git a/wrap/tests/expected/matlab/PrimitiveRefDouble.m b/wrap/tests/expected/matlab/PrimitiveRefDouble.m index dd0a6d2da..0b8e7714e 100644 --- a/wrap/tests/expected/matlab/PrimitiveRefDouble.m +++ b/wrap/tests/expected/matlab/PrimitiveRefDouble.m @@ -19,9 +19,9 @@ classdef PrimitiveRefDouble < handle function obj = PrimitiveRefDouble(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(40, my_ptr); + class_wrapper(41, my_ptr); elseif nargin == 0 - my_ptr = class_wrapper(41); + my_ptr = class_wrapper(42); else error('Arguments do not match any overload of PrimitiveRefDouble constructor'); end @@ -29,7 +29,7 @@ classdef PrimitiveRefDouble < handle end function delete(obj) - class_wrapper(42, obj.ptr_PrimitiveRefDouble); + class_wrapper(43, obj.ptr_PrimitiveRefDouble); end function display(obj), obj.print(''); end @@ -43,7 +43,7 @@ classdef PrimitiveRefDouble < handle % BRUTAL usage: Brutal(double t) : returns PrimitiveRefdouble % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = class_wrapper(43, varargin{:}); + varargout{1} = class_wrapper(44, varargin{:}); return end diff --git a/wrap/tests/expected/matlab/ScopedTemplateResult.m b/wrap/tests/expected/matlab/ScopedTemplateResult.m new file mode 100644 index 000000000..8cb8ed7d0 --- /dev/null +++ b/wrap/tests/expected/matlab/ScopedTemplateResult.m @@ -0,0 +1,36 @@ +%class ScopedTemplateResult, see Doxygen page for details +%at https://gtsam.org/doxygen/ +% +%-------Constructors------- +%ScopedTemplateResult(Result::Value arg) +% +classdef ScopedTemplateResult < handle + properties + ptr_ScopedTemplateResult = 0 + end + methods + function obj = ScopedTemplateResult(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + template_wrapper(6, my_ptr); + elseif nargin == 1 && isa(varargin{1},'Result::Value') + my_ptr = template_wrapper(7, varargin{1}); + else + error('Arguments do not match any overload of ScopedTemplateResult constructor'); + end + obj.ptr_ScopedTemplateResult = my_ptr; + end + + function delete(obj) + template_wrapper(8, obj.ptr_ScopedTemplateResult); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected/matlab/TemplatedConstructor.m b/wrap/tests/expected/matlab/TemplatedConstructor.m new file mode 100644 index 000000000..70beb26ce --- /dev/null +++ b/wrap/tests/expected/matlab/TemplatedConstructor.m @@ -0,0 +1,45 @@ +%class TemplatedConstructor, see Doxygen page for details +%at https://gtsam.org/doxygen/ +% +%-------Constructors------- +%TemplatedConstructor() +%TemplatedConstructor(string arg) +%TemplatedConstructor(int arg) +%TemplatedConstructor(double arg) +% +classdef TemplatedConstructor < handle + properties + ptr_TemplatedConstructor = 0 + end + methods + function obj = TemplatedConstructor(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + template_wrapper(0, my_ptr); + elseif nargin == 0 + my_ptr = template_wrapper(1); + elseif nargin == 1 && isa(varargin{1},'char') + my_ptr = template_wrapper(2, varargin{1}); + elseif nargin == 1 && isa(varargin{1},'numeric') + my_ptr = template_wrapper(3, varargin{1}); + elseif nargin == 1 && isa(varargin{1},'double') + my_ptr = template_wrapper(4, varargin{1}); + else + error('Arguments do not match any overload of TemplatedConstructor constructor'); + end + obj.ptr_TemplatedConstructor = my_ptr; + end + + function delete(obj) + template_wrapper(5, obj.ptr_TemplatedConstructor); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected/matlab/Test.m b/wrap/tests/expected/matlab/Test.m index 8569120c5..ac00a6689 100644 --- a/wrap/tests/expected/matlab/Test.m +++ b/wrap/tests/expected/matlab/Test.m @@ -40,11 +40,11 @@ classdef Test < handle function obj = Test(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(11, my_ptr); + class_wrapper(12, my_ptr); elseif nargin == 0 - my_ptr = class_wrapper(12); + my_ptr = class_wrapper(13); elseif nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double') - my_ptr = class_wrapper(13, varargin{1}, varargin{2}); + my_ptr = class_wrapper(14, varargin{1}, varargin{2}); else error('Arguments do not match any overload of Test constructor'); end @@ -52,7 +52,7 @@ classdef Test < handle end function delete(obj) - class_wrapper(14, obj.ptr_Test); + class_wrapper(15, obj.ptr_Test); end function display(obj), obj.print(''); end @@ -63,7 +63,7 @@ classdef Test < handle % ARG_EIGENCONSTREF usage: arg_EigenConstRef(Matrix value) : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') - class_wrapper(15, this, varargin{:}); + class_wrapper(16, this, varargin{:}); return end error('Arguments do not match any overload of function Test.arg_EigenConstRef'); @@ -73,7 +73,7 @@ classdef Test < handle % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Test, Test > % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 - [ varargout{1} varargout{2} ] = class_wrapper(16, this, varargin{:}); + [ varargout{1} varargout{2} ] = class_wrapper(17, this, varargin{:}); return end error('Arguments do not match any overload of function Test.create_MixedPtrs'); @@ -83,7 +83,7 @@ classdef Test < handle % CREATE_PTRS usage: create_ptrs() : returns pair< Test, Test > % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 - [ varargout{1} varargout{2} ] = class_wrapper(17, this, varargin{:}); + [ varargout{1} varargout{2} ] = class_wrapper(18, this, varargin{:}); return end error('Arguments do not match any overload of function Test.create_ptrs'); @@ -93,7 +93,7 @@ classdef Test < handle % GET_CONTAINER usage: get_container() : returns std.vectorTest % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 - varargout{1} = class_wrapper(18, this, varargin{:}); + varargout{1} = class_wrapper(19, this, varargin{:}); return end error('Arguments do not match any overload of function Test.get_container'); @@ -103,7 +103,7 @@ classdef Test < handle % LAMBDA usage: lambda() : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 - class_wrapper(19, this, varargin{:}); + class_wrapper(20, this, varargin{:}); return end error('Arguments do not match any overload of function Test.lambda'); @@ -113,7 +113,7 @@ classdef Test < handle % PRINT usage: print() : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 - class_wrapper(20, this, varargin{:}); + class_wrapper(21, this, varargin{:}); return end error('Arguments do not match any overload of function Test.print'); @@ -123,7 +123,7 @@ classdef Test < handle % RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns Point2 % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'logical') - varargout{1} = class_wrapper(21, this, varargin{:}); + varargout{1} = class_wrapper(22, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_Point2Ptr'); @@ -133,7 +133,7 @@ classdef Test < handle % RETURN_TEST usage: return_Test(Test value) : returns Test % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'Test') - varargout{1} = class_wrapper(22, this, varargin{:}); + varargout{1} = class_wrapper(23, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_Test'); @@ -143,7 +143,7 @@ classdef Test < handle % RETURN_TESTPTR usage: return_TestPtr(Test value) : returns Test % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'Test') - varargout{1} = class_wrapper(23, this, varargin{:}); + varargout{1} = class_wrapper(24, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_TestPtr'); @@ -153,7 +153,7 @@ classdef Test < handle % RETURN_BOOL usage: return_bool(bool value) : returns bool % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'logical') - varargout{1} = class_wrapper(24, this, varargin{:}); + varargout{1} = class_wrapper(25, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_bool'); @@ -163,7 +163,7 @@ classdef Test < handle % RETURN_DOUBLE usage: return_double(double value) : returns double % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = class_wrapper(25, this, varargin{:}); + varargout{1} = class_wrapper(26, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_double'); @@ -173,7 +173,7 @@ classdef Test < handle % RETURN_FIELD usage: return_field(Test t) : returns bool % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'Test') - varargout{1} = class_wrapper(26, this, varargin{:}); + varargout{1} = class_wrapper(27, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_field'); @@ -183,7 +183,7 @@ classdef Test < handle % RETURN_INT usage: return_int(int value) : returns int % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'numeric') - varargout{1} = class_wrapper(27, this, varargin{:}); + varargout{1} = class_wrapper(28, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_int'); @@ -193,7 +193,7 @@ classdef Test < handle % RETURN_MATRIX1 usage: return_matrix1(Matrix value) : returns Matrix % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = class_wrapper(28, this, varargin{:}); + varargout{1} = class_wrapper(29, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_matrix1'); @@ -203,7 +203,7 @@ classdef Test < handle % RETURN_MATRIX2 usage: return_matrix2(Matrix value) : returns Matrix % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = class_wrapper(29, this, varargin{:}); + varargout{1} = class_wrapper(30, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_matrix2'); @@ -213,13 +213,13 @@ classdef Test < handle % RETURN_PAIR usage: return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix > % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 2 && isa(varargin{1},'double') && size(varargin{1},2)==1 && isa(varargin{2},'double') - [ varargout{1} varargout{2} ] = class_wrapper(30, this, varargin{:}); + [ varargout{1} varargout{2} ] = class_wrapper(31, this, varargin{:}); return end % RETURN_PAIR usage: return_pair(Vector v) : returns pair< Vector, Matrix > % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1 - [ varargout{1} varargout{2} ] = class_wrapper(31, this, varargin{:}); + [ varargout{1} varargout{2} ] = class_wrapper(32, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_pair'); @@ -229,7 +229,7 @@ classdef Test < handle % RETURN_PTRS usage: return_ptrs(Test p1, Test p2) : returns pair< Test, Test > % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 2 && isa(varargin{1},'Test') && isa(varargin{2},'Test') - [ varargout{1} varargout{2} ] = class_wrapper(32, this, varargin{:}); + [ varargout{1} varargout{2} ] = class_wrapper(33, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_ptrs'); @@ -239,7 +239,7 @@ classdef Test < handle % RETURN_SIZE_T usage: return_size_t(size_t value) : returns size_t % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'numeric') - varargout{1} = class_wrapper(33, this, varargin{:}); + varargout{1} = class_wrapper(34, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_size_t'); @@ -249,7 +249,7 @@ classdef Test < handle % RETURN_STRING usage: return_string(string value) : returns string % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'char') - varargout{1} = class_wrapper(34, this, varargin{:}); + varargout{1} = class_wrapper(35, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_string'); @@ -259,7 +259,7 @@ classdef Test < handle % RETURN_VECTOR1 usage: return_vector1(Vector value) : returns Vector % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1 - varargout{1} = class_wrapper(35, this, varargin{:}); + varargout{1} = class_wrapper(36, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_vector1'); @@ -269,19 +269,13 @@ classdef Test < handle % RETURN_VECTOR2 usage: return_vector2(Vector value) : returns Vector % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1 - varargout{1} = class_wrapper(36, this, varargin{:}); + varargout{1} = class_wrapper(37, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_vector2'); end function varargout = set_container(this, varargin) - % SET_CONTAINER usage: set_container(vector container) : returns void - % Doxygen can be found at https://gtsam.org/doxygen/ - if length(varargin) == 1 && isa(varargin{1},'std.vectorTest') - class_wrapper(37, this, varargin{:}); - return - end % SET_CONTAINER usage: set_container(vector container) : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'std.vectorTest') @@ -294,6 +288,12 @@ classdef Test < handle class_wrapper(39, this, varargin{:}); return end + % SET_CONTAINER usage: set_container(vector container) : returns void + % Doxygen can be found at https://gtsam.org/doxygen/ + if length(varargin) == 1 && isa(varargin{1},'std.vectorTest') + class_wrapper(40, this, varargin{:}); + return + end error('Arguments do not match any overload of function Test.set_container'); end diff --git a/wrap/tests/expected/matlab/class_wrapper.cpp b/wrap/tests/expected/matlab/class_wrapper.cpp index df6cb3307..bdb0d1de6 100644 --- a/wrap/tests/expected/matlab/class_wrapper.cpp +++ b/wrap/tests/expected/matlab/class_wrapper.cpp @@ -231,7 +231,14 @@ void FunDouble_multiTemplatedMethod_7(int nargout, mxArray *out[], int nargin, c out[0] = wrap_shared_ptr(boost::make_shared>(obj->multiTemplatedMethod(d,t,u)),"Fun", false); } -void FunDouble_templatedMethod_8(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void FunDouble_sets_8(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("sets",nargout,nargin-1,0); + auto obj = unwrap_shared_ptr>(in[0], "ptr_FunDouble"); + out[0] = wrap_shared_ptr(boost::make_shared::double>>(obj->sets()),"std.mapdoubledouble", false); +} + +void FunDouble_templatedMethod_9(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("templatedMethodString",nargout,nargin-1,2); auto obj = unwrap_shared_ptr>(in[0], "ptr_FunDouble"); @@ -240,20 +247,20 @@ void FunDouble_templatedMethod_8(int nargout, mxArray *out[], int nargin, const out[0] = wrap_shared_ptr(boost::make_shared>(obj->templatedMethod(d,t)),"Fun", false); } -void FunDouble_staticMethodWithThis_9(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void FunDouble_staticMethodWithThis_10(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("FunDouble.staticMethodWithThis",nargout,nargin,0); + checkArguments("Fun.staticMethodWithThis",nargout,nargin,0); out[0] = wrap_shared_ptr(boost::make_shared>(Fun::staticMethodWithThis()),"Fundouble", false); } -void FunDouble_templatedStaticMethodInt_10(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void FunDouble_templatedStaticMethodInt_11(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("FunDouble.templatedStaticMethodInt",nargout,nargin,1); + checkArguments("Fun.templatedStaticMethodInt",nargout,nargin,1); int m = unwrap< int >(in[0]); - out[0] = wrap< double >(Fun::templatedStaticMethodInt(m)); + out[0] = wrap< double >(Fun::templatedStaticMethod(m)); } -void Test_collectorInsertAndMakeBase_11(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_collectorInsertAndMakeBase_12(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -262,7 +269,7 @@ void Test_collectorInsertAndMakeBase_11(int nargout, mxArray *out[], int nargin, collector_Test.insert(self); } -void Test_constructor_12(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_constructor_13(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -273,7 +280,7 @@ void Test_constructor_12(int nargout, mxArray *out[], int nargin, const mxArray *reinterpret_cast (mxGetData(out[0])) = self; } -void Test_constructor_13(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_constructor_14(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -286,7 +293,7 @@ void Test_constructor_13(int nargout, mxArray *out[], int nargin, const mxArray *reinterpret_cast (mxGetData(out[0])) = self; } -void Test_deconstructor_14(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_deconstructor_15(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr Shared; checkArguments("delete_Test",nargout,nargin,1); @@ -299,7 +306,7 @@ void Test_deconstructor_14(int nargout, mxArray *out[], int nargin, const mxArra } } -void Test_arg_EigenConstRef_15(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_arg_EigenConstRef_16(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("arg_EigenConstRef",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -307,7 +314,7 @@ void Test_arg_EigenConstRef_15(int nargout, mxArray *out[], int nargin, const mx obj->arg_EigenConstRef(value); } -void Test_create_MixedPtrs_16(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_create_MixedPtrs_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("create_MixedPtrs",nargout,nargin-1,0); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -316,7 +323,7 @@ void Test_create_MixedPtrs_16(int nargout, mxArray *out[], int nargin, const mxA out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } -void Test_create_ptrs_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_create_ptrs_18(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("create_ptrs",nargout,nargin-1,0); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -325,28 +332,28 @@ void Test_create_ptrs_17(int nargout, mxArray *out[], int nargin, const mxArray out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } -void Test_get_container_18(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_get_container_19(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("get_container",nargout,nargin-1,0); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); out[0] = wrap_shared_ptr(boost::make_shared>(obj->get_container()),"std.vectorTest", false); } -void Test_lambda_19(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_lambda_20(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("lambda",nargout,nargin-1,0); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); obj->lambda(); } -void Test_print_20(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_print_21(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,0); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); obj->print(); } -void Test_return_Point2Ptr_21(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_Point2Ptr_22(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_Point2Ptr",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -357,7 +364,7 @@ void Test_return_Point2Ptr_21(int nargout, mxArray *out[], int nargin, const mxA } } -void Test_return_Test_22(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_Test_23(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_Test",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -365,7 +372,7 @@ void Test_return_Test_22(int nargout, mxArray *out[], int nargin, const mxArray out[0] = wrap_shared_ptr(boost::make_shared(obj->return_Test(value)),"Test", false); } -void Test_return_TestPtr_23(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_TestPtr_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_TestPtr",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -373,7 +380,7 @@ void Test_return_TestPtr_23(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap_shared_ptr(obj->return_TestPtr(value),"Test", false); } -void Test_return_bool_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_bool_25(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_bool",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -381,7 +388,7 @@ void Test_return_bool_24(int nargout, mxArray *out[], int nargin, const mxArray out[0] = wrap< bool >(obj->return_bool(value)); } -void Test_return_double_25(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_double_26(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_double",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -389,7 +396,7 @@ void Test_return_double_25(int nargout, mxArray *out[], int nargin, const mxArra out[0] = wrap< double >(obj->return_double(value)); } -void Test_return_field_26(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_field_27(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_field",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -397,7 +404,7 @@ void Test_return_field_26(int nargout, mxArray *out[], int nargin, const mxArray out[0] = wrap< bool >(obj->return_field(t)); } -void Test_return_int_27(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_int_28(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_int",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -405,7 +412,7 @@ void Test_return_int_27(int nargout, mxArray *out[], int nargin, const mxArray * out[0] = wrap< int >(obj->return_int(value)); } -void Test_return_matrix1_28(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_matrix1_29(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_matrix1",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -413,7 +420,7 @@ void Test_return_matrix1_28(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap< Matrix >(obj->return_matrix1(value)); } -void Test_return_matrix2_29(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_matrix2_30(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_matrix2",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -421,7 +428,7 @@ void Test_return_matrix2_29(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap< Matrix >(obj->return_matrix2(value)); } -void Test_return_pair_30(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_pair_31(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_pair",nargout,nargin-1,2); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -432,7 +439,7 @@ void Test_return_pair_30(int nargout, mxArray *out[], int nargin, const mxArray out[1] = wrap< Matrix >(pairResult.second); } -void Test_return_pair_31(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_pair_32(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_pair",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -442,7 +449,7 @@ void Test_return_pair_31(int nargout, mxArray *out[], int nargin, const mxArray out[1] = wrap< Matrix >(pairResult.second); } -void Test_return_ptrs_32(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_ptrs_33(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_ptrs",nargout,nargin-1,2); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -453,7 +460,7 @@ void Test_return_ptrs_32(int nargout, mxArray *out[], int nargin, const mxArray out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } -void Test_return_size_t_33(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_size_t_34(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_size_t",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -461,7 +468,7 @@ void Test_return_size_t_33(int nargout, mxArray *out[], int nargin, const mxArra out[0] = wrap< size_t >(obj->return_size_t(value)); } -void Test_return_string_34(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_string_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_string",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -469,7 +476,7 @@ void Test_return_string_34(int nargout, mxArray *out[], int nargin, const mxArra out[0] = wrap< string >(obj->return_string(value)); } -void Test_return_vector1_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_vector1_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_vector1",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -477,7 +484,7 @@ void Test_return_vector1_35(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap< Vector >(obj->return_vector1(value)); } -void Test_return_vector2_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_vector2_37(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_vector2",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -485,14 +492,6 @@ void Test_return_vector2_36(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap< Vector >(obj->return_vector2(value)); } -void Test_set_container_37(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - checkArguments("set_container",nargout,nargin-1,1); - auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); - boost::shared_ptr> container = unwrap_shared_ptr< std::vector >(in[1], "ptr_stdvectorTest"); - obj->set_container(*container); -} - void Test_set_container_38(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("set_container",nargout,nargin-1,1); @@ -509,7 +508,15 @@ void Test_set_container_39(int nargout, mxArray *out[], int nargin, const mxArra obj->set_container(*container); } -void PrimitiveRefDouble_collectorInsertAndMakeBase_40(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_set_container_40(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("set_container",nargout,nargin-1,1); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); + boost::shared_ptr> container = unwrap_shared_ptr< std::vector >(in[1], "ptr_stdvectorTest"); + obj->set_container(*container); +} + +void PrimitiveRefDouble_collectorInsertAndMakeBase_41(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -518,7 +525,7 @@ void PrimitiveRefDouble_collectorInsertAndMakeBase_40(int nargout, mxArray *out[ collector_PrimitiveRefDouble.insert(self); } -void PrimitiveRefDouble_constructor_41(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void PrimitiveRefDouble_constructor_42(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -529,7 +536,7 @@ void PrimitiveRefDouble_constructor_41(int nargout, mxArray *out[], int nargin, *reinterpret_cast (mxGetData(out[0])) = self; } -void PrimitiveRefDouble_deconstructor_42(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void PrimitiveRefDouble_deconstructor_43(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_PrimitiveRefDouble",nargout,nargin,1); @@ -542,14 +549,14 @@ void PrimitiveRefDouble_deconstructor_42(int nargout, mxArray *out[], int nargin } } -void PrimitiveRefDouble_Brutal_43(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void PrimitiveRefDouble_Brutal_44(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("PrimitiveRefDouble.Brutal",nargout,nargin,1); + checkArguments("PrimitiveRef.Brutal",nargout,nargin,1); double t = unwrap< double >(in[0]); out[0] = wrap_shared_ptr(boost::make_shared>(PrimitiveRef::Brutal(t)),"PrimitiveRefdouble", false); } -void MyVector3_collectorInsertAndMakeBase_44(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector3_collectorInsertAndMakeBase_45(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -558,7 +565,7 @@ void MyVector3_collectorInsertAndMakeBase_44(int nargout, mxArray *out[], int na collector_MyVector3.insert(self); } -void MyVector3_constructor_45(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector3_constructor_46(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -569,7 +576,7 @@ void MyVector3_constructor_45(int nargout, mxArray *out[], int nargin, const mxA *reinterpret_cast (mxGetData(out[0])) = self; } -void MyVector3_deconstructor_46(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector3_deconstructor_47(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MyVector3",nargout,nargin,1); @@ -582,7 +589,7 @@ void MyVector3_deconstructor_46(int nargout, mxArray *out[], int nargin, const m } } -void MyVector12_collectorInsertAndMakeBase_47(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector12_collectorInsertAndMakeBase_48(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -591,7 +598,7 @@ void MyVector12_collectorInsertAndMakeBase_47(int nargout, mxArray *out[], int n collector_MyVector12.insert(self); } -void MyVector12_constructor_48(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector12_constructor_49(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -602,7 +609,7 @@ void MyVector12_constructor_48(int nargout, mxArray *out[], int nargin, const mx *reinterpret_cast (mxGetData(out[0])) = self; } -void MyVector12_deconstructor_49(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector12_deconstructor_50(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MyVector12",nargout,nargin,1); @@ -615,7 +622,7 @@ void MyVector12_deconstructor_49(int nargout, mxArray *out[], int nargin, const } } -void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_50(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_51(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -624,7 +631,7 @@ void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_50(int nargout, mxArr collector_MultipleTemplatesIntDouble.insert(self); } -void MultipleTemplatesIntDouble_deconstructor_51(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MultipleTemplatesIntDouble_deconstructor_52(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MultipleTemplatesIntDouble",nargout,nargin,1); @@ -637,7 +644,7 @@ void MultipleTemplatesIntDouble_deconstructor_51(int nargout, mxArray *out[], in } } -void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_52(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_53(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -646,7 +653,7 @@ void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_52(int nargout, mxArra collector_MultipleTemplatesIntFloat.insert(self); } -void MultipleTemplatesIntFloat_deconstructor_53(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MultipleTemplatesIntFloat_deconstructor_54(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MultipleTemplatesIntFloat",nargout,nargin,1); @@ -659,7 +666,7 @@ void MultipleTemplatesIntFloat_deconstructor_53(int nargout, mxArray *out[], int } } -void ForwardKinematics_collectorInsertAndMakeBase_54(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void ForwardKinematics_collectorInsertAndMakeBase_55(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -668,7 +675,7 @@ void ForwardKinematics_collectorInsertAndMakeBase_54(int nargout, mxArray *out[] collector_ForwardKinematics.insert(self); } -void ForwardKinematics_constructor_55(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void ForwardKinematics_constructor_56(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -684,7 +691,7 @@ void ForwardKinematics_constructor_55(int nargout, mxArray *out[], int nargin, c *reinterpret_cast (mxGetData(out[0])) = self; } -void ForwardKinematics_deconstructor_56(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void ForwardKinematics_deconstructor_57(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr Shared; checkArguments("delete_ForwardKinematics",nargout,nargin,1); @@ -697,7 +704,7 @@ void ForwardKinematics_deconstructor_56(int nargout, mxArray *out[], int nargin, } } -void TemplatedConstructor_collectorInsertAndMakeBase_57(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void TemplatedConstructor_collectorInsertAndMakeBase_58(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -706,7 +713,7 @@ void TemplatedConstructor_collectorInsertAndMakeBase_57(int nargout, mxArray *ou collector_TemplatedConstructor.insert(self); } -void TemplatedConstructor_constructor_58(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void TemplatedConstructor_constructor_59(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -717,7 +724,7 @@ void TemplatedConstructor_constructor_58(int nargout, mxArray *out[], int nargin *reinterpret_cast (mxGetData(out[0])) = self; } -void TemplatedConstructor_constructor_59(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void TemplatedConstructor_constructor_60(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -729,7 +736,7 @@ void TemplatedConstructor_constructor_59(int nargout, mxArray *out[], int nargin *reinterpret_cast (mxGetData(out[0])) = self; } -void TemplatedConstructor_constructor_60(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void TemplatedConstructor_constructor_61(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -741,7 +748,7 @@ void TemplatedConstructor_constructor_60(int nargout, mxArray *out[], int nargin *reinterpret_cast (mxGetData(out[0])) = self; } -void TemplatedConstructor_constructor_61(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void TemplatedConstructor_constructor_62(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -753,7 +760,7 @@ void TemplatedConstructor_constructor_61(int nargout, mxArray *out[], int nargin *reinterpret_cast (mxGetData(out[0])) = self; } -void TemplatedConstructor_deconstructor_62(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void TemplatedConstructor_deconstructor_63(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr Shared; checkArguments("delete_TemplatedConstructor",nargout,nargin,1); @@ -766,7 +773,7 @@ void TemplatedConstructor_deconstructor_62(int nargout, mxArray *out[], int narg } } -void MyFactorPosePoint2_collectorInsertAndMakeBase_63(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_collectorInsertAndMakeBase_64(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -775,7 +782,7 @@ void MyFactorPosePoint2_collectorInsertAndMakeBase_63(int nargout, mxArray *out[ collector_MyFactorPosePoint2.insert(self); } -void MyFactorPosePoint2_constructor_64(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_constructor_65(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -790,7 +797,7 @@ void MyFactorPosePoint2_constructor_64(int nargout, mxArray *out[], int nargin, *reinterpret_cast (mxGetData(out[0])) = self; } -void MyFactorPosePoint2_deconstructor_65(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_deconstructor_66(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1); @@ -803,7 +810,7 @@ void MyFactorPosePoint2_deconstructor_65(int nargout, mxArray *out[], int nargin } } -void MyFactorPosePoint2_print_66(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_print_67(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,2); auto obj = unwrap_shared_ptr>(in[0], "ptr_MyFactorPosePoint2"); @@ -849,94 +856,94 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) FunDouble_multiTemplatedMethod_7(nargout, out, nargin-1, in+1); break; case 8: - FunDouble_templatedMethod_8(nargout, out, nargin-1, in+1); + FunDouble_sets_8(nargout, out, nargin-1, in+1); break; case 9: - FunDouble_staticMethodWithThis_9(nargout, out, nargin-1, in+1); + FunDouble_templatedMethod_9(nargout, out, nargin-1, in+1); break; case 10: - FunDouble_templatedStaticMethodInt_10(nargout, out, nargin-1, in+1); + FunDouble_staticMethodWithThis_10(nargout, out, nargin-1, in+1); break; case 11: - Test_collectorInsertAndMakeBase_11(nargout, out, nargin-1, in+1); + FunDouble_templatedStaticMethodInt_11(nargout, out, nargin-1, in+1); break; case 12: - Test_constructor_12(nargout, out, nargin-1, in+1); + Test_collectorInsertAndMakeBase_12(nargout, out, nargin-1, in+1); break; case 13: Test_constructor_13(nargout, out, nargin-1, in+1); break; case 14: - Test_deconstructor_14(nargout, out, nargin-1, in+1); + Test_constructor_14(nargout, out, nargin-1, in+1); break; case 15: - Test_arg_EigenConstRef_15(nargout, out, nargin-1, in+1); + Test_deconstructor_15(nargout, out, nargin-1, in+1); break; case 16: - Test_create_MixedPtrs_16(nargout, out, nargin-1, in+1); + Test_arg_EigenConstRef_16(nargout, out, nargin-1, in+1); break; case 17: - Test_create_ptrs_17(nargout, out, nargin-1, in+1); + Test_create_MixedPtrs_17(nargout, out, nargin-1, in+1); break; case 18: - Test_get_container_18(nargout, out, nargin-1, in+1); + Test_create_ptrs_18(nargout, out, nargin-1, in+1); break; case 19: - Test_lambda_19(nargout, out, nargin-1, in+1); + Test_get_container_19(nargout, out, nargin-1, in+1); break; case 20: - Test_print_20(nargout, out, nargin-1, in+1); + Test_lambda_20(nargout, out, nargin-1, in+1); break; case 21: - Test_return_Point2Ptr_21(nargout, out, nargin-1, in+1); + Test_print_21(nargout, out, nargin-1, in+1); break; case 22: - Test_return_Test_22(nargout, out, nargin-1, in+1); + Test_return_Point2Ptr_22(nargout, out, nargin-1, in+1); break; case 23: - Test_return_TestPtr_23(nargout, out, nargin-1, in+1); + Test_return_Test_23(nargout, out, nargin-1, in+1); break; case 24: - Test_return_bool_24(nargout, out, nargin-1, in+1); + Test_return_TestPtr_24(nargout, out, nargin-1, in+1); break; case 25: - Test_return_double_25(nargout, out, nargin-1, in+1); + Test_return_bool_25(nargout, out, nargin-1, in+1); break; case 26: - Test_return_field_26(nargout, out, nargin-1, in+1); + Test_return_double_26(nargout, out, nargin-1, in+1); break; case 27: - Test_return_int_27(nargout, out, nargin-1, in+1); + Test_return_field_27(nargout, out, nargin-1, in+1); break; case 28: - Test_return_matrix1_28(nargout, out, nargin-1, in+1); + Test_return_int_28(nargout, out, nargin-1, in+1); break; case 29: - Test_return_matrix2_29(nargout, out, nargin-1, in+1); + Test_return_matrix1_29(nargout, out, nargin-1, in+1); break; case 30: - Test_return_pair_30(nargout, out, nargin-1, in+1); + Test_return_matrix2_30(nargout, out, nargin-1, in+1); break; case 31: Test_return_pair_31(nargout, out, nargin-1, in+1); break; case 32: - Test_return_ptrs_32(nargout, out, nargin-1, in+1); + Test_return_pair_32(nargout, out, nargin-1, in+1); break; case 33: - Test_return_size_t_33(nargout, out, nargin-1, in+1); + Test_return_ptrs_33(nargout, out, nargin-1, in+1); break; case 34: - Test_return_string_34(nargout, out, nargin-1, in+1); + Test_return_size_t_34(nargout, out, nargin-1, in+1); break; case 35: - Test_return_vector1_35(nargout, out, nargin-1, in+1); + Test_return_string_35(nargout, out, nargin-1, in+1); break; case 36: - Test_return_vector2_36(nargout, out, nargin-1, in+1); + Test_return_vector1_36(nargout, out, nargin-1, in+1); break; case 37: - Test_set_container_37(nargout, out, nargin-1, in+1); + Test_return_vector2_37(nargout, out, nargin-1, in+1); break; case 38: Test_set_container_38(nargout, out, nargin-1, in+1); @@ -945,61 +952,61 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) Test_set_container_39(nargout, out, nargin-1, in+1); break; case 40: - PrimitiveRefDouble_collectorInsertAndMakeBase_40(nargout, out, nargin-1, in+1); + Test_set_container_40(nargout, out, nargin-1, in+1); break; case 41: - PrimitiveRefDouble_constructor_41(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_collectorInsertAndMakeBase_41(nargout, out, nargin-1, in+1); break; case 42: - PrimitiveRefDouble_deconstructor_42(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_constructor_42(nargout, out, nargin-1, in+1); break; case 43: - PrimitiveRefDouble_Brutal_43(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_deconstructor_43(nargout, out, nargin-1, in+1); break; case 44: - MyVector3_collectorInsertAndMakeBase_44(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_Brutal_44(nargout, out, nargin-1, in+1); break; case 45: - MyVector3_constructor_45(nargout, out, nargin-1, in+1); + MyVector3_collectorInsertAndMakeBase_45(nargout, out, nargin-1, in+1); break; case 46: - MyVector3_deconstructor_46(nargout, out, nargin-1, in+1); + MyVector3_constructor_46(nargout, out, nargin-1, in+1); break; case 47: - MyVector12_collectorInsertAndMakeBase_47(nargout, out, nargin-1, in+1); + MyVector3_deconstructor_47(nargout, out, nargin-1, in+1); break; case 48: - MyVector12_constructor_48(nargout, out, nargin-1, in+1); + MyVector12_collectorInsertAndMakeBase_48(nargout, out, nargin-1, in+1); break; case 49: - MyVector12_deconstructor_49(nargout, out, nargin-1, in+1); + MyVector12_constructor_49(nargout, out, nargin-1, in+1); break; case 50: - MultipleTemplatesIntDouble_collectorInsertAndMakeBase_50(nargout, out, nargin-1, in+1); + MyVector12_deconstructor_50(nargout, out, nargin-1, in+1); break; case 51: - MultipleTemplatesIntDouble_deconstructor_51(nargout, out, nargin-1, in+1); + MultipleTemplatesIntDouble_collectorInsertAndMakeBase_51(nargout, out, nargin-1, in+1); break; case 52: - MultipleTemplatesIntFloat_collectorInsertAndMakeBase_52(nargout, out, nargin-1, in+1); + MultipleTemplatesIntDouble_deconstructor_52(nargout, out, nargin-1, in+1); break; case 53: - MultipleTemplatesIntFloat_deconstructor_53(nargout, out, nargin-1, in+1); + MultipleTemplatesIntFloat_collectorInsertAndMakeBase_53(nargout, out, nargin-1, in+1); break; case 54: - ForwardKinematics_collectorInsertAndMakeBase_54(nargout, out, nargin-1, in+1); + MultipleTemplatesIntFloat_deconstructor_54(nargout, out, nargin-1, in+1); break; case 55: - ForwardKinematics_constructor_55(nargout, out, nargin-1, in+1); + ForwardKinematics_collectorInsertAndMakeBase_55(nargout, out, nargin-1, in+1); break; case 56: - ForwardKinematics_deconstructor_56(nargout, out, nargin-1, in+1); + ForwardKinematics_constructor_56(nargout, out, nargin-1, in+1); break; case 57: - TemplatedConstructor_collectorInsertAndMakeBase_57(nargout, out, nargin-1, in+1); + ForwardKinematics_deconstructor_57(nargout, out, nargin-1, in+1); break; case 58: - TemplatedConstructor_constructor_58(nargout, out, nargin-1, in+1); + TemplatedConstructor_collectorInsertAndMakeBase_58(nargout, out, nargin-1, in+1); break; case 59: TemplatedConstructor_constructor_59(nargout, out, nargin-1, in+1); @@ -1011,19 +1018,22 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) TemplatedConstructor_constructor_61(nargout, out, nargin-1, in+1); break; case 62: - TemplatedConstructor_deconstructor_62(nargout, out, nargin-1, in+1); + TemplatedConstructor_constructor_62(nargout, out, nargin-1, in+1); break; case 63: - MyFactorPosePoint2_collectorInsertAndMakeBase_63(nargout, out, nargin-1, in+1); + TemplatedConstructor_deconstructor_63(nargout, out, nargin-1, in+1); break; case 64: - MyFactorPosePoint2_constructor_64(nargout, out, nargin-1, in+1); + MyFactorPosePoint2_collectorInsertAndMakeBase_64(nargout, out, nargin-1, in+1); break; case 65: - MyFactorPosePoint2_deconstructor_65(nargout, out, nargin-1, in+1); + MyFactorPosePoint2_constructor_65(nargout, out, nargin-1, in+1); break; case 66: - MyFactorPosePoint2_print_66(nargout, out, nargin-1, in+1); + MyFactorPosePoint2_deconstructor_66(nargout, out, nargin-1, in+1); + break; + case 67: + MyFactorPosePoint2_print_67(nargout, out, nargin-1, in+1); break; } } catch(const std::exception& e) { diff --git a/wrap/tests/expected/matlab/geometry_wrapper.cpp b/wrap/tests/expected/matlab/geometry_wrapper.cpp index 81631390c..544c8b256 100644 --- a/wrap/tests/expected/matlab/geometry_wrapper.cpp +++ b/wrap/tests/expected/matlab/geometry_wrapper.cpp @@ -286,14 +286,14 @@ void gtsamPoint3_string_serialize_22(int nargout, mxArray *out[], int nargin, co } void gtsamPoint3_StaticFunctionRet_23(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("gtsamPoint3.StaticFunctionRet",nargout,nargin,1); + checkArguments("gtsam::Point3.StaticFunctionRet",nargout,nargin,1); double z = unwrap< double >(in[0]); out[0] = wrap< Point3 >(gtsam::Point3::StaticFunctionRet(z)); } void gtsamPoint3_staticFunction_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("gtsamPoint3.staticFunction",nargout,nargin,0); + checkArguments("gtsam::Point3.staticFunction",nargout,nargin,0); out[0] = wrap< double >(gtsam::Point3::staticFunction()); } diff --git a/wrap/tests/expected/matlab/inheritance_wrapper.cpp b/wrap/tests/expected/matlab/inheritance_wrapper.cpp index 8e61ac8c6..f2eef7f85 100644 --- a/wrap/tests/expected/matlab/inheritance_wrapper.cpp +++ b/wrap/tests/expected/matlab/inheritance_wrapper.cpp @@ -289,7 +289,7 @@ void MyTemplatePoint2_templatedMethod_17(int nargout, mxArray *out[], int nargin void MyTemplatePoint2_Level_18(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("MyTemplatePoint2.Level",nargout,nargin,1); + checkArguments("MyTemplate.Level",nargout,nargin,1); Point2 K = unwrap< Point2 >(in[0]); out[0] = wrap_shared_ptr(boost::make_shared>(MyTemplate::Level(K)),"MyTemplatePoint2", false); } @@ -457,7 +457,7 @@ void MyTemplateMatrix_templatedMethod_33(int nargout, mxArray *out[], int nargin void MyTemplateMatrix_Level_34(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("MyTemplateMatrix.Level",nargout,nargin,1); + checkArguments("MyTemplate.Level",nargout,nargin,1); Matrix K = unwrap< Matrix >(in[0]); out[0] = wrap_shared_ptr(boost::make_shared>(MyTemplate::Level(K)),"MyTemplateMatrix", false); } diff --git a/wrap/tests/expected/matlab/namespaces_wrapper.cpp b/wrap/tests/expected/matlab/namespaces_wrapper.cpp index 604ede5da..903815e8e 100644 --- a/wrap/tests/expected/matlab/namespaces_wrapper.cpp +++ b/wrap/tests/expected/matlab/namespaces_wrapper.cpp @@ -248,7 +248,7 @@ void ns2ClassA_nsReturn_12(int nargout, mxArray *out[], int nargin, const mxArra void ns2ClassA_afunction_13(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("ns2ClassA.afunction",nargout,nargin,0); + checkArguments("ns2::ClassA.afunction",nargout,nargin,0); out[0] = wrap< double >(ns2::ClassA::afunction()); } diff --git a/wrap/tests/expected/matlab/setPose.m b/wrap/tests/expected/matlab/setPose.m new file mode 100644 index 000000000..ed573b872 --- /dev/null +++ b/wrap/tests/expected/matlab/setPose.m @@ -0,0 +1,6 @@ +function varargout = setPose(varargin) + if length(varargin) == 1 && isa(varargin{1},'gtsam.Pose3') + functions_wrapper(13, varargin{:}); + else + error('Arguments do not match any overload of function setPose'); + end diff --git a/wrap/tests/expected/python/class_pybind.cpp b/wrap/tests/expected/python/class_pybind.cpp index a54d9135b..1801f2e49 100644 --- a/wrap/tests/expected/python/class_pybind.cpp +++ b/wrap/tests/expected/python/class_pybind.cpp @@ -31,6 +31,7 @@ PYBIND11_MODULE(class_py, m_) { py::class_, std::shared_ptr>>(m_, "FunDouble") .def("templatedMethodString",[](Fun* self, double d, string t){return self->templatedMethod(d, t);}, py::arg("d"), py::arg("t")) .def("multiTemplatedMethodStringSize_t",[](Fun* self, double d, string t, size_t u){return self->multiTemplatedMethod(d, t, u);}, py::arg("d"), py::arg("t"), py::arg("u")) + .def("sets",[](Fun* self){return self->sets();}) .def_static("staticMethodWithThis",[](){return Fun::staticMethodWithThis();}) .def_static("templatedStaticMethodInt",[](const int& m){return Fun::templatedStaticMethod(m);}, py::arg("m")); diff --git a/wrap/tests/fixtures/class.i b/wrap/tests/fixtures/class.i index 40a565506..1ce617776 100644 --- a/wrap/tests/fixtures/class.i +++ b/wrap/tests/fixtures/class.i @@ -18,6 +18,8 @@ class Fun { template This multiTemplatedMethod(double d, T t, U u); + + std::map sets(); }; diff --git a/wrap/tests/test_matlab_wrapper.py b/wrap/tests/test_matlab_wrapper.py index 34940d62e..43fedf7aa 100644 --- a/wrap/tests/test_matlab_wrapper.py +++ b/wrap/tests/test_matlab_wrapper.py @@ -92,10 +92,19 @@ class TestWrap(unittest.TestCase): wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) files = [ - 'functions_wrapper.cpp', 'aGlobalFunction.m', 'load2D.m', + 'functions_wrapper.cpp', + 'aGlobalFunction.m', + 'load2D.m', 'MultiTemplatedFunctionDoubleSize_tDouble.m', 'MultiTemplatedFunctionStringSize_tDouble.m', - 'overloadedGlobalFunction.m', 'TemplatedFunctionRot3.m' + 'overloadedGlobalFunction.m', + 'TemplatedFunctionRot3.m', + 'DefaultFuncInt.m', + 'DefaultFuncObj.m', + 'DefaultFuncString.m', + 'DefaultFuncVector.m', + 'DefaultFuncZero.m', + 'setPose.m', ] for file in files: @@ -115,10 +124,17 @@ class TestWrap(unittest.TestCase): wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) files = [ - 'class_wrapper.cpp', 'FunDouble.m', 'FunRange.m', - 'MultipleTemplatesIntDouble.m', 'MultipleTemplatesIntFloat.m', - 'MyFactorPosePoint2.m', 'MyVector3.m', 'MyVector12.m', - 'PrimitiveRefDouble.m', 'Test.m' + 'class_wrapper.cpp', + 'FunDouble.m', + 'FunRange.m', + 'MultipleTemplatesIntDouble.m', + 'MultipleTemplatesIntFloat.m', + 'MyFactorPosePoint2.m', + 'MyVector3.m', + 'MyVector12.m', + 'PrimitiveRefDouble.m', + 'Test.m', + 'ForwardKinematics.m', ] for file in files: @@ -137,7 +153,10 @@ class TestWrap(unittest.TestCase): wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) - files = ['template_wrapper.cpp'] + files = [ + 'template_wrapper.cpp', 'ScopedTemplateResult.m', + 'TemplatedConstructor.m' + ] for file in files: actual = osp.join(self.MATLAB_ACTUAL_DIR, file) @@ -155,8 +174,11 @@ class TestWrap(unittest.TestCase): wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) files = [ - 'inheritance_wrapper.cpp', 'MyBase.m', 'MyTemplateMatrix.m', - 'MyTemplatePoint2.m' + 'inheritance_wrapper.cpp', + 'MyBase.m', + 'MyTemplateMatrix.m', + 'MyTemplatePoint2.m', + 'ForwardKinematicsFactor.m', ] for file in files: @@ -178,10 +200,17 @@ class TestWrap(unittest.TestCase): wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) files = [ - 'namespaces_wrapper.cpp', '+ns1/aGlobalFunction.m', - '+ns1/ClassA.m', '+ns1/ClassB.m', '+ns2/+ns3/ClassB.m', - '+ns2/aGlobalFunction.m', '+ns2/ClassA.m', '+ns2/ClassC.m', - '+ns2/overloadedGlobalFunction.m', 'ClassD.m' + 'namespaces_wrapper.cpp', + '+ns1/aGlobalFunction.m', + '+ns1/ClassA.m', + '+ns1/ClassB.m', + '+ns2/+ns3/ClassB.m', + '+ns2/aGlobalFunction.m', + '+ns2/ClassA.m', + '+ns2/ClassC.m', + '+ns2/overloadedGlobalFunction.m', + 'ClassD.m', + '+gtsam/Values.m', ] for file in files: @@ -203,8 +232,10 @@ class TestWrap(unittest.TestCase): files = [ 'special_cases_wrapper.cpp', - '+gtsam/PinholeCameraCal3Bundler.m', + '+gtsam/GeneralSFMFactorCal3Bundler.m', '+gtsam/NonlinearFactorGraph.m', + '+gtsam/PinholeCameraCal3Bundler.m', + '+gtsam/SfmTrack.m', ] for file in files: