diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 9ca3a8fe5..5483c32cf 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -57,7 +57,7 @@ jobs: # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository # This key is not in the keystore by default for Ubuntu so we need to add it. LLVM_KEY=15CF4D18AF4F7421 - gpg --keyserver ipv4.pool.sks-keyservers.net --recv-key $LLVM_KEY || gpg --keyserver ha.pool.sks-keyservers.net --recv-key $LLVM_KEY + gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY gpg -a --export $LLVM_KEY | sudo apt-key add - sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main" fi diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index df11c5e95..1f87b5119 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -77,7 +77,7 @@ jobs: # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository # This key is not in the keystore by default for Ubuntu so we need to add it. LLVM_KEY=15CF4D18AF4F7421 - gpg --keyserver ipv4.pool.sks-keyservers.net --recv-key $LLVM_KEY || gpg --keyserver ha.pool.sks-keyservers.net --recv-key $LLVM_KEY + gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY gpg -a --export $LLVM_KEY | sudo apt-key add - sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main" fi diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 3ac88bdc8..6427e13bc 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -64,7 +64,7 @@ jobs: run: | # LLVM 9 is not in Bionic's repositories so we add the official LLVM repository. if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then - gpg --keyserver pool.sks-keyservers.net --recv-key 15CF4D18AF4F7421 + gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add - sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main" fi diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index aa0bde8f6..97f4c84dc 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -15,8 +15,8 @@ * @author Frank Dellaert */ -#include #include +#include #include @@ -26,22 +26,16 @@ using namespace gtsam; /* ************************************************************************* */ -void createExampleBALFile(const string& filename, const vector& P, - const Pose3& pose1, const Pose3& pose2, const Cal3Bundler& K = - Cal3Bundler()) { - +void createExampleBALFile(const string& filename, const vector& points, + const Pose3& pose1, const Pose3& pose2, + const Cal3Bundler& K = Cal3Bundler()) { // Class that will gather all data SfmData data; - - // Create two cameras - Rot3 aRb = Rot3::Yaw(M_PI_2); - Point3 aTb(0.1, 0, 0); - Pose3 identity, aPb(aRb, aTb); + // Create two cameras and add them to data data.cameras.push_back(SfmCamera(pose1, K)); data.cameras.push_back(SfmCamera(pose2, K)); - for(const Point3& p: P) { - + for (const Point3& p : points) { // Create the track SfmTrack track; track.p = p; @@ -51,7 +45,7 @@ void createExampleBALFile(const string& filename, const vector& P, // Project points in both cameras for (size_t i = 0; i < 2; i++) - track.measurements.push_back(make_pair(i, data.cameras[i].project(p))); + track.measurements.push_back(make_pair(i, data.cameras[i].project(p))); // Add track to data data.tracks.push_back(track); @@ -63,49 +57,66 @@ void createExampleBALFile(const string& filename, const vector& P, /* ************************************************************************* */ void create5PointExample1() { - // Create two cameras poses Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(0.1, 0, 0); Pose3 pose1, pose2(aRb, aTb); // Create test data, we need at least 5 points - vector P; - P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), // - Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5); + vector points = { + {0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, {0, 0.5, 0.5}, {0, -0.5, 0.5}}; // Assumes example is run in ${GTSAM_TOP}/build/examples - const string filename = "../../examples/data/5pointExample1.txt"; - createExampleBALFile(filename, P, pose1, pose2); + const string filename = "../../examples/Data/5pointExample1.txt"; + createExampleBALFile(filename, points, pose1, pose2); } /* ************************************************************************* */ void create5PointExample2() { - // Create two cameras poses Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(10, 0, 0); Pose3 pose1, pose2(aRb, aTb); // Create test data, we need at least 5 points - vector P; - P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), // - Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), Point3(20, -50, 80); + vector points = {{0, 0, 100}, {-10, 0, 100}, {10, 0, 100}, // + {0, 50, 50}, {0, -50, 50}, {-20, 0, 80}, // + {20, -50, 80}}; // Assumes example is run in ${GTSAM_TOP}/build/examples - const string filename = "../../examples/data/5pointExample2.txt"; + const string filename = "../../examples/Data/5pointExample2.txt"; Cal3Bundler K(500, 0, 0); - createExampleBALFile(filename, P, pose1, pose2,K); + createExampleBALFile(filename, points, pose1, pose2, K); } /* ************************************************************************* */ +void create18PointExample1() { + // Create two cameras poses + Rot3 aRb = Rot3::Yaw(M_PI_2); + Point3 aTb(0.1, 0, 0); + Pose3 pose1, pose2(aRb, aTb); + + // Create test data, we need 15 points + vector points = { + {0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, // + {0, 0.5, 0.5}, {0, -0.5, 0.5}, {-1, -0.5, 2}, // + {-1, 0.5, 2}, {0.25, -0.5, 1.5}, {0.25, 0.5, 1.5}, // + {-0.1, -0.5, 0.5}, {0.1, -0.5, 1}, {0.1, 0.5, 1}, // + {-0.1, 0, 0.5}, {-0.1, 0.5, 0.5}, {0, 0, 0.5}, // + {0.1, -0.5, 0.5}, {0.1, 0, 0.5}, {0.1, 0.5, 0.5}}; + + // Assumes example is run in ${GTSAM_TOP}/build/examples + const string filename = "../../examples/Data/18pointExample1.txt"; + createExampleBALFile(filename, points, pose1, pose2); +} + int main(int argc, char* argv[]) { create5PointExample1(); create5PointExample2(); + create18PointExample1(); return 0; } /* ************************************************************************* */ - diff --git a/examples/Data/18pointExample1.txt b/examples/Data/18pointExample1.txt new file mode 100644 index 000000000..484a7944b --- /dev/null +++ b/examples/Data/18pointExample1.txt @@ -0,0 +1,131 @@ +2 18 36 + +0 0 0 -0 +1 0 -6.123233995736766344e-18 -0.10000000000000000555 +0 1 -0.10000000000000000555 -0 +1 1 -1.2246467991473532688e-17 -0.2000000000000000111 +0 2 0.10000000000000000555 -0 +1 2 0 -0 +0 3 0 -1 +1 3 1 -0.20000000000000006661 +0 4 0 1 +1 4 -1 -0.19999999999999995559 +0 5 -0.5 0.25 +1 5 -0.25000000000000005551 -0.55000000000000004441 +0 6 -0.5 -0.25 +1 6 0.24999999999999997224 -0.55000000000000004441 +0 7 0.16666666666666665741 0.33333333333333331483 +1 7 -0.33333333333333331483 0.10000000000000000555 +0 8 0.16666666666666665741 -0.33333333333333331483 +1 8 0.33333333333333331483 0.099999999999999977796 +0 9 -0.2000000000000000111 1 +1 9 -1 -0.39999999999999996669 +0 10 0.10000000000000000555 0.5 +1 10 -0.5 3.0616169978683830179e-17 +0 11 0.10000000000000000555 -0.5 +1 11 0.5 -3.0616169978683830179e-17 +0 12 -0.2000000000000000111 -0 +1 12 -2.4492935982947065376e-17 -0.4000000000000000222 +0 13 -0.2000000000000000111 -1 +1 13 1 -0.40000000000000007772 +0 14 0 -0 +1 14 -1.2246467991473532688e-17 -0.2000000000000000111 +0 15 0.2000000000000000111 1 +1 15 -1 6.1232339957367660359e-17 +0 16 0.2000000000000000111 -0 +1 16 0 -0 +0 17 0.2000000000000000111 -1 +1 17 1 -6.1232339957367660359e-17 + +3.141592653589793116 + 0 + 0 +-0 +0 +0 +1 +0 +0 + +2.2214414690791830509 +2.2214414690791826068 + 0 +-6.123233995736766344e-18 +-0.10000000000000000555 +0 +1 +0 +0 + +0 +0 +1 + +-0.10000000000000000555 +0 +1 + +0.10000000000000000555 +0 +1 + +0 +0.5 +0.5 + +0 +-0.5 +0.5 + +-1 +-0.5 +2 + +-1 +0.5 +2 + +0.25 +-0.5 +1.5 + +0.25 +0.5 +1.5 + +-0.10000000000000000555 +-0.5 +0.5 + +0.10000000000000000555 +-0.5 +1 + +0.10000000000000000555 +0.5 +1 + +-0.10000000000000000555 +0 +0.5 + +-0.10000000000000000555 +0.5 +0.5 + +0 +0 +0.5 + +0.10000000000000000555 +-0.5 +0.5 + +0.10000000000000000555 +0 +0.5 + +0.10000000000000000555 +0.5 +0.5 + diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 7a88f72eb..30cec3b9a 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -158,9 +158,8 @@ void DepthFirstForestParallel(FOREST& forest, DATA& rootData, // Typedefs typedef typename FOREST::Node Node; - tbb::task::spawn_root_and_wait( - internal::CreateRootTask(forest.roots(), rootData, visitorPre, - visitorPost, problemSizeThreshold)); + internal::CreateRootTask(forest.roots(), rootData, visitorPre, + visitorPost, problemSizeThreshold); #else DepthFirstForest(forest, rootData, visitorPre, visitorPost); #endif diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index 87d5b0d4c..dc1b45906 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -22,7 +22,7 @@ #include #ifdef GTSAM_USE_TBB -#include // tbb::task, tbb::task_list +#include // tbb::task_group #include // tbb::scalable_allocator namespace gtsam { @@ -34,7 +34,7 @@ namespace gtsam { /* ************************************************************************* */ template - class PreOrderTask : public tbb::task + class PreOrderTask { public: const boost::shared_ptr& treeNode; @@ -42,28 +42,30 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; + tbb::task_group& tg; bool makeNewTasks; - bool isPostOrderPhase; + // Keep track of order phase across multiple calls to the same functor + mutable bool isPostOrderPhase; PreOrderTask(const boost::shared_ptr& treeNode, const boost::shared_ptr& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold, - bool makeNewTasks = true) + tbb::task_group& tg, bool makeNewTasks = true) : treeNode(treeNode), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), problemSizeThreshold(problemSizeThreshold), + tg(tg), makeNewTasks(makeNewTasks), isPostOrderPhase(false) {} - tbb::task* execute() override + void operator()() const { if(isPostOrderPhase) { // Run the post-order visitor since this task was recycled to run the post-order visitor (void) visitorPost(treeNode, *myData); - return nullptr; } else { @@ -71,14 +73,10 @@ namespace gtsam { { if(!treeNode->children.empty()) { - // Allocate post-order task as a continuation - isPostOrderPhase = true; - recycle_as_continuation(); - bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold); - tbb::task* firstChild = 0; - tbb::task_list childTasks; + // If we have child tasks, start subtasks and wait for them to complete + tbb::task_group ctg; for(const boost::shared_ptr& child: treeNode->children) { // Process child in a subtask. Important: Run visitorPre before calling @@ -86,37 +84,30 @@ namespace gtsam { // allocated an extra child, this causes a TBB error. boost::shared_ptr childData = boost::allocate_shared( tbb::scalable_allocator(), visitorPre(child, *myData)); - tbb::task* childTask = - new (allocate_child()) PreOrderTask(child, childData, visitorPre, visitorPost, - problemSizeThreshold, overThreshold); - if (firstChild) - childTasks.push_back(*childTask); - else - firstChild = childTask; + ctg.run(PreOrderTask(child, childData, visitorPre, visitorPost, + problemSizeThreshold, ctg, overThreshold)); } + ctg.wait(); - // If we have child tasks, start subtasks and wait for them to complete - set_ref_count((int)treeNode->children.size()); - spawn(childTasks); - return firstChild; + // Allocate post-order task as a continuation + isPostOrderPhase = true; + tg.run(*this); } else { // Run the post-order visitor in this task if we have no children (void) visitorPost(treeNode, *myData); - return nullptr; } } else { // Process this node and its children in this task processNodeRecursively(treeNode, *myData); - return nullptr; } } } - void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) + void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) const { for(const boost::shared_ptr& child: node->children) { @@ -131,7 +122,7 @@ namespace gtsam { /* ************************************************************************* */ template - class RootTask : public tbb::task + class RootTask { public: const ROOTS& roots; @@ -139,38 +130,31 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; + tbb::task_group& tg; RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, - int problemSizeThreshold) : + int problemSizeThreshold, tbb::task_group& tg) : roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), - problemSizeThreshold(problemSizeThreshold) {} + problemSizeThreshold(problemSizeThreshold), tg(tg) {} - tbb::task* execute() override + void operator()() const { typedef PreOrderTask PreOrderTask; // Create data and tasks for our children - tbb::task_list tasks; for(const boost::shared_ptr& root: roots) { boost::shared_ptr rootData = boost::allocate_shared(tbb::scalable_allocator(), visitorPre(root, myData)); - tasks.push_back(*new(allocate_child()) - PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold)); + tg.run(PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); } - // Set TBB ref count - set_ref_count(1 + (int) roots.size()); - // Spawn tasks - spawn_and_wait_for_all(tasks); - // Return nullptr - return nullptr; } }; template - RootTask& - CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) + void CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) { typedef RootTask RootTask; - return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold); - } + tbb::task_group tg; + tg.run_and_wait(RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); + } } diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 400e98a8e..94d10953b 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2257,6 +2257,7 @@ class Values { void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert(size_t j, const gtsam::NavState& nav_state); + void insert(size_t j, double c); void update(size_t j, const gtsam::Point2& point2); void update(size_t j, const gtsam::Point3& point3); @@ -2278,13 +2279,31 @@ class Values { void update(size_t j, const gtsam::NavState& nav_state); void update(size_t j, Vector vector); void update(size_t j, Matrix matrix); + void update(size_t j, double c); - template, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}> + template , + gtsam::imuBias::ConstantBias, + gtsam::NavState, + Vector, + Matrix, + double}> T at(size_t j); - /// version for double - void insertDouble(size_t j, double c); - double atDouble(size_t j) const; }; #include diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 42fe5ae57..8e4cf277c 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -354,7 +354,7 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li _LinearizeOneFactor(*this, linearizationPoint, *linearFG)); // Linearize all non-sendable factors - for(int i = 0; i < size(); i++) { + for(size_t i = 0; i < size(); i++) { auto& factor = (*this)[i]; if(factor && !(factor->sendable())) { (*linearFG)[i] = factor->linearize(linearizationPoint); diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index a99ac9512..787efac51 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -7,9 +7,10 @@ #pragma once -#include #include #include +#include + #include namespace gtsam { @@ -17,25 +18,24 @@ namespace gtsam { /** * Factor that evaluates epipolar error p'Ep for given essential matrix */ -class EssentialMatrixFactor: public NoiseModelFactor1 { - - Vector3 vA_, vB_; ///< Homogeneous versions, in ideal coordinates +class EssentialMatrixFactor : public NoiseModelFactor1 { + Vector3 vA_, vB_; ///< Homogeneous versions, in ideal coordinates typedef NoiseModelFactor1 Base; typedef EssentialMatrixFactor This; -public: - + public: /** * Constructor * @param key Essential Matrix variable key * @param pA point in first camera, in calibrated coordinates * @param pB point in second camera, in calibrated coordinates - * @param model noise model is about dot product in ideal, homogeneous coordinates + * @param model noise model is about dot product in ideal, homogeneous + * coordinates */ EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB, - const SharedNoiseModel& model) : - Base(model, key) { + const SharedNoiseModel& model) + : Base(model, key) { vA_ = EssentialMatrix::Homogeneous(pA); vB_ = EssentialMatrix::Homogeneous(pB); } @@ -45,13 +45,15 @@ public: * @param key Essential Matrix variable key * @param pA point in first camera, in pixel coordinates * @param pB point in second camera, in pixel coordinates - * @param model noise model is about dot product in ideal, homogeneous coordinates + * @param model noise model is about dot product in ideal, homogeneous + * coordinates * @param K calibration object, will be used only in constructor */ - template + template EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB, - const SharedNoiseModel& model, boost::shared_ptr K) : - Base(model, key) { + const SharedNoiseModel& model, + boost::shared_ptr K) + : Base(model, key) { assert(K); vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA)); vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB)); @@ -64,23 +66,25 @@ public: } /// print - void print(const std::string& s = "", + void print( + const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s); std::cout << " EssentialMatrixFactor with measurements\n (" - << vA_.transpose() << ")' and (" << vB_.transpose() << ")'" - << std::endl; + << vA_.transpose() << ")' and (" << vB_.transpose() << ")'" + << std::endl; } /// vector of errors returns 1D vector - Vector evaluateError(const EssentialMatrix& E, boost::optional H = - boost::none) const override { + Vector evaluateError( + const EssentialMatrix& E, + boost::optional H = boost::none) const override { Vector error(1); error << E.error(vA_, vB_, H); return error; } -public: + public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; @@ -88,17 +92,16 @@ public: * Binary factor that optimizes for E and inverse depth d: assumes measurement * in image 2 is perfect, and returns re-projection error in image 1 */ -class EssentialMatrixFactor2: public NoiseModelFactor2 { - - Point3 dP1_; ///< 3D point corresponding to measurement in image 1 - Point2 pn_; ///< Measurement in image 2, in ideal coordinates - double f_; ///< approximate conversion factor for error scaling +class EssentialMatrixFactor2 + : public NoiseModelFactor2 { + Point3 dP1_; ///< 3D point corresponding to measurement in image 1 + Point2 pn_; ///< Measurement in image 2, in ideal coordinates + double f_; ///< approximate conversion factor for error scaling typedef NoiseModelFactor2 Base; typedef EssentialMatrixFactor2 This; -public: - + public: /** * Constructor * @param key1 Essential Matrix variable key @@ -108,8 +111,10 @@ public: * @param model noise model should be in pixels, as well */ EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, - const SharedNoiseModel& model) : - Base(model, key1, key2), dP1_(EssentialMatrix::Homogeneous(pA)), pn_(pB) { + const SharedNoiseModel& model) + : Base(model, key1, key2), + dP1_(EssentialMatrix::Homogeneous(pA)), + pn_(pB) { f_ = 1.0; } @@ -122,11 +127,13 @@ public: * @param K calibration object, will be used only in constructor * @param model noise model should be in pixels, as well */ - template + template EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, - const SharedNoiseModel& model, boost::shared_ptr K) : - Base(model, key1, key2), dP1_( - EssentialMatrix::Homogeneous(K->calibrate(pA))), pn_(K->calibrate(pB)) { + const SharedNoiseModel& model, + boost::shared_ptr K) + : Base(model, key1, key2), + dP1_(EssentialMatrix::Homogeneous(K->calibrate(pA))), + pn_(K->calibrate(pB)) { f_ = 0.5 * (K->fx() + K->fy()); } @@ -137,12 +144,13 @@ public: } /// print - void print(const std::string& s = "", + void print( + const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s); std::cout << " EssentialMatrixFactor2 with measurements\n (" - << dP1_.transpose() << ")' and (" << pn_.transpose() - << ")'" << std::endl; + << dP1_.transpose() << ")' and (" << pn_.transpose() << ")'" + << std::endl; } /* @@ -150,30 +158,28 @@ public: * @param E essential matrix * @param d inverse depth d */ - Vector evaluateError(const EssentialMatrix& E, const double& d, - boost::optional DE = boost::none, boost::optional Dd = - boost::none) const override { - + Vector evaluateError( + const EssentialMatrix& E, const double& d, + boost::optional DE = boost::none, + boost::optional Dd = boost::none) const override { // We have point x,y in image 1 // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d // We then convert to second camera by P2 = 1R2'*(P1-1T2) // The homogeneous coordinates of can be written as // 2R1*(P1-1T2) == 2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2) - // where we multiplied with d which yields equivalent homogeneous coordinates. - // Note that this is just the homography 2R1 for d==0 - // The point d*P1 = (x,y,1) is computed in constructor as dP1_ + // where we multiplied with d which yields equivalent homogeneous + // coordinates. Note that this is just the homography 2R1 for d==0 The point + // d*P1 = (x,y,1) is computed in constructor as dP1_ // Project to normalized image coordinates, then uncalibrate - Point2 pn(0,0); + Point2 pn(0, 0); if (!DE && !Dd) { - Point3 _1T2 = E.direction().point3(); Point3 d1T2 = d * _1T2; - Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2) + Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2) pn = PinholeBase::Project(dP2); } else { - // Calculate derivatives. TODO if slow: optimize with Mathematica // 3*2 3*3 3*3 Matrix D_1T2_dir, DdP2_rot, DP2_point; @@ -187,20 +193,19 @@ public: if (DE) { Matrix DdP2_E(3, 5); - DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2) - *DE = f_ * Dpn_dP2 * DdP2_E; // (2*3) * (3*5) + DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2) + *DE = f_ * Dpn_dP2 * DdP2_E; // (2*3) * (3*5) } - if (Dd) // efficient backwards computation: + if (Dd) // efficient backwards computation: // (2*3) * (3*3) * (3*1) *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2)); - } Point2 reprojectionError = pn - pn_; return f_ * reprojectionError; } -public: + public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // EssentialMatrixFactor2 @@ -210,15 +215,13 @@ public: * in image 2 is perfect, and returns re-projection error in image 1 * This version takes an extrinsic rotation to allow for omni-directional rigs */ -class EssentialMatrixFactor3: public EssentialMatrixFactor2 { - +class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { typedef EssentialMatrixFactor2 Base; typedef EssentialMatrixFactor3 This; - Rot3 cRb_; ///< Rotation from body to camera frame - -public: + Rot3 cRb_; ///< Rotation from body to camera frame + public: /** * Constructor * @param key1 Essential Matrix variable key @@ -229,9 +232,8 @@ public: * @param model noise model should be in calibrated coordinates, as well */ EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB, - const Rot3& cRb, const SharedNoiseModel& model) : - EssentialMatrixFactor2(key1, key2, pA, pB, model), cRb_(cRb) { - } + const Rot3& cRb, const SharedNoiseModel& model) + : EssentialMatrixFactor2(key1, key2, pA, pB, model), cRb_(cRb) {} /** * Constructor @@ -242,12 +244,11 @@ public: * @param K calibration object, will be used only in constructor * @param model noise model should be in pixels, as well */ - template + template EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB, - const Rot3& cRb, const SharedNoiseModel& model, - boost::shared_ptr K) : - EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) { - } + const Rot3& cRb, const SharedNoiseModel& model, + boost::shared_ptr K) + : EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) {} /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { @@ -256,7 +257,8 @@ public: } /// print - void print(const std::string& s = "", + void print( + const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s); std::cout << " EssentialMatrixFactor3 with rotation " << cRb_ << std::endl; @@ -267,9 +269,10 @@ public: * @param E essential matrix * @param d inverse depth d */ - Vector evaluateError(const EssentialMatrix& E, const double& d, - boost::optional DE = boost::none, boost::optional Dd = - boost::none) const override { + Vector evaluateError( + const EssentialMatrix& E, const double& d, + boost::optional DE = boost::none, + boost::optional Dd = boost::none) const override { if (!DE) { // Convert E from body to camera frame EssentialMatrix cameraE = cRb_ * E; @@ -277,18 +280,117 @@ public: return Base::evaluateError(cameraE, d, boost::none, Dd); } else { // Version with derivatives - Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5 + Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5 EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E); Vector e = Base::evaluateError(cameraE, d, D_e_cameraE, Dd); - *DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5) + *DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5) return e; } } -public: + public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // EssentialMatrixFactor3 -}// gtsam +/** + * Binary factor that optimizes for E and calibration K using the algebraic + * epipolar error (K^-1 pA)'E (K^-1 pB). The calibration is shared between two + * images. + * + * Note: As correspondences between 2d coordinates can only recover 7 DoF, + * this factor should always be used with a prior factor on calibration. + * Even with a prior, we can only optimize 2 DoF in the calibration. So the + * prior should have a noise model with very low sigma in the remaining + * dimensions. This has been tested to work on Cal3_S2. With Cal3Bundler, it + * works only with a strong prior (low sigma noisemodel) on all degrees of + * freedom. + */ +template +class EssentialMatrixFactor4 + : public NoiseModelFactor2 { + private: + Point2 pA_, pB_; ///< points in pixel coordinates + typedef NoiseModelFactor2 Base; + typedef EssentialMatrixFactor4 This; + + static constexpr int DimK = FixedDimension::value; + typedef Eigen::Matrix JacobianCalibration; + + public: + /** + * Constructor + * @param keyE Essential Matrix (from camera B to A) variable key + * @param keyK Calibration variable key (common for both cameras) + * @param pA point in first camera, in pixel coordinates + * @param pB point in second camera, in pixel coordinates + * @param model noise model is about dot product in ideal, homogeneous + * coordinates + */ + EssentialMatrixFactor4(Key keyE, Key keyK, const Point2& pA, const Point2& pB, + const SharedNoiseModel& model) + : Base(model, keyE, keyK), pA_(pA), pB_(pB) {} + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print + void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + Base::print(s); + std::cout << " EssentialMatrixFactor4 with measurements\n (" + << pA_.transpose() << ")' and (" << pB_.transpose() << ")'" + << std::endl; + } + + /** + * @brief Calculate the algebraic epipolar error pA' (K^-1)' E K pB. + * + * @param E essential matrix for key keyE + * @param K calibration (common for both images) for key keyK + * @param H1 optional jacobian of error w.r.t E + * @param H2 optional jacobian of error w.r.t K + * @return * Vector 1D vector of algebraic error + */ + Vector evaluateError( + const EssentialMatrix& E, const CALIBRATION& K, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override { + // converting from pixel coordinates to normalized coordinates cA and cB + JacobianCalibration cA_H_K; // dcA/dK + JacobianCalibration cB_H_K; // dcB/dK + Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, boost::none); + Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, boost::none); + + // convert to homogeneous coordinates + Vector3 vA = EssentialMatrix::Homogeneous(cA); + Vector3 vB = EssentialMatrix::Homogeneous(cB); + + if (H2) { + // compute the jacobian of error w.r.t K + + // error function f = vA.T * E * vB + // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK + // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK + // and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]] + *H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K + + vA.transpose() * E.matrix().leftCols<2>() * cB_H_K; + } + + Vector error(1); + error << E.error(vA, vB, H1); + + return error; + } + + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +}; +// EssentialMatrixFactor4 + +} // namespace gtsam diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 0c9f5c42d..79a86865d 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -5,28 +5,27 @@ * @date December 17, 2013 */ -#include - -#include -#include -#include -#include -#include -#include -#include +#include #include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include -#include using namespace boost::placeholders; using namespace std; using namespace gtsam; // Noise model for first type of factor is evaluating algebraic error -noiseModel::Isotropic::shared_ptr model1 = noiseModel::Isotropic::Sigma(1, - 0.01); +noiseModel::Isotropic::shared_ptr model1 = + noiseModel::Isotropic::Sigma(1, 0.01); // Noise model for second type of factor is evaluating pixel coordinates noiseModel::Unit::shared_ptr model2 = noiseModel::Unit::Create(2); @@ -36,39 +35,33 @@ gtsam::Rot3 cRb = gtsam::Rot3(bX, bZ, -bY).inverse(); namespace example1 { -const string filename = findExampleDataFile("5pointExample1.txt"); +const string filename = findExampleDataFile("18pointExample1.txt"); SfmData data; bool readOK = readBAL(filename, data); Rot3 c1Rc2 = data.cameras[1].pose().rotation(); Point3 c1Tc2 = data.cameras[1].pose().translation(); -PinholeCamera camera2(data.cameras[1].pose(), Cal3_S2()); +// TODO: maybe default value not good; assert with 0th +Cal3_S2 trueK = Cal3_S2(); +PinholeCamera camera2(data.cameras[1].pose(), trueK); Rot3 trueRotation(c1Rc2); Unit3 trueDirection(c1Tc2); EssentialMatrix trueE(trueRotation, trueDirection); -double baseline = 0.1; // actual baseline of the camera +double baseline = 0.1; // actual baseline of the camera -Point2 pA(size_t i) { - return data.tracks[i].measurements[0].second; -} -Point2 pB(size_t i) { - return data.tracks[i].measurements[1].second; -} -Vector vA(size_t i) { - return EssentialMatrix::Homogeneous(pA(i)); -} -Vector vB(size_t i) { - return EssentialMatrix::Homogeneous(pB(i)); -} +Point2 pA(size_t i) { return data.tracks[i].measurements[0].second; } +Point2 pB(size_t i) { return data.tracks[i].measurements[1].second; } +Vector vA(size_t i) { return EssentialMatrix::Homogeneous(pA(i)); } +Vector vB(size_t i) { return EssentialMatrix::Homogeneous(pB(i)); } //************************************************************************* -TEST (EssentialMatrixFactor, testData) { +TEST(EssentialMatrixFactor, testData) { CHECK(readOK); // Check E matrix Matrix expected(3, 3); expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0; - Matrix aEb_matrix = skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z()) - * c1Rc2.matrix(); + Matrix aEb_matrix = + skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z()) * c1Rc2.matrix(); EXPECT(assert_equal(expected, aEb_matrix, 1e-8)); // Check some projections @@ -90,7 +83,7 @@ TEST (EssentialMatrixFactor, testData) { } //************************************************************************* -TEST (EssentialMatrixFactor, factor) { +TEST(EssentialMatrixFactor, factor) { Key key(1); for (size_t i = 0; i < 5; i++) { EssentialMatrixFactor factor(key, pA(i), pB(i), model1); @@ -98,19 +91,12 @@ TEST (EssentialMatrixFactor, factor) { // Check evaluation Vector expected(1); expected << 0; - Matrix Hactual; - Vector actual = factor.evaluateError(trueE, Hactual); + Vector actual = factor.evaluateError(trueE); EXPECT(assert_equal(expected, actual, 1e-7)); - // Use numerical derivatives to calculate the expected Jacobian - Matrix Hexpected; - typedef Eigen::Matrix Vector1; - Hexpected = numericalDerivative11( - boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1, - boost::none), trueE); - - // Verify the Jacobian is correct - EXPECT(assert_equal(Hexpected, Hactual, 1e-8)); + Values val; + val.insert(key, trueE); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-7); } } @@ -118,10 +104,10 @@ TEST (EssentialMatrixFactor, factor) { TEST(EssentialMatrixFactor, ExpressionFactor) { Key key(1); for (size_t i = 0; i < 5; i++) { - boost::function)> f = + boost::function)> f = boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); - Expression E_(key); // leaf expression - Expression expr(f, E_); // unary expression + Expression E_(key); // leaf expression + Expression expr(f, E_); // unary expression // Test the derivatives using Paul's magic Values values; @@ -144,13 +130,16 @@ TEST(EssentialMatrixFactor, ExpressionFactor) { TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { Key key(1); for (size_t i = 0; i < 5; i++) { - boost::function)> f = + boost::function)> f = boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); - boost::function, - OptionalJacobian<5, 2>)> g; + boost::function, + OptionalJacobian<5, 2>)> + g; Expression R_(key); Expression d_(trueDirection); - Expression E_(&EssentialMatrix::FromRotationAndDirection, R_, d_); + Expression E_(&EssentialMatrix::FromRotationAndDirection, + R_, d_); Expression expr(f, E_); // Test the derivatives using Paul's magic @@ -171,7 +160,7 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { } //************************************************************************* -TEST (EssentialMatrixFactor, minimization) { +TEST(EssentialMatrixFactor, minimization) { // Here we want to optimize directly on essential matrix constraints // Yi Ma's algorithm (Ma01ijcv) is a bit cumbersome to implement, // but GTSAM does the equivalent anyway, provided we give the right @@ -190,8 +179,8 @@ TEST (EssentialMatrixFactor, minimization) { // Check error at initial estimate Values initial; - EssentialMatrix initialE = trueE.retract( - (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); + EssentialMatrix initialE = + trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); @@ -214,11 +203,10 @@ TEST (EssentialMatrixFactor, minimization) { // Check errors individually for (size_t i = 0; i < 5; i++) EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6); - } //************************************************************************* -TEST (EssentialMatrixFactor2, factor) { +TEST(EssentialMatrixFactor2, factor) { for (size_t i = 0; i < 5; i++) { EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2); @@ -232,22 +220,15 @@ TEST (EssentialMatrixFactor2, factor) { Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2); EXPECT(assert_equal(expected, actual, 1e-7)); - // Use numerical derivatives to calculate the expected Jacobian - Matrix Hexpected1, Hexpected2; - boost::function f = boost::bind( - &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, - boost::none); - Hexpected1 = numericalDerivative21(f, trueE, d); - Hexpected2 = numericalDerivative22(f, trueE, d); - - // Verify the Jacobian is correct - EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); - EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8)); + Values val; + val.insert(100, trueE); + val.insert(i, d); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-7); } } //************************************************************************* -TEST (EssentialMatrixFactor2, minimization) { +TEST(EssentialMatrixFactor2, minimization) { // Here we want to optimize for E and inverse depths at the same time // We start with a factor graph and add constraints to it @@ -290,8 +271,7 @@ TEST (EssentialMatrixFactor2, minimization) { EssentialMatrix bodyE = cRb.inverse() * trueE; //************************************************************************* -TEST (EssentialMatrixFactor3, factor) { - +TEST(EssentialMatrixFactor3, factor) { for (size_t i = 0; i < 5; i++) { EssentialMatrixFactor3 factor(100, i, pA(i), pB(i), cRb, model2); @@ -305,28 +285,21 @@ TEST (EssentialMatrixFactor3, factor) { Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2); EXPECT(assert_equal(expected, actual, 1e-7)); - // Use numerical derivatives to calculate the expected Jacobian - Matrix Hexpected1, Hexpected2; - boost::function f = boost::bind( - &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, - boost::none); - Hexpected1 = numericalDerivative21(f, bodyE, d); - Hexpected2 = numericalDerivative22(f, bodyE, d); - - // Verify the Jacobian is correct - EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); - EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8)); + Values val; + val.insert(100, bodyE); + val.insert(i, d); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-6, 1e-7); } } //************************************************************************* -TEST (EssentialMatrixFactor3, minimization) { - +TEST(EssentialMatrixFactor3, minimization) { // As before, we start with a factor graph and add constraints to it NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) // but now we specify the rotation bRc - graph.emplace_shared(100, i, pA(i), pB(i), cRb, model2); + graph.emplace_shared(100, i, pA(i), pB(i), cRb, + model2); // Check error at ground truth Values truth; @@ -353,7 +326,214 @@ TEST (EssentialMatrixFactor3, minimization) { EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); } -} // namespace example1 +//************************************************************************* +TEST(EssentialMatrixFactor4, factor) { + Key keyE(1); + Key keyK(2); + for (size_t i = 0; i < 5; i++) { + EssentialMatrixFactor4 factor(keyE, keyK, pA(i), pB(i), model1); + + // Check evaluation + Vector1 expected; + expected << 0; + Vector actual = factor.evaluateError(trueE, trueK); + EXPECT(assert_equal(expected, actual, 1e-7)); + + Values truth; + truth.insert(keyE, trueE); + truth.insert(keyK, trueK); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, truth, 1e-6, 1e-7); + } +} + +//************************************************************************* +TEST(EssentialMatrixFactor4, evaluateErrorJacobiansCal3S2) { + Key keyE(1); + Key keyK(2); + // initialize essential matrix + Rot3 r = Rot3::Expmap(Vector3(M_PI / 6, M_PI / 3, M_PI / 9)); + Unit3 t(Point3(2, -1, 0.5)); + EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(r, t); + Cal3_S2 K(200, 1, 1, 10, 10); + Values val; + val.insert(keyE, E); + val.insert(keyK, K); + + Point2 pA(10.0, 20.0); + Point2 pB(12.0, 15.0); + + EssentialMatrixFactor4 f(keyE, keyK, pA, pB, model1); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-6); +} + +//************************************************************************* +TEST(EssentialMatrixFactor4, evaluateErrorJacobiansCal3Bundler) { + Key keyE(1); + Key keyK(2); + // initialize essential matrix + Rot3 r = Rot3::Expmap(Vector3(0, 0, M_PI_2)); + Unit3 t(Point3(0.1, 0, 0)); + EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(r, t); + Cal3Bundler K; + Values val; + val.insert(keyE, E); + val.insert(keyK, K); + + Point2 pA(-0.1, 0.5); + Point2 pB(-0.5, -0.2); + + EssentialMatrixFactor4 f(keyE, keyK, pA, pB, model1); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-5); +} + +//************************************************************************* +TEST(EssentialMatrixFactor4, minimizationWithStrongCal3S2Prior) { + NonlinearFactorGraph graph; + for (size_t i = 0; i < 5; i++) + graph.emplace_shared>(1, 2, pA(i), pB(i), + model1); + + // Check error at ground truth + Values truth; + truth.insert(1, trueE); + truth.insert(2, trueK); + EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + + // Initialization + Values initial; + EssentialMatrix initialE = + trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); + initial.insert(1, initialE); + initial.insert(2, trueK); + + // add prior factor for calibration + Vector5 priorNoiseModelSigma; + priorNoiseModelSigma << 10, 10, 10, 10, 10; + graph.emplace_shared>( + 2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + + LevenbergMarquardtOptimizer optimizer(graph, initial); + Values result = optimizer.optimize(); + + // Check result + EssentialMatrix actualE = result.at(1); + Cal3_S2 actualK = result.at(2); + EXPECT(assert_equal(trueE, actualE, 1e-1)); + EXPECT(assert_equal(trueK, actualK, 1e-2)); + + // Check error at result + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); + + // Check errors individually + for (size_t i = 0; i < 5; i++) + EXPECT_DOUBLES_EQUAL( + 0, + actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))), + EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))), + 1e-6); +} + +//************************************************************************* +TEST(EssentialMatrixFactor4, minimizationWithWeakCal3S2Prior) { + // We need 7 points here as the prior on the focal length parameters is weak + // and the initialization is noisy. So in total we are trying to optimize 7 + // DOF, with a strong prior on the remaining 3 DOF. + NonlinearFactorGraph graph; + for (size_t i = 0; i < 7; i++) + graph.emplace_shared>(1, 2, pA(i), pB(i), + model1); + + // Check error at ground truth + Values truth; + truth.insert(1, trueE); + truth.insert(2, trueK); + EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + + // Initialization + Values initial; + EssentialMatrix initialE = + trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); + Cal3_S2 initialK = + trueK.retract((Vector(5) << 0.1, -0.1, 0.0, -0.0, 0.0).finished()); + initial.insert(1, initialE); + initial.insert(2, initialK); + + // add prior factor for calibration + Vector5 priorNoiseModelSigma; + priorNoiseModelSigma << 20, 20, 1, 1, 1; + graph.emplace_shared>( + 2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + + LevenbergMarquardtOptimizer optimizer(graph, initial); + Values result = optimizer.optimize(); + + // Check result + EssentialMatrix actualE = result.at(1); + Cal3_S2 actualK = result.at(2); + EXPECT(assert_equal(trueE, actualE, 1e-1)); + EXPECT(assert_equal(trueK, actualK, 1e-2)); + + // Check error at result + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); + + // Check errors individually + for (size_t i = 0; i < 7; i++) + EXPECT_DOUBLES_EQUAL( + 0, + actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))), + EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))), + 1e-5); +} + +//************************************************************************* +TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) { + NonlinearFactorGraph graph; + for (size_t i = 0; i < 5; i++) + graph.emplace_shared>(1, 2, pA(i), + pB(i), model1); + Cal3Bundler trueK(1, 0, 0, 0, 0, /*tolerance=*/5e-3); + // Check error at ground truth + Values truth; + truth.insert(1, trueE); + truth.insert(2, trueK); + EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + + // Check error at initial estimate + Values initial; + EssentialMatrix initialE = + trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); + Cal3Bundler initialK = trueK; + initial.insert(1, initialE); + initial.insert(2, initialK); + + // add prior factor for calibration + Vector3 priorNoiseModelSigma; + priorNoiseModelSigma << 0.1, 0.1, 0.1; + graph.emplace_shared>( + 2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + + LevenbergMarquardtOptimizer optimizer(graph, initial); + Values result = optimizer.optimize(); + + // Check result + EssentialMatrix actualE = result.at(1); + Cal3Bundler actualK = result.at(2); + EXPECT(assert_equal(trueE, actualE, 1e-1)); + EXPECT(assert_equal(trueK, actualK, 1e-2)); + + // Check error at result + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); + + // Check errors individually + for (size_t i = 0; i < 5; i++) + EXPECT_DOUBLES_EQUAL( + 0, + actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))), + EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))), + 1e-6); +} + +} // namespace example1 //************************************************************************* @@ -366,30 +546,26 @@ Rot3 aRb = data.cameras[1].pose().rotation(); Point3 aTb = data.cameras[1].pose().translation(); EssentialMatrix trueE(aRb, Unit3(aTb)); -double baseline = 10; // actual baseline of the camera +double baseline = 10; // actual baseline of the camera -Point2 pA(size_t i) { - return data.tracks[i].measurements[0].second; -} -Point2 pB(size_t i) { - return data.tracks[i].measurements[1].second; -} +Point2 pA(size_t i) { return data.tracks[i].measurements[0].second; } +Point2 pB(size_t i) { return data.tracks[i].measurements[1].second; } -boost::shared_ptr // -K = boost::make_shared(500, 0, 0); -PinholeCamera camera2(data.cameras[1].pose(), *K); +Cal3Bundler trueK = Cal3Bundler(500, 0, 0); +boost::shared_ptr K = boost::make_shared(trueK); +PinholeCamera camera2(data.cameras[1].pose(), trueK); Vector vA(size_t i) { - Point2 xy = K->calibrate(pA(i)); + Point2 xy = trueK.calibrate(pA(i)); return EssentialMatrix::Homogeneous(xy); } Vector vB(size_t i) { - Point2 xy = K->calibrate(pB(i)); + Point2 xy = trueK.calibrate(pB(i)); return EssentialMatrix::Homogeneous(xy); } //************************************************************************* -TEST (EssentialMatrixFactor, extraMinimization) { +TEST(EssentialMatrixFactor, extraMinimization) { // Additional test with camera moving in positive X direction NonlinearFactorGraph graph; @@ -403,8 +579,8 @@ TEST (EssentialMatrixFactor, extraMinimization) { // Check error at initial estimate Values initial; - EssentialMatrix initialE = trueE.retract( - (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); + EssentialMatrix initialE = + trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) @@ -428,11 +604,10 @@ TEST (EssentialMatrixFactor, extraMinimization) { // Check errors individually for (size_t i = 0; i < 5; i++) EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6); - } //************************************************************************* -TEST (EssentialMatrixFactor2, extraTest) { +TEST(EssentialMatrixFactor2, extraTest) { for (size_t i = 0; i < 5; i++) { EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2, K); @@ -441,34 +616,27 @@ TEST (EssentialMatrixFactor2, extraTest) { const Point2 pi = camera2.project(P1); Point2 expected(pi - pB(i)); - Matrix Hactual1, Hactual2; double d(baseline / P1.z()); - Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2); + Vector actual = factor.evaluateError(trueE, d); EXPECT(assert_equal(expected, actual, 1e-7)); - // Use numerical derivatives to calculate the expected Jacobian - Matrix Hexpected1, Hexpected2; - boost::function f = boost::bind( - &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, - boost::none); - Hexpected1 = numericalDerivative21(f, trueE, d); - Hexpected2 = numericalDerivative22(f, trueE, d); - - // Verify the Jacobian is correct - EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); - EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8)); + Values val; + val.insert(100, trueE); + val.insert(i, d); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-6); } } //************************************************************************* -TEST (EssentialMatrixFactor2, extraMinimization) { +TEST(EssentialMatrixFactor2, extraMinimization) { // Additional test with camera moving in positive X direction // We start with a factor graph and add constraints to it // Noise sigma is 1, assuming pixel measurements NonlinearFactorGraph graph; for (size_t i = 0; i < data.number_tracks(); i++) - graph.emplace_shared(100, i, pA(i), pB(i), model2, K); + graph.emplace_shared(100, i, pA(i), pB(i), model2, + K); // Check error at ground truth Values truth; @@ -496,8 +664,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) { } //************************************************************************* -TEST (EssentialMatrixFactor3, extraTest) { - +TEST(EssentialMatrixFactor3, extraTest) { // The "true E" in the body frame is EssentialMatrix bodyE = cRb.inverse() * trueE; @@ -509,26 +676,18 @@ TEST (EssentialMatrixFactor3, extraTest) { const Point2 pi = camera2.project(P1); Point2 expected(pi - pB(i)); - Matrix Hactual1, Hactual2; double d(baseline / P1.z()); - Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2); + Vector actual = factor.evaluateError(bodyE, d); EXPECT(assert_equal(expected, actual, 1e-7)); - // Use numerical derivatives to calculate the expected Jacobian - Matrix Hexpected1, Hexpected2; - boost::function f = boost::bind( - &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, - boost::none); - Hexpected1 = numericalDerivative21(f, bodyE, d); - Hexpected2 = numericalDerivative22(f, bodyE, d); - - // Verify the Jacobian is correct - EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); - EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8)); + Values val; + val.insert(100, bodyE); + val.insert(i, d); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-6); } } -} // namespace example2 +} // namespace example2 /* ************************************************************************* */ int main() { @@ -536,4 +695,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam_unstable/timing/timeShonanAveraging.cpp b/gtsam_unstable/timing/timeShonanAveraging.cpp index d42635404..e932b6400 100644 --- a/gtsam_unstable/timing/timeShonanAveraging.cpp +++ b/gtsam_unstable/timing/timeShonanAveraging.cpp @@ -52,7 +52,7 @@ void saveResult(string name, const Values& values) { myfile.open("shonan_result_of_" + name + ".dat"); size_t nrSO3 = values.count(); myfile << "#Type SO3 Number " << nrSO3 << "\n"; - for (int i = 0; i < nrSO3; ++i) { + for (size_t i = 0; i < nrSO3; ++i) { Matrix R = values.at(i).matrix(); // Check if the result of R.Transpose*R satisfy orthogonal constraint checkR(R); @@ -72,7 +72,7 @@ void saveG2oResult(string name, const Values& values, std::map poses ofstream myfile; myfile.open("shonan_result_of_" + name + ".g2o"); size_t nrSO3 = values.count(); - for (int i = 0; i < nrSO3; ++i) { + for (size_t i = 0; i < nrSO3; ++i) { Matrix R = values.at(i).matrix(); // Check if the result of R.Transpose*R satisfy orthogonal constraint checkR(R); @@ -92,7 +92,7 @@ void saveResultQuat(const Values& values) { ofstream myfile; myfile.open("shonan_result.dat"); size_t nrSOn = values.count(); - for (int i = 0; i < nrSOn; ++i) { + for (size_t i = 0; i < nrSOn; ++i) { GTSAM_PRINT(values.at(i)); Rot3 R = Rot3(values.at(i).matrix()); float x = R.toQuaternion().x(); diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index fb4d89148..5f51368e6 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -142,3 +142,13 @@ add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} COMMAND ${PYTHON_EXECUTABLE} ${GTSAM_PYTHON_BUILD_DIRECTORY}/setup.py install DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) + +# Custom make command to run all GTSAM Python tests +add_custom_target( + python-test + COMMAND + ${CMAKE_COMMAND} -E env # add package to python path so no need to install + "PYTHONPATH=${GTSAM_PYTHON_BUILD_DIRECTORY}/$ENV{PYTHONPATH}" + ${PYTHON_EXECUTABLE} -m unittest discover + DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} + WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/tests) diff --git a/python/README.md b/python/README.md index c485d12bd..ec9808ce0 100644 --- a/python/README.md +++ b/python/README.md @@ -35,12 +35,8 @@ For instructions on updating the version of the [wrap library](https://github.co ## Unit Tests The Python toolbox also has a small set of unit tests located in the -test directory. To run them: - - ```bash - cd /python/gtsam/tests - python -m unittest discover - ``` +test directory. +To run them, use `make python-test`. ## Utils diff --git a/timing/timeLago.cpp b/timing/timeLago.cpp index 7aaf37e92..c3ee6ff4b 100644 --- a/timing/timeLago.cpp +++ b/timing/timeLago.cpp @@ -41,11 +41,11 @@ int main(int argc, char *argv[]) { // add noise to create initial estimate Values initial; - Sampler sampler(42u); Values::ConstFiltered poses = solution->filter(); SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0).finished()); + Sampler sampler(noise); for(const Values::ConstFiltered::KeyValuePair& it: poses) - initial.insert(it.key, it.value.retract(sampler.sampleNewModel(noise))); + initial.insert(it.key, it.value.retract(sampler.sample())); // Add prior on the pose having index (key) = 0 noiseModel::Diagonal::shared_ptr priorModel = // diff --git a/wrap/.github/workflows/linux-ci.yml b/wrap/.github/workflows/linux-ci.yml index 34623385e..0ca9ba8f5 100644 --- a/wrap/.github/workflows/linux-ci.yml +++ b/wrap/.github/workflows/linux-ci.yml @@ -10,7 +10,7 @@ jobs: strategy: fail-fast: false matrix: - python-version: [3.6, 3.7, 3.8, 3.9] + python-version: [3.5, 3.6, 3.7, 3.8, 3.9] steps: - name: Checkout diff --git a/wrap/.github/workflows/macos-ci.yml b/wrap/.github/workflows/macos-ci.yml index 3910d28d8..b0ccb3fbe 100644 --- a/wrap/.github/workflows/macos-ci.yml +++ b/wrap/.github/workflows/macos-ci.yml @@ -10,7 +10,7 @@ jobs: strategy: fail-fast: false matrix: - python-version: [3.6, 3.7, 3.8, 3.9] + python-version: [3.5, 3.6, 3.7, 3.8, 3.9] steps: - name: Checkout diff --git a/wrap/DOCS.md b/wrap/DOCS.md index a5ca3fb0c..8537ddd27 100644 --- a/wrap/DOCS.md +++ b/wrap/DOCS.md @@ -100,7 +100,8 @@ The python wrapper supports keyword arguments for functions/methods. Hence, the ``` - Using classes defined in other modules - - If you are using a class `OtherClass` not wrapped in an interface file, add `class OtherClass;` as a forward declaration to avoid a dependency error. `OtherClass` should be in the same project. + - If you are using a class `OtherClass` not wrapped in an interface file, add `class OtherClass;` as a forward declaration to avoid a dependency error. + - `OtherClass` may not be in the same project. If this is the case, include the header for the appropriate project `#include `. - Virtual inheritance - Specify fully-qualified base classes, i.e. `virtual class Derived : ns::Base {` where `ns` is the namespace. diff --git a/wrap/gtwrap/interface_parser/classes.py b/wrap/gtwrap/interface_parser/classes.py index ee4a9725c..ea7a3b3c3 100644 --- a/wrap/gtwrap/interface_parser/classes.py +++ b/wrap/gtwrap/interface_parser/classes.py @@ -189,7 +189,7 @@ class Operator: # Check to ensure arg and return type are the same. if len(args) == 1 and self.operator not in ("()", "[]"): - assert args.args_list[0].ctype.typename.name == return_type.type1.typename.name, \ + assert args.list()[0].ctype.typename.name == return_type.type1.typename.name, \ "Mixed type overloading not supported. Both arg and return type must be the same." def __repr__(self) -> str: diff --git a/wrap/gtwrap/interface_parser/declaration.py b/wrap/gtwrap/interface_parser/declaration.py index 2a916899d..292d6aeaa 100644 --- a/wrap/gtwrap/interface_parser/declaration.py +++ b/wrap/gtwrap/interface_parser/declaration.py @@ -15,6 +15,7 @@ from pyparsing import CharsNotIn, Optional from .tokens import (CLASS, COLON, INCLUDE, LOPBRACK, ROPBRACK, SEMI_COLON, VIRTUAL) from .type import Typename +from .utils import collect_namespaces class Include: @@ -42,11 +43,12 @@ class ForwardDeclaration: t.name, t.parent_type, t.is_virtual)) def __init__(self, - name: Typename, + typename: Typename, parent_type: str, is_virtual: str, parent: str = ''): - self.name = name + self.name = typename.name + self.typename = typename if parent_type: self.parent_type = parent_type else: @@ -55,6 +57,9 @@ class ForwardDeclaration: self.is_virtual = is_virtual self.parent = parent + def namespaces(self) -> list: + """Get the namespaces which this class is nested under as a list.""" + return collect_namespaces(self) + def __repr__(self) -> str: - return "ForwardDeclaration: {} {}({})".format(self.is_virtual, - self.name, self.parent) + return "ForwardDeclaration: {} {}".format(self.is_virtual, self.name) diff --git a/wrap/gtwrap/interface_parser/function.py b/wrap/gtwrap/interface_parser/function.py index bf9b15256..3b9a5d4ad 100644 --- a/wrap/gtwrap/interface_parser/function.py +++ b/wrap/gtwrap/interface_parser/function.py @@ -29,11 +29,13 @@ class Argument: void sayHello(/*`s` is the method argument with type `const string&`*/ const string& s); ``` """ - rule = ((Type.rule ^ TemplatedType.rule)("ctype") + IDENT("name") + \ - Optional(EQUAL + (DEFAULT_ARG ^ Type.rule ^ TemplatedType.rule) + \ - Optional(LPAREN + RPAREN) # Needed to parse the parens for default constructors - )("default") - ).setParseAction(lambda t: Argument(t.ctype, t.name, t.default)) + rule = ((Type.rule ^ TemplatedType.rule)("ctype") # + + IDENT("name") # + + Optional(EQUAL + DEFAULT_ARG)("default") + ).setParseAction(lambda t: Argument( + t.ctype, # + t.name, # + t.default[0] if isinstance(t.default, ParseResults) else None)) def __init__(self, ctype: Union[Type, TemplatedType], @@ -44,18 +46,8 @@ class Argument: else: self.ctype = ctype self.name = name - # If the length is 1, it's a regular type, - if len(default) == 1: - default = default[0] - # This means a tuple has been passed so we convert accordingly - elif len(default) > 1: - default = tuple(default.asList()) - else: - # set to None explicitly so we can support empty strings - default = None self.default = default - - self.parent: Union[ArgumentList, None] = None + self.parent = None # type: Union[ArgumentList, None] def __repr__(self) -> str: return self.to_cpp() @@ -94,10 +86,14 @@ class ArgumentList: def __len__(self) -> int: return len(self.args_list) - def args_names(self) -> List[str]: + def names(self) -> List[str]: """Return a list of the names of all the arguments.""" return [arg.name for arg in self.args_list] + def list(self) -> List[Argument]: + """Return a list of the names of all the arguments.""" + return self.args_list + def to_cpp(self, use_boost: bool) -> List[str]: """Generate the C++ code for wrapping.""" return [arg.ctype.to_cpp(use_boost) for arg in self.args_list] diff --git a/wrap/gtwrap/interface_parser/namespace.py b/wrap/gtwrap/interface_parser/namespace.py index 8aa2e71cc..575d98237 100644 --- a/wrap/gtwrap/interface_parser/namespace.py +++ b/wrap/gtwrap/interface_parser/namespace.py @@ -102,7 +102,7 @@ class Namespace: res = [] for namespace in found_namespaces: classes_and_funcs = (c for c in namespace.content - if isinstance(c, (Class, GlobalFunction))) + if isinstance(c, (Class, GlobalFunction, ForwardDeclaration))) res += [c for c in classes_and_funcs if c.name == typename.name] if not res: raise ValueError("Cannot find class {} in module!".format( diff --git a/wrap/gtwrap/interface_parser/tokens.py b/wrap/gtwrap/interface_parser/tokens.py index c6a40bc31..4eba95900 100644 --- a/wrap/gtwrap/interface_parser/tokens.py +++ b/wrap/gtwrap/interface_parser/tokens.py @@ -10,9 +10,9 @@ All the token definitions. Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert """ -from pyparsing import (Keyword, Literal, Or, QuotedString, Suppress, Word, - alphanums, alphas, delimitedList, nums, - pyparsing_common) +from pyparsing import (Keyword, Literal, OneOrMore, Or, QuotedString, Suppress, + Word, alphanums, alphas, nestedExpr, nums, + originalTextFor, printables) # rule for identifiers (e.g. variable names) IDENT = Word(alphas + '_', alphanums + '_') ^ Word(nums) @@ -22,16 +22,20 @@ RAW_POINTER, SHARED_POINTER, REF = map(Literal, "@*&") LPAREN, RPAREN, LBRACE, RBRACE, COLON, SEMI_COLON = map(Suppress, "(){}:;") LOPBRACK, ROPBRACK, COMMA, EQUAL = map(Suppress, "<>,=") -# Encapsulating type for numbers, and single and double quoted strings. -# The pyparsing_common utilities ensure correct coversion to the corresponding type. -# E.g. pyparsing_common.number will convert 3.1415 to a float type. -NUMBER_OR_STRING = (pyparsing_common.number ^ QuotedString('"') ^ QuotedString("'")) - -# A python tuple, e.g. (1, 9, "random", 3.1415) -TUPLE = (LPAREN + delimitedList(NUMBER_OR_STRING) + RPAREN) - # Default argument passed to functions/methods. -DEFAULT_ARG = (NUMBER_OR_STRING ^ pyparsing_common.identifier ^ TUPLE) +# Allow anything up to ',' or ';' except when they +# appear inside matched expressions such as +# (a, b) {c, b} "hello, world", templates, initializer lists, etc. +DEFAULT_ARG = originalTextFor( + OneOrMore( + QuotedString('"') ^ # parse double quoted strings + QuotedString("'") ^ # parse single quoted strings + Word(printables, excludeChars="(){}[]<>,;") ^ # parse arbitrary words + nestedExpr(opener='(', closer=')') ^ # parse expression in parentheses + nestedExpr(opener='[', closer=']') ^ # parse expression in brackets + nestedExpr(opener='{', closer='}') ^ # parse expression in braces + nestedExpr(opener='<', closer='>') # parse template expressions + )) CONST, VIRTUAL, CLASS, STATIC, PAIR, TEMPLATE, TYPEDEF, INCLUDE = map( Keyword, diff --git a/wrap/gtwrap/interface_parser/variable.py b/wrap/gtwrap/interface_parser/variable.py index dffa2de12..fcb02666f 100644 --- a/wrap/gtwrap/interface_parser/variable.py +++ b/wrap/gtwrap/interface_parser/variable.py @@ -12,7 +12,7 @@ Author: Varun Agrawal, Gerry Chen from pyparsing import Optional, ParseResults -from .tokens import DEFAULT_ARG, EQUAL, IDENT, SEMI_COLON, STATIC +from .tokens import DEFAULT_ARG, EQUAL, IDENT, SEMI_COLON from .type import TemplatedType, Type @@ -32,10 +32,12 @@ class Variable: """ rule = ((Type.rule ^ TemplatedType.rule)("ctype") # + IDENT("name") # - #TODO(Varun) Add support for non-basic types - + Optional(EQUAL + (DEFAULT_ARG))("default") # + + Optional(EQUAL + DEFAULT_ARG)("default") # + SEMI_COLON # - ).setParseAction(lambda t: Variable(t.ctype, t.name, t.default)) + ).setParseAction(lambda t: Variable( + t.ctype, # + t.name, # + t.default[0] if isinstance(t.default, ParseResults) else None)) def __init__(self, ctype: Type, @@ -44,11 +46,7 @@ class Variable: parent=''): self.ctype = ctype[0] # ParseResult is a list self.name = name - if default: - self.default = default[0] - else: - self.default = None - + self.default = default self.parent = parent def __repr__(self) -> str: diff --git a/wrap/gtwrap/matlab_wrapper.py b/wrap/gtwrap/matlab_wrapper.py index 1f9f3146f..de6221bbc 100755 --- a/wrap/gtwrap/matlab_wrapper.py +++ b/wrap/gtwrap/matlab_wrapper.py @@ -57,7 +57,7 @@ class MatlabWrapper(object): # Methods that should be ignored ignore_methods = ['pickle'] # Datatypes that do not need to be checked in methods - not_check_type: list = [] + not_check_type = [] # type: list # Data types that are primitive types not_ptr_type = ['int', 'double', 'bool', 'char', 'unsigned char', 'size_t'] # Ignore the namespace for these datatypes @@ -65,16 +65,18 @@ class MatlabWrapper(object): # The amount of times the wrapper has created a call to geometry_wrapper wrapper_id = 0 # Map each wrapper id to what its collector function namespace, class, type, and string format - wrapper_map: dict = {} + wrapper_map = {} # Set of all the includes in the namespace - includes: Dict[parser.Include, int] = {} + includes = {} # type: Dict[parser.Include, int] # Set of all classes in the namespace - classes: List[Union[parser.Class, instantiator.InstantiatedClass]] = [] - classes_elems: Dict[Union[parser.Class, instantiator.InstantiatedClass], int] = {} + classes = [ + ] # type: List[Union[parser.Class, instantiator.InstantiatedClass]] + classes_elems = { + } # type: Dict[Union[parser.Class, instantiator.InstantiatedClass], int] # Id for ordering global functions in the wrapper global_function_id = 0 # Files and their content - content: List[str] = [] + content = [] # type: List[str] # Ensure the template file is always picked up from the correct directory. dir_path = osp.dirname(osp.realpath(__file__)) @@ -82,11 +84,9 @@ class MatlabWrapper(object): wrapper_file_header = f.read() def __init__(self, - module, module_name, top_module_namespace='', ignore_classes=()): - self.module = module self.module_name = module_name self.top_module_namespace = top_module_namespace self.ignore_classes = ignore_classes @@ -374,14 +374,14 @@ class MatlabWrapper(object): """ arg_wrap = '' - for i, arg in enumerate(args.args_list, 1): + for i, arg in enumerate(args.list(), 1): c_type = self._format_type_name(arg.ctype.typename, include_namespace=False) arg_wrap += '{c_type} {arg_name}{comma}'.format( c_type=c_type, arg_name=arg.name, - comma='' if i == len(args.args_list) else ', ') + comma='' if i == len(args.list()) else ', ') return arg_wrap @@ -396,7 +396,7 @@ class MatlabWrapper(object): """ var_arg_wrap = '' - for i, arg in enumerate(args.args_list, 1): + for i, arg in enumerate(args.list(), 1): name = arg.ctype.typename.name if name in self.not_check_type: continue @@ -442,7 +442,7 @@ class MatlabWrapper(object): var_list_wrap = '' first = True - for i in range(1, len(args.args_list) + 1): + for i in range(1, len(args.list()) + 1): if first: var_list_wrap += 'varargin{{{}}}'.format(i) first = False @@ -462,9 +462,9 @@ class MatlabWrapper(object): if check_statement == '': check_statement = \ 'if length(varargin) == {param_count}'.format( - param_count=len(args.args_list)) + param_count=len(args.list())) - for _, arg in enumerate(args.args_list): + for _, arg in enumerate(args.list()): name = arg.ctype.typename.name if name in self.not_check_type: @@ -516,7 +516,7 @@ class MatlabWrapper(object): params = '' body_args = '' - for arg in args.args_list: + for arg in args.list(): if params != '': params += ',' @@ -725,10 +725,10 @@ class MatlabWrapper(object): param_wrap += ' if' if i == 0 else ' elseif' param_wrap += ' length(varargin) == ' - if len(overload.args.args_list) == 0: + if len(overload.args.list()) == 0: param_wrap += '0\n' else: - param_wrap += str(len(overload.args.args_list)) \ + param_wrap += str(len(overload.args.list())) \ + self._wrap_variable_arguments(overload.args, False) + '\n' # Determine format of return and varargout statements @@ -825,14 +825,14 @@ class MatlabWrapper(object): methods_wrap += textwrap.indent(textwrap.dedent('''\ elseif nargin == {len}{varargin} {ptr}{wrapper}({num}{comma}{var_arg}); - ''').format(len=len(ctor.args.args_list), + ''').format(len=len(ctor.args.list()), varargin=self._wrap_variable_arguments( ctor.args, False), ptr=wrapper_return, wrapper=self._wrapper_name(), num=self._update_wrapper_id( (namespace_name, inst_class, 'constructor', ctor)), - comma='' if len(ctor.args.args_list) == 0 else ', ', + comma='' if len(ctor.args.list()) == 0 else ', ', var_arg=self._wrap_list_variable_arguments(ctor.args)), prefix=' ') @@ -938,7 +938,7 @@ class MatlabWrapper(object): namespace_name, inst_class, methods, - serialize=(False,)): + serialize=(False, )): """Wrap the methods in the class. Args: @@ -1075,7 +1075,7 @@ class MatlabWrapper(object): static_overload.return_type, include_namespace=True, separator="."), - length=len(static_overload.args.args_list), + length=len(static_overload.args.list()), var_args_list=self._wrap_variable_arguments( static_overload.args), check_statement=check_statement, @@ -1097,7 +1097,8 @@ class MatlabWrapper(object): method_text += textwrap.indent(textwrap.dedent("""\ end\n - """), prefix=" ") + """), + prefix=" ") if serialize: method_text += textwrap.indent(textwrap.dedent("""\ @@ -1349,14 +1350,14 @@ class MatlabWrapper(object): else: if isinstance(method.parent, instantiator.InstantiatedClass): - method_name = method.parent.cpp_class() + "::" + method_name = method.parent.to_cpp() + "::" else: method_name = self._format_static_method(method, '::') method_name += method.name if "MeasureRange" in method_name: self._debug("method: {}, method: {}, inst: {}".format( - method_name, method.name, method.parent.cpp_class())) + method_name, method.name, method.parent.to_cpp())) obj = ' ' if return_1_name == 'void' else '' obj += '{}{}({})'.format(obj_start, method_name, params) @@ -1447,7 +1448,7 @@ class MatlabWrapper(object): extra = collector_func[4] class_name = collector_func[0] + collector_func[1].name - class_name_separated = collector_func[1].cpp_class() + class_name_separated = collector_func[1].to_cpp() is_method = isinstance(extra, parser.Method) is_static_method = isinstance(extra, parser.StaticMethod) @@ -1510,12 +1511,12 @@ class MatlabWrapper(object): elif extra == 'serialize': body += self.wrap_collector_function_serialize( collector_func[1].name, - full_name=collector_func[1].cpp_class(), + full_name=collector_func[1].to_cpp(), namespace=collector_func[0]) elif extra == 'deserialize': body += self.wrap_collector_function_deserialize( collector_func[1].name, - full_name=collector_func[1].cpp_class(), + full_name=collector_func[1].to_cpp(), namespace=collector_func[0]) elif is_method or is_static_method: method_name = '' @@ -1548,7 +1549,7 @@ class MatlabWrapper(object): min1='-1' if is_method else '', shared_obj=shared_obj, method_name=method_name, - num_args=len(extra.args.args_list), + num_args=len(extra.args.list()), body_args=body_args, return_body=return_body) @@ -1565,10 +1566,11 @@ class MatlabWrapper(object): checkArguments("{function_name}",nargout,nargin,{len}); ''').format(function_name=collector_func[1].name, id=self.global_function_id, - len=len(collector_func[1].args.args_list)) + len=len(collector_func[1].args.list())) body += self._wrapper_unwrap_arguments(collector_func[1].args)[1] - body += self.wrap_collector_function_return(collector_func[1]) + '\n}\n' + body += self.wrap_collector_function_return( + collector_func[1]) + '\n}\n' collector_function += body @@ -1723,7 +1725,7 @@ class MatlabWrapper(object): cls_insts += self._format_type_name(inst) typedef_instances += 'typedef {original_class_name} {class_name_sep};\n' \ - .format(original_class_name=cls.cpp_class(), + .format(original_class_name=cls.to_cpp(), class_name_sep=cls.name) class_name_sep = cls.name @@ -1734,7 +1736,7 @@ class MatlabWrapper(object): boost_class_export_guid += 'BOOST_CLASS_EXPORT_GUID({}, "{}");\n'.format( class_name_sep, class_name) else: - class_name_sep = cls.cpp_class() + class_name_sep = cls.to_cpp() class_name = self._format_class_name(cls) if len(cls.original.namespaces()) > 1 and _has_serialization( @@ -1780,7 +1782,7 @@ class MatlabWrapper(object): if queue_set_next_case: ptr_ctor_frag += self.wrap_collector_function_upcast_from_void( - id_val[1].name, idx, id_val[1].cpp_class()) + id_val[1].name, idx, id_val[1].to_cpp()) wrapper_file += textwrap.dedent('''\ {typedef_instances} @@ -1872,10 +1874,14 @@ class MatlabWrapper(object): namespace=namespace), prefix=' ') - def wrap(self): + def wrap(self, content): """High level function to wrap the project.""" - self.wrap_namespace(self.module) - self.generate_wrapper(self.module) + # Parse the contents of the interface file + parsed_result = parser.Module.parseString(content) + # Instantiate the module + module = instantiator.instantiate_namespace(parsed_result) + self.wrap_namespace(module) + self.generate_wrapper(module) return self.content diff --git a/wrap/gtwrap/pybind_wrapper.py b/wrap/gtwrap/pybind_wrapper.py index 8f8dde224..0e1b3c7ea 100755 --- a/wrap/gtwrap/pybind_wrapper.py +++ b/wrap/gtwrap/pybind_wrapper.py @@ -23,48 +23,49 @@ class PybindWrapper: Class to generate binding code for Pybind11 specifically. """ def __init__(self, - module, module_name, top_module_namespaces='', use_boost=False, ignore_classes=(), module_template=""): - self.module = module self.module_name = module_name self.top_module_namespaces = top_module_namespaces self.use_boost = use_boost self.ignore_classes = ignore_classes self._serializing_classes = list() self.module_template = module_template - self.python_keywords = ['print', 'lambda'] + self.python_keywords = [ + 'lambda', 'False', 'def', 'if', 'raise', 'None', 'del', 'import', + 'return', 'True', 'elif', 'in', 'try', 'and', 'else', 'is', + 'while', 'as', 'except', 'lambda', 'with', 'assert', 'finally', + 'nonlocal', 'yield', 'break', 'for', 'not', 'class', 'from', 'or', + 'continue', 'global', 'pass' + ] # amount of indentation to add before each function/method declaration. self.method_indent = '\n' + (' ' * 8) - def _py_args_names(self, args_list): + def _py_args_names(self, args): """Set the argument names in Pybind11 format.""" - names = args_list.args_names() + names = args.names() if names: py_args = [] - for arg in args_list.args_list: - if isinstance(arg.default, str) and arg.default is not None: - # string default arg - arg.default = ' = "{arg.default}"'.format(arg=arg) - elif arg.default: # Other types - arg.default = ' = {arg.default}'.format(arg=arg) + for arg in args.list(): + if arg.default is not None: + default = ' = {arg.default}'.format(arg=arg) else: - arg.default = '' + default = '' argument = 'py::arg("{name}"){default}'.format( - name=arg.name, default='{0}'.format(arg.default)) + name=arg.name, default='{0}'.format(default)) py_args.append(argument) return ", " + ", ".join(py_args) else: return '' - def _method_args_signature_with_names(self, args_list): - """Define the method signature types with the argument names.""" - cpp_types = args_list.to_cpp(self.use_boost) - names = args_list.args_names() + def _method_args_signature(self, args): + """Generate the argument types and names as per the method signature.""" + cpp_types = args.to_cpp(self.use_boost) + names = args.names() types_names = [ "{} {}".format(ctype, name) for ctype, name in zip(cpp_types, names) @@ -99,7 +100,8 @@ class PybindWrapper: serialize_method = self.method_indent + \ ".def(\"serialize\", []({class_inst} self){{ return gtsam::serialize(*self); }})".format(class_inst=cpp_class + '*') deserialize_method = self.method_indent + \ - ".def(\"deserialize\", []({class_inst} self, string serialized){{ gtsam::deserialize(serialized, *self); }}, py::arg(\"serialized\"))" \ + '.def("deserialize", []({class_inst} self, string serialized)' \ + '{{ gtsam::deserialize(serialized, *self); }}, py::arg("serialized"))' \ .format(class_inst=cpp_class + '*') return serialize_method + deserialize_method @@ -112,20 +114,23 @@ class PybindWrapper: return pickle_method.format(cpp_class=cpp_class, indent=self.method_indent) + # Add underscore to disambiguate if the method name matches a python keyword + if py_method in self.python_keywords: + py_method = py_method + "_" + is_method = isinstance(method, instantiator.InstantiatedMethod) is_static = isinstance(method, parser.StaticMethod) return_void = method.return_type.is_void() - args_names = method.args.args_names() + args_names = method.args.names() py_args_names = self._py_args_names(method.args) - args_signature_with_names = self._method_args_signature_with_names( - method.args) + args_signature_with_names = self._method_args_signature(method.args) caller = cpp_class + "::" if not is_method else "self->" - function_call = ('{opt_return} {caller}{function_name}' + function_call = ('{opt_return} {caller}{method_name}' '({args_names});'.format( opt_return='return' if not return_void else '', caller=caller, - function_name=cpp_method, + method_name=cpp_method, args_names=', '.join(args_names), )) @@ -136,8 +141,7 @@ class PybindWrapper: '{py_args_names}){suffix}'.format( prefix=prefix, cdef="def_static" if is_static else "def", - py_method=py_method if not py_method in self.python_keywords - else py_method + "_", + py_method=py_method, opt_self="{cpp_class}* self".format( cpp_class=cpp_class) if is_method else "", opt_comma=', ' if is_method and args_names else '', @@ -181,15 +185,13 @@ class PybindWrapper: suffix=''): """ Wrap all the methods in the `cpp_class`. - - This function is also used to wrap global functions. """ res = "" for method in methods: - # To avoid type confusion for insert, currently unused + # To avoid type confusion for insert if method.name == 'insert' and cpp_class == 'gtsam::Values': - name_list = method.args.args_names() + name_list = method.args.names() type_list = method.args.to_cpp(self.use_boost) # inserting non-wrapped value types if type_list[0].strip() == 'size_t': @@ -214,7 +216,8 @@ class PybindWrapper: module_var, variable, prefix='\n' + ' ' * 8): - """Wrap a variable that's not part of a class (i.e. global) + """ + Wrap a variable that's not part of a class (i.e. global) """ variable_value = "" if variable.default is None: @@ -288,23 +291,20 @@ class PybindWrapper: def wrap_enums(self, enums, instantiated_class, prefix=' ' * 4): """Wrap multiple enums defined in a class.""" - cpp_class = instantiated_class.cpp_class() + cpp_class = instantiated_class.to_cpp() module_var = instantiated_class.name.lower() res = '' for enum in enums: res += "\n" + self.wrap_enum( - enum, - class_name=cpp_class, - module=module_var, - prefix=prefix) + enum, class_name=cpp_class, module=module_var, prefix=prefix) return res def wrap_instantiated_class( self, instantiated_class: instantiator.InstantiatedClass): """Wrap the class.""" module_var = self._gen_module_var(instantiated_class.namespaces()) - cpp_class = instantiated_class.cpp_class() + cpp_class = instantiated_class.to_cpp() if cpp_class in self.ignore_classes: return "" if instantiated_class.parent_class: @@ -356,10 +356,27 @@ class PybindWrapper: wrapped_operators=self.wrap_operators( instantiated_class.operators, cpp_class))) + def wrap_instantiated_declaration( + self, instantiated_decl: instantiator.InstantiatedDeclaration): + """Wrap the class.""" + module_var = self._gen_module_var(instantiated_decl.namespaces()) + cpp_class = instantiated_decl.to_cpp() + if cpp_class in self.ignore_classes: + return "" + + res = ( + '\n py::class_<{cpp_class}, ' + '{shared_ptr_type}::shared_ptr<{cpp_class}>>({module_var}, "{class_name}")' + ).format(shared_ptr_type=('boost' if self.use_boost else 'std'), + cpp_class=cpp_class, + class_name=instantiated_decl.name, + module_var=module_var) + return res + def wrap_stl_class(self, stl_class): """Wrap STL containers.""" module_var = self._gen_module_var(stl_class.namespaces()) - cpp_class = stl_class.cpp_class() + cpp_class = stl_class.to_cpp() if cpp_class in self.ignore_classes: return "" @@ -385,6 +402,59 @@ class PybindWrapper: stl_class.properties, cpp_class), )) + def wrap_functions(self, + functions, + namespace, + prefix='\n' + ' ' * 8, + suffix=''): + """ + Wrap all the global functions. + """ + res = "" + for function in functions: + + function_name = function.name + + # Add underscore to disambiguate if the function name matches a python keyword + python_keywords = self.python_keywords + ['print'] + if function_name in python_keywords: + function_name = function_name + "_" + + cpp_method = function.to_cpp() + + is_static = isinstance(function, parser.StaticMethod) + return_void = function.return_type.is_void() + args_names = function.args.names() + py_args_names = self._py_args_names(function.args) + args_signature = self._method_args_signature(function.args) + + caller = namespace + "::" + function_call = ('{opt_return} {caller}{function_name}' + '({args_names});'.format( + opt_return='return' + if not return_void else '', + caller=caller, + function_name=cpp_method, + args_names=', '.join(args_names), + )) + + ret = ('{prefix}.{cdef}("{function_name}",' + '[]({args_signature}){{' + '{function_call}' + '}}' + '{py_args_names}){suffix}'.format( + prefix=prefix, + cdef="def_static" if is_static else "def", + function_name=function_name, + args_signature=args_signature, + function_call=function_call, + py_args_names=py_args_names, + suffix=suffix)) + + res += ret + + return res + def _partial_match(self, namespaces1, namespaces2): for i in range(min(len(namespaces1), len(namespaces2))): if namespaces1[i] != namespaces2[i]: @@ -460,6 +530,9 @@ class PybindWrapper: wrapped += self.wrap_instantiated_class(element) wrapped += self.wrap_enums(element.enums, element) + elif isinstance(element, instantiator.InstantiatedDeclaration): + wrapped += self.wrap_instantiated_declaration(element) + elif isinstance(element, parser.Variable): variable_namespace = self._add_namespaces('', namespaces) wrapped += self.wrap_variable(namespace=variable_namespace, @@ -476,7 +549,7 @@ class PybindWrapper: if isinstance(func, (parser.GlobalFunction, instantiator.InstantiatedGlobalFunction)) ] - wrapped += self.wrap_methods( + wrapped += self.wrap_functions( all_funcs, self._add_namespaces('', namespaces)[:-2], prefix='\n' + ' ' * 4 + module_var, @@ -484,9 +557,14 @@ class PybindWrapper: ) return wrapped, includes - def wrap(self): + def wrap(self, content): """Wrap the code in the interface file.""" - wrapped_namespace, includes = self.wrap_namespace(self.module) + # Parse the contents of the interface file + module = parser.Module.parseString(content) + # Instantiate all templates + module = instantiator.instantiate_namespace(module) + + wrapped_namespace, includes = self.wrap_namespace(module) # Export classes for serialization. boost_class_export = "" diff --git a/wrap/gtwrap/template_instantiator.py b/wrap/gtwrap/template_instantiator.py index a66fa9544..c47424648 100644 --- a/wrap/gtwrap/template_instantiator.py +++ b/wrap/gtwrap/template_instantiator.py @@ -95,10 +95,9 @@ def instantiate_args_list(args_list, template_typenames, instantiations, for arg in args_list: new_type = instantiate_type(arg.ctype, template_typenames, instantiations, cpp_typename) - default = [arg.default] if isinstance(arg, parser.Argument) else '' - instantiated_args.append(parser.Argument(name=arg.name, - ctype=new_type, - default=default)) + instantiated_args.append( + parser.Argument(name=arg.name, ctype=new_type, + default=arg.default)) return instantiated_args @@ -131,7 +130,6 @@ def instantiate_name(original_name, instantiations): TODO(duy): To avoid conflicts, we should include the instantiation's namespaces, but I find that too verbose. """ - inst_name = '' instantiated_names = [] for inst in instantiations: # Ensure the first character of the type is capitalized @@ -172,7 +170,7 @@ class InstantiatedGlobalFunction(parser.GlobalFunction): cpp_typename='', ) instantiated_args = instantiate_args_list( - original.args.args_list, + original.args.list(), self.original.template.typenames, self.instantiations, # Keyword type name `This` should already be replaced in the @@ -206,7 +204,13 @@ class InstantiatedGlobalFunction(parser.GlobalFunction): class InstantiatedMethod(parser.Method): """ - We can only instantiate template methods with a single template parameter. + Instantiate method with template parameters. + + E.g. + class A { + template + void func(X x, Y y); + } """ def __init__(self, original, instantiations: List[parser.Typename] = ''): self.original = original @@ -216,7 +220,7 @@ class InstantiatedMethod(parser.Method): self.parent = original.parent # Check for typenames if templated. - # This way, we can gracefully handle bot templated and non-templated methois. + # This way, we can gracefully handle both templated and non-templated methods. typenames = self.original.template.typenames if self.original.template else [] self.name = instantiate_name(original.name, self.instantiations) self.return_type = instantiate_return_type( @@ -229,7 +233,7 @@ class InstantiatedMethod(parser.Method): ) instantiated_args = instantiate_args_list( - original.args.args_list, + original.args.list(), typenames, self.instantiations, # Keyword type name `This` should already be replaced in the @@ -342,7 +346,7 @@ class InstantiatedClass(parser.Class): "{ctors}\n{static_methods}\n{methods}".format( virtual="virtual" if self.is_virtual else '', name=self.name, - cpp_class=self.cpp_class(), + cpp_class=self.to_cpp(), parent_class=self.parent, ctors="\n".join([repr(ctor) for ctor in self.ctors]), methods="\n".join([repr(m) for m in self.methods]), @@ -364,7 +368,7 @@ class InstantiatedClass(parser.Class): for ctor in self.original.ctors: instantiated_args = instantiate_args_list( - ctor.args.args_list, + ctor.args.list(), typenames, self.instantiations, self.cpp_typename(), @@ -389,7 +393,7 @@ class InstantiatedClass(parser.Class): instantiated_static_methods = [] for static_method in self.original.static_methods: instantiated_args = instantiate_args_list( - static_method.args.args_list, typenames, self.instantiations, + static_method.args.list(), typenames, self.instantiations, self.cpp_typename()) instantiated_static_methods.append( parser.StaticMethod( @@ -426,7 +430,7 @@ class InstantiatedClass(parser.Class): class_instantiated_methods = [] for method in self.original.methods: instantiated_args = instantiate_args_list( - method.args.args_list, + method.args.list(), typenames, self.instantiations, self.cpp_typename(), @@ -459,7 +463,7 @@ class InstantiatedClass(parser.Class): instantiated_operators = [] for operator in self.original.operators: instantiated_args = instantiate_args_list( - operator.args.args_list, + operator.args.list(), typenames, self.instantiations, self.cpp_typename(), @@ -497,10 +501,6 @@ class InstantiatedClass(parser.Class): ) return instantiated_properties - def cpp_class(self): - """Generate the C++ code for wrapping.""" - return self.cpp_typename().to_cpp() - def cpp_typename(self): """ Return a parser.Typename including namespaces and cpp name of this @@ -516,8 +516,53 @@ class InstantiatedClass(parser.Class): namespaces_name.append(name) return parser.Typename(namespaces_name) + def to_cpp(self): + """Generate the C++ code for wrapping.""" + return self.cpp_typename().to_cpp() -def instantiate_namespace_inplace(namespace): + +class InstantiatedDeclaration(parser.ForwardDeclaration): + """ + Instantiate typedefs of forward declarations. + This is useful when we wish to typedef a templated class + which is not defined in the current project. + + E.g. + class FactorFromAnotherMother; + + typedef FactorFromAnotherMother FactorWeCanUse; + """ + def __init__(self, original, instantiations=(), new_name=''): + super().__init__(original.typename, + original.parent_type, + original.is_virtual, + parent=original.parent) + + self.original = original + self.instantiations = instantiations + self.parent = original.parent + + self.name = instantiate_name( + original.name, instantiations) if not new_name else new_name + + def to_cpp(self): + """Generate the C++ code for wrapping.""" + instantiated_names = [ + inst.qualified_name() for inst in self.instantiations + ] + name = "{}<{}>".format(self.original.name, + ",".join(instantiated_names)) + namespaces_name = self.namespaces() + namespaces_name.append(name) + # Leverage Typename to generate the fully qualified C++ name + return parser.Typename(namespaces_name).to_cpp() + + def __repr__(self): + return "Instantiated {}".format( + super(InstantiatedDeclaration, self).__repr__()) + + +def instantiate_namespace(namespace): """ Instantiate the classes and other elements in the `namespace` content and assign it back to the namespace content attribute. @@ -567,7 +612,8 @@ def instantiate_namespace_inplace(namespace): original_element = top_level.find_class_or_function( typedef_inst.typename) - # Check if element is a typedef'd class or function. + # Check if element is a typedef'd class, function or + # forward declaration from another project. if isinstance(original_element, parser.Class): typedef_content.append( InstantiatedClass(original_element, @@ -578,12 +624,19 @@ def instantiate_namespace_inplace(namespace): InstantiatedGlobalFunction( original_element, typedef_inst.typename.instantiations, typedef_inst.new_name)) + elif isinstance(original_element, parser.ForwardDeclaration): + typedef_content.append( + InstantiatedDeclaration( + original_element, typedef_inst.typename.instantiations, + typedef_inst.new_name)) elif isinstance(element, parser.Namespace): - instantiate_namespace_inplace(element) + element = instantiate_namespace(element) instantiated_content.append(element) else: instantiated_content.append(element) instantiated_content.extend(typedef_content) namespace.content = instantiated_content + + return namespace diff --git a/wrap/scripts/matlab_wrap.py b/wrap/scripts/matlab_wrap.py index 232e93490..be6043947 100644 --- a/wrap/scripts/matlab_wrap.py +++ b/wrap/scripts/matlab_wrap.py @@ -8,9 +8,8 @@ and invoked during the wrapping by CMake. import argparse import os +import sys -import gtwrap.interface_parser as parser -import gtwrap.template_instantiator as instantiator from gtwrap.matlab_wrapper import MatlabWrapper, generate_content if __name__ == "__main__": @@ -51,18 +50,11 @@ if __name__ == "__main__": if not os.path.exists(args.src): os.mkdir(args.src) - module = parser.Module.parseString(content) - - instantiator.instantiate_namespace_inplace(module) - - import sys - print("Ignoring classes: {}".format(args.ignore), file=sys.stderr) - wrapper = MatlabWrapper(module=module, - module_name=args.module_name, + wrapper = MatlabWrapper(module_name=args.module_name, top_module_namespace=top_module_namespaces, ignore_classes=args.ignore) - cc_content = wrapper.wrap() + cc_content = wrapper.wrap(content) generate_content(cc_content, args.out) diff --git a/wrap/scripts/pybind_wrap.py b/wrap/scripts/pybind_wrap.py index 26e63d51c..7f2f8d419 100644 --- a/wrap/scripts/pybind_wrap.py +++ b/wrap/scripts/pybind_wrap.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 - """ Helper script to wrap C++ to Python with Pybind. This script is installed via CMake to the user's binary directory @@ -10,8 +9,6 @@ and invoked during the wrapping by CMake. import argparse -import gtwrap.interface_parser as parser -import gtwrap.template_instantiator as instantiator from gtwrap.pybind_wrapper import PybindWrapper @@ -19,11 +16,10 @@ def main(): """Main runner.""" arg_parser = argparse.ArgumentParser( formatter_class=argparse.ArgumentDefaultsHelpFormatter) - arg_parser.add_argument( - "--src", - type=str, - required=True, - help="Input interface .i/.h file") + arg_parser.add_argument("--src", + type=str, + required=True, + help="Input interface .i/.h file") arg_parser.add_argument( "--module_name", type=str, @@ -62,7 +58,8 @@ def main(): help="A space-separated list of classes to ignore. " "Class names must include their full namespaces.", ) - arg_parser.add_argument("--template", type=str, + arg_parser.add_argument("--template", + type=str, help="The module template file") args = arg_parser.parse_args() @@ -74,14 +71,10 @@ def main(): with open(args.src, "r") as f: content = f.read() - module = parser.Module.parseString(content) - instantiator.instantiate_namespace_inplace(module) - with open(args.template, "r") as f: template_content = f.read() wrapper = PybindWrapper( - module=module, module_name=args.module_name, use_boost=args.use_boost, top_module_namespaces=top_module_namespaces, @@ -90,7 +83,7 @@ def main(): ) # Wrap the code and get back the cpp/cc code. - cc_content = wrapper.wrap() + cc_content = wrapper.wrap(content) # Generate the C++ code which Pybind11 will use. with open(args.out, "w") as f: diff --git a/wrap/setup.py b/wrap/setup.py index 8ef61f312..e8962f175 100644 --- a/wrap/setup.py +++ b/wrap/setup.py @@ -10,7 +10,7 @@ packages = find_packages() setup( name='gtwrap', description='Library to wrap C++ with Python and Matlab', - version='1.1.0', + version='2.0.0', author="Frank Dellaert et. al.", author_email="dellaert@gatech.edu", license='BSD', diff --git a/wrap/tests/expected/matlab/MultipleTemplatesIntDouble.m b/wrap/tests/expected/matlab/MultipleTemplatesIntDouble.m index 85de8002f..1a00572e0 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(48, my_ptr); + class_wrapper(49, 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(49, obj.ptr_MultipleTemplatesIntDouble); + class_wrapper(50, 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 90b79d560..6239b1bd1 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(50, my_ptr); + class_wrapper(51, 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(51, obj.ptr_MultipleTemplatesIntFloat); + class_wrapper(52, 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 ea2e335c7..2dd4b5428 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(52, my_ptr); + class_wrapper(56, 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(53, varargin{1}, varargin{2}, varargin{3}, varargin{4}); + my_ptr = class_wrapper(57, 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(54, obj.ptr_MyFactorPosePoint2); + class_wrapper(58, 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(55, this, varargin{:}); + class_wrapper(59, 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 c95136cc9..00a8f1965 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(45, my_ptr); + class_wrapper(46, my_ptr); elseif nargin == 0 - my_ptr = class_wrapper(46); + my_ptr = class_wrapper(47); 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(47, obj.ptr_MyVector12); + class_wrapper(48, 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 fe0ec9c5f..5d4a80cd8 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(42, my_ptr); + class_wrapper(43, my_ptr); elseif nargin == 0 - my_ptr = class_wrapper(43); + my_ptr = class_wrapper(44); 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(44, obj.ptr_MyVector3); + class_wrapper(45, 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 8e8e94dc6..14f04a825 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(38, my_ptr); + class_wrapper(39, my_ptr); elseif nargin == 0 - my_ptr = class_wrapper(39); + my_ptr = class_wrapper(40); 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(40, obj.ptr_PrimitiveRefDouble); + class_wrapper(41, 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(41, varargin{:}); + varargout{1} = class_wrapper(42, varargin{:}); return end diff --git a/wrap/tests/expected/matlab/TemplatedFunctionRot3.m b/wrap/tests/expected/matlab/TemplatedFunctionRot3.m index 5b90c2473..4216201b4 100644 --- a/wrap/tests/expected/matlab/TemplatedFunctionRot3.m +++ b/wrap/tests/expected/matlab/TemplatedFunctionRot3.m @@ -1,6 +1,6 @@ function varargout = TemplatedFunctionRot3(varargin) if length(varargin) == 1 && isa(varargin{1},'gtsam.Rot3') - functions_wrapper(11, varargin{:}); + functions_wrapper(14, varargin{:}); else error('Arguments do not match any overload of function TemplatedFunctionRot3'); end diff --git a/wrap/tests/expected/matlab/Test.m b/wrap/tests/expected/matlab/Test.m index 4c7b5eeab..c39882a93 100644 --- a/wrap/tests/expected/matlab/Test.m +++ b/wrap/tests/expected/matlab/Test.m @@ -10,6 +10,7 @@ %create_MixedPtrs() : returns pair< Test, Test > %create_ptrs() : returns pair< Test, Test > %get_container() : returns std::vector +%lambda() : returns void %print() : returns void %return_Point2Ptr(bool value) : returns Point2 %return_Test(Test value) : returns Test @@ -98,11 +99,21 @@ classdef Test < handle error('Arguments do not match any overload of function Test.get_container'); end + function varargout = lambda(this, varargin) + % LAMBDA usage: lambda() : returns void + % Doxygen can be found at https://gtsam.org/doxygen/ + if length(varargin) == 0 + class_wrapper(18, this, varargin{:}); + return + end + error('Arguments do not match any overload of function Test.lambda'); + end + function varargout = print(this, varargin) % PRINT usage: print() : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 - class_wrapper(18, this, varargin{:}); + class_wrapper(19, this, varargin{:}); return end error('Arguments do not match any overload of function Test.print'); @@ -112,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(19, this, varargin{:}); + varargout{1} = class_wrapper(20, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_Point2Ptr'); @@ -122,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(20, this, varargin{:}); + varargout{1} = class_wrapper(21, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_Test'); @@ -132,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(21, this, varargin{:}); + varargout{1} = class_wrapper(22, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_TestPtr'); @@ -142,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(22, this, varargin{:}); + varargout{1} = class_wrapper(23, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_bool'); @@ -152,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(23, this, varargin{:}); + varargout{1} = class_wrapper(24, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_double'); @@ -162,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(24, this, varargin{:}); + varargout{1} = class_wrapper(25, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_field'); @@ -172,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(25, this, varargin{:}); + varargout{1} = class_wrapper(26, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_int'); @@ -182,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(26, this, varargin{:}); + varargout{1} = class_wrapper(27, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_matrix1'); @@ -192,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(27, this, varargin{:}); + varargout{1} = class_wrapper(28, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_matrix2'); @@ -202,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(28, this, varargin{:}); + [ varargout{1} varargout{2} ] = class_wrapper(29, 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(29, this, varargin{:}); + [ varargout{1} varargout{2} ] = class_wrapper(30, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_pair'); @@ -218,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(30, this, varargin{:}); + [ varargout{1} varargout{2} ] = class_wrapper(31, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_ptrs'); @@ -228,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(31, this, varargin{:}); + varargout{1} = class_wrapper(32, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_size_t'); @@ -238,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(32, this, varargin{:}); + varargout{1} = class_wrapper(33, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_string'); @@ -248,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(33, this, varargin{:}); + varargout{1} = class_wrapper(34, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_vector1'); @@ -258,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(34, this, varargin{:}); + varargout{1} = class_wrapper(35, 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(35, 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') @@ -283,6 +288,12 @@ classdef Test < handle 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') + class_wrapper(38, 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 3fc2e5daf..e644ac00f 100644 --- a/wrap/tests/expected/matlab/class_wrapper.cpp +++ b/wrap/tests/expected/matlab/class_wrapper.cpp @@ -33,6 +33,8 @@ typedef std::set*> Collector_Multi static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; typedef std::set*> Collector_MultipleTemplatesIntFloat; static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; +typedef std::set*> Collector_ForwardKinematics; +static Collector_ForwardKinematics collector_ForwardKinematics; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; @@ -90,6 +92,12 @@ void _deleteAllObjects() collector_MultipleTemplatesIntFloat.erase(iter++); anyDeleted = true; } } + { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); + iter != collector_ForwardKinematics.end(); ) { + delete *iter; + collector_ForwardKinematics.erase(iter++); + anyDeleted = true; + } } { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); iter != collector_MyFactorPosePoint2.end(); ) { delete *iter; @@ -304,14 +312,21 @@ void Test_get_container_17(int nargout, mxArray *out[], int nargin, const mxArra out[0] = wrap_shared_ptr(boost::make_shared>(obj->get_container()),"std.vectorTest", false); } -void Test_print_18(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_lambda_18(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_19(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_19(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_Point2Ptr_20(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"); @@ -322,7 +337,7 @@ void Test_return_Point2Ptr_19(int nargout, mxArray *out[], int nargin, const mxA } } -void Test_return_Test_20(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_Test_21(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"); @@ -330,7 +345,7 @@ void Test_return_Test_20(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_21(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_TestPtr_22(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"); @@ -338,7 +353,7 @@ void Test_return_TestPtr_21(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap_shared_ptr(obj->return_TestPtr(value),"Test", false); } -void Test_return_bool_22(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_bool_23(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"); @@ -346,7 +361,7 @@ void Test_return_bool_22(int nargout, mxArray *out[], int nargin, const mxArray out[0] = wrap< bool >(obj->return_bool(value)); } -void Test_return_double_23(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_double_24(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"); @@ -354,7 +369,7 @@ void Test_return_double_23(int nargout, mxArray *out[], int nargin, const mxArra out[0] = wrap< double >(obj->return_double(value)); } -void Test_return_field_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_field_25(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"); @@ -362,7 +377,7 @@ void Test_return_field_24(int nargout, mxArray *out[], int nargin, const mxArray out[0] = wrap< bool >(obj->return_field(t)); } -void Test_return_int_25(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_int_26(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"); @@ -370,7 +385,7 @@ void Test_return_int_25(int nargout, mxArray *out[], int nargin, const mxArray * out[0] = wrap< int >(obj->return_int(value)); } -void Test_return_matrix1_26(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_matrix1_27(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"); @@ -378,7 +393,7 @@ void Test_return_matrix1_26(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap< Matrix >(obj->return_matrix1(value)); } -void Test_return_matrix2_27(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_matrix2_28(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"); @@ -386,7 +401,7 @@ void Test_return_matrix2_27(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap< Matrix >(obj->return_matrix2(value)); } -void Test_return_pair_28(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_pair_29(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"); @@ -397,7 +412,7 @@ void Test_return_pair_28(int nargout, mxArray *out[], int nargin, const mxArray out[1] = wrap< Matrix >(pairResult.second); } -void Test_return_pair_29(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_pair_30(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"); @@ -407,7 +422,7 @@ void Test_return_pair_29(int nargout, mxArray *out[], int nargin, const mxArray out[1] = wrap< Matrix >(pairResult.second); } -void Test_return_ptrs_30(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_ptrs_31(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"); @@ -418,7 +433,7 @@ void Test_return_ptrs_30(int nargout, mxArray *out[], int nargin, const mxArray out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } -void Test_return_size_t_31(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_size_t_32(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"); @@ -426,7 +441,7 @@ void Test_return_size_t_31(int nargout, mxArray *out[], int nargin, const mxArra out[0] = wrap< size_t >(obj->return_size_t(value)); } -void Test_return_string_32(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_string_33(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"); @@ -434,7 +449,7 @@ void Test_return_string_32(int nargout, mxArray *out[], int nargin, const mxArra out[0] = wrap< string >(obj->return_string(value)); } -void Test_return_vector1_33(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_vector1_34(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"); @@ -442,7 +457,7 @@ void Test_return_vector1_33(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap< Vector >(obj->return_vector1(value)); } -void Test_return_vector2_34(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_vector2_35(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"); @@ -450,14 +465,6 @@ void Test_return_vector2_34(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap< Vector >(obj->return_vector2(value)); } -void Test_set_container_35(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_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("set_container",nargout,nargin-1,1); @@ -474,7 +481,15 @@ void Test_set_container_37(int nargout, mxArray *out[], int nargin, const mxArra obj->set_container(*container); } -void PrimitiveRefDouble_collectorInsertAndMakeBase_38(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_set_container_38(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_39(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -483,7 +498,7 @@ void PrimitiveRefDouble_collectorInsertAndMakeBase_38(int nargout, mxArray *out[ collector_PrimitiveRefDouble.insert(self); } -void PrimitiveRefDouble_constructor_39(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void PrimitiveRefDouble_constructor_40(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -494,7 +509,7 @@ void PrimitiveRefDouble_constructor_39(int nargout, mxArray *out[], int nargin, *reinterpret_cast (mxGetData(out[0])) = self; } -void PrimitiveRefDouble_deconstructor_40(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void PrimitiveRefDouble_deconstructor_41(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_PrimitiveRefDouble",nargout,nargin,1); @@ -507,14 +522,14 @@ void PrimitiveRefDouble_deconstructor_40(int nargout, mxArray *out[], int nargin } } -void PrimitiveRefDouble_Brutal_41(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void PrimitiveRefDouble_Brutal_42(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("PrimitiveRefDouble.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_42(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector3_collectorInsertAndMakeBase_43(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -523,7 +538,7 @@ void MyVector3_collectorInsertAndMakeBase_42(int nargout, mxArray *out[], int na collector_MyVector3.insert(self); } -void MyVector3_constructor_43(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector3_constructor_44(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -534,7 +549,7 @@ void MyVector3_constructor_43(int nargout, mxArray *out[], int nargin, const mxA *reinterpret_cast (mxGetData(out[0])) = self; } -void MyVector3_deconstructor_44(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector3_deconstructor_45(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MyVector3",nargout,nargin,1); @@ -547,7 +562,7 @@ void MyVector3_deconstructor_44(int nargout, mxArray *out[], int nargin, const m } } -void MyVector12_collectorInsertAndMakeBase_45(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector12_collectorInsertAndMakeBase_46(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -556,7 +571,7 @@ void MyVector12_collectorInsertAndMakeBase_45(int nargout, mxArray *out[], int n collector_MyVector12.insert(self); } -void MyVector12_constructor_46(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector12_constructor_47(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -567,7 +582,7 @@ void MyVector12_constructor_46(int nargout, mxArray *out[], int nargin, const mx *reinterpret_cast (mxGetData(out[0])) = self; } -void MyVector12_deconstructor_47(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector12_deconstructor_48(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MyVector12",nargout,nargin,1); @@ -580,7 +595,7 @@ void MyVector12_deconstructor_47(int nargout, mxArray *out[], int nargin, const } } -void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_48(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_49(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -589,7 +604,7 @@ void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_48(int nargout, mxArr collector_MultipleTemplatesIntDouble.insert(self); } -void MultipleTemplatesIntDouble_deconstructor_49(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MultipleTemplatesIntDouble_deconstructor_50(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MultipleTemplatesIntDouble",nargout,nargin,1); @@ -602,7 +617,7 @@ void MultipleTemplatesIntDouble_deconstructor_49(int nargout, mxArray *out[], in } } -void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_50(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_51(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -611,7 +626,7 @@ void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_50(int nargout, mxArra collector_MultipleTemplatesIntFloat.insert(self); } -void MultipleTemplatesIntFloat_deconstructor_51(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MultipleTemplatesIntFloat_deconstructor_52(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MultipleTemplatesIntFloat",nargout,nargin,1); @@ -624,7 +639,45 @@ void MultipleTemplatesIntFloat_deconstructor_51(int nargout, mxArray *out[], int } } -void MyFactorPosePoint2_collectorInsertAndMakeBase_52(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void ForwardKinematics_collectorInsertAndMakeBase_53(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_ForwardKinematics.insert(self); +} + +void ForwardKinematics_constructor_54(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + gtdynamics::Robot& robot = *unwrap_shared_ptr< gtdynamics::Robot >(in[0], "ptr_gtdynamicsRobot"); + string& start_link_name = *unwrap_shared_ptr< string >(in[1], "ptr_string"); + string& end_link_name = *unwrap_shared_ptr< string >(in[2], "ptr_string"); + gtsam::Values& joint_angles = *unwrap_shared_ptr< gtsam::Values >(in[3], "ptr_gtsamValues"); + gtsam::Pose3& l2Tp = *unwrap_shared_ptr< gtsam::Pose3 >(in[4], "ptr_gtsamPose3"); + Shared *self = new Shared(new ForwardKinematics(robot,start_link_name,end_link_name,joint_angles,l2Tp)); + collector_ForwardKinematics.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void ForwardKinematics_deconstructor_55(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_ForwardKinematics",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_ForwardKinematics::iterator item; + item = collector_ForwardKinematics.find(self); + if(item != collector_ForwardKinematics.end()) { + delete self; + collector_ForwardKinematics.erase(item); + } +} + +void MyFactorPosePoint2_collectorInsertAndMakeBase_56(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -633,7 +686,7 @@ void MyFactorPosePoint2_collectorInsertAndMakeBase_52(int nargout, mxArray *out[ collector_MyFactorPosePoint2.insert(self); } -void MyFactorPosePoint2_constructor_53(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_constructor_57(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -648,7 +701,7 @@ void MyFactorPosePoint2_constructor_53(int nargout, mxArray *out[], int nargin, *reinterpret_cast (mxGetData(out[0])) = self; } -void MyFactorPosePoint2_deconstructor_54(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_deconstructor_58(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1); @@ -661,7 +714,7 @@ void MyFactorPosePoint2_deconstructor_54(int nargout, mxArray *out[], int nargin } } -void MyFactorPosePoint2_print_55(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_print_59(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,2); auto obj = unwrap_shared_ptr>(in[0], "ptr_MyFactorPosePoint2"); @@ -737,58 +790,58 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) Test_get_container_17(nargout, out, nargin-1, in+1); break; case 18: - Test_print_18(nargout, out, nargin-1, in+1); + Test_lambda_18(nargout, out, nargin-1, in+1); break; case 19: - Test_return_Point2Ptr_19(nargout, out, nargin-1, in+1); + Test_print_19(nargout, out, nargin-1, in+1); break; case 20: - Test_return_Test_20(nargout, out, nargin-1, in+1); + Test_return_Point2Ptr_20(nargout, out, nargin-1, in+1); break; case 21: - Test_return_TestPtr_21(nargout, out, nargin-1, in+1); + Test_return_Test_21(nargout, out, nargin-1, in+1); break; case 22: - Test_return_bool_22(nargout, out, nargin-1, in+1); + Test_return_TestPtr_22(nargout, out, nargin-1, in+1); break; case 23: - Test_return_double_23(nargout, out, nargin-1, in+1); + Test_return_bool_23(nargout, out, nargin-1, in+1); break; case 24: - Test_return_field_24(nargout, out, nargin-1, in+1); + Test_return_double_24(nargout, out, nargin-1, in+1); break; case 25: - Test_return_int_25(nargout, out, nargin-1, in+1); + Test_return_field_25(nargout, out, nargin-1, in+1); break; case 26: - Test_return_matrix1_26(nargout, out, nargin-1, in+1); + Test_return_int_26(nargout, out, nargin-1, in+1); break; case 27: - Test_return_matrix2_27(nargout, out, nargin-1, in+1); + Test_return_matrix1_27(nargout, out, nargin-1, in+1); break; case 28: - Test_return_pair_28(nargout, out, nargin-1, in+1); + Test_return_matrix2_28(nargout, out, nargin-1, in+1); break; case 29: Test_return_pair_29(nargout, out, nargin-1, in+1); break; case 30: - Test_return_ptrs_30(nargout, out, nargin-1, in+1); + Test_return_pair_30(nargout, out, nargin-1, in+1); break; case 31: - Test_return_size_t_31(nargout, out, nargin-1, in+1); + Test_return_ptrs_31(nargout, out, nargin-1, in+1); break; case 32: - Test_return_string_32(nargout, out, nargin-1, in+1); + Test_return_size_t_32(nargout, out, nargin-1, in+1); break; case 33: - Test_return_vector1_33(nargout, out, nargin-1, in+1); + Test_return_string_33(nargout, out, nargin-1, in+1); break; case 34: - Test_return_vector2_34(nargout, out, nargin-1, in+1); + Test_return_vector1_34(nargout, out, nargin-1, in+1); break; case 35: - Test_set_container_35(nargout, out, nargin-1, in+1); + Test_return_vector2_35(nargout, out, nargin-1, in+1); break; case 36: Test_set_container_36(nargout, out, nargin-1, in+1); @@ -797,58 +850,70 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) Test_set_container_37(nargout, out, nargin-1, in+1); break; case 38: - PrimitiveRefDouble_collectorInsertAndMakeBase_38(nargout, out, nargin-1, in+1); + Test_set_container_38(nargout, out, nargin-1, in+1); break; case 39: - PrimitiveRefDouble_constructor_39(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_collectorInsertAndMakeBase_39(nargout, out, nargin-1, in+1); break; case 40: - PrimitiveRefDouble_deconstructor_40(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_constructor_40(nargout, out, nargin-1, in+1); break; case 41: - PrimitiveRefDouble_Brutal_41(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_deconstructor_41(nargout, out, nargin-1, in+1); break; case 42: - MyVector3_collectorInsertAndMakeBase_42(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_Brutal_42(nargout, out, nargin-1, in+1); break; case 43: - MyVector3_constructor_43(nargout, out, nargin-1, in+1); + MyVector3_collectorInsertAndMakeBase_43(nargout, out, nargin-1, in+1); break; case 44: - MyVector3_deconstructor_44(nargout, out, nargin-1, in+1); + MyVector3_constructor_44(nargout, out, nargin-1, in+1); break; case 45: - MyVector12_collectorInsertAndMakeBase_45(nargout, out, nargin-1, in+1); + MyVector3_deconstructor_45(nargout, out, nargin-1, in+1); break; case 46: - MyVector12_constructor_46(nargout, out, nargin-1, in+1); + MyVector12_collectorInsertAndMakeBase_46(nargout, out, nargin-1, in+1); break; case 47: - MyVector12_deconstructor_47(nargout, out, nargin-1, in+1); + MyVector12_constructor_47(nargout, out, nargin-1, in+1); break; case 48: - MultipleTemplatesIntDouble_collectorInsertAndMakeBase_48(nargout, out, nargin-1, in+1); + MyVector12_deconstructor_48(nargout, out, nargin-1, in+1); break; case 49: - MultipleTemplatesIntDouble_deconstructor_49(nargout, out, nargin-1, in+1); + MultipleTemplatesIntDouble_collectorInsertAndMakeBase_49(nargout, out, nargin-1, in+1); break; case 50: - MultipleTemplatesIntFloat_collectorInsertAndMakeBase_50(nargout, out, nargin-1, in+1); + MultipleTemplatesIntDouble_deconstructor_50(nargout, out, nargin-1, in+1); break; case 51: - MultipleTemplatesIntFloat_deconstructor_51(nargout, out, nargin-1, in+1); + MultipleTemplatesIntFloat_collectorInsertAndMakeBase_51(nargout, out, nargin-1, in+1); break; case 52: - MyFactorPosePoint2_collectorInsertAndMakeBase_52(nargout, out, nargin-1, in+1); + MultipleTemplatesIntFloat_deconstructor_52(nargout, out, nargin-1, in+1); break; case 53: - MyFactorPosePoint2_constructor_53(nargout, out, nargin-1, in+1); + ForwardKinematics_collectorInsertAndMakeBase_53(nargout, out, nargin-1, in+1); break; case 54: - MyFactorPosePoint2_deconstructor_54(nargout, out, nargin-1, in+1); + ForwardKinematics_constructor_54(nargout, out, nargin-1, in+1); break; case 55: - MyFactorPosePoint2_print_55(nargout, out, nargin-1, in+1); + ForwardKinematics_deconstructor_55(nargout, out, nargin-1, in+1); + break; + case 56: + MyFactorPosePoint2_collectorInsertAndMakeBase_56(nargout, out, nargin-1, in+1); + break; + case 57: + MyFactorPosePoint2_constructor_57(nargout, out, nargin-1, in+1); + break; + case 58: + MyFactorPosePoint2_deconstructor_58(nargout, out, nargin-1, in+1); + break; + case 59: + MyFactorPosePoint2_print_59(nargout, out, nargin-1, in+1); break; } } catch(const std::exception& e) { diff --git a/wrap/tests/expected/matlab/functions_wrapper.cpp b/wrap/tests/expected/matlab/functions_wrapper.cpp index 536733bdc..ae7f49c41 100644 --- a/wrap/tests/expected/matlab/functions_wrapper.cpp +++ b/wrap/tests/expected/matlab/functions_wrapper.cpp @@ -33,6 +33,8 @@ typedef std::set*> Collector_Multi static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; typedef std::set*> Collector_MultipleTemplatesIntFloat; static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; +typedef std::set*> Collector_ForwardKinematics; +static Collector_ForwardKinematics collector_ForwardKinematics; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; @@ -90,6 +92,12 @@ void _deleteAllObjects() collector_MultipleTemplatesIntFloat.erase(iter++); anyDeleted = true; } } + { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); + iter != collector_ForwardKinematics.end(); ) { + delete *iter; + collector_ForwardKinematics.erase(iter++); + anyDeleted = true; + } } { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); iter != collector_MyFactorPosePoint2.end(); ) { delete *iter; @@ -198,9 +206,10 @@ void MultiTemplatedFunctionDoubleSize_tDouble_7(int nargout, mxArray *out[], int } void DefaultFuncInt_8(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("DefaultFuncInt",nargout,nargin,1); + checkArguments("DefaultFuncInt",nargout,nargin,2); int a = unwrap< int >(in[0]); - DefaultFuncInt(a); + int b = unwrap< int >(in[1]); + DefaultFuncInt(a,b); } void DefaultFuncString_9(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { @@ -215,7 +224,30 @@ void DefaultFuncObj_10(int nargout, mxArray *out[], int nargin, const mxArray *i gtsam::KeyFormatter& keyFormatter = *unwrap_shared_ptr< gtsam::KeyFormatter >(in[0], "ptr_gtsamKeyFormatter"); DefaultFuncObj(keyFormatter); } -void TemplatedFunctionRot3_11(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void DefaultFuncZero_11(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("DefaultFuncZero",nargout,nargin,5); + int a = unwrap< int >(in[0]); + int b = unwrap< int >(in[1]); + double c = unwrap< double >(in[2]); + bool d = unwrap< bool >(in[3]); + bool e = unwrap< bool >(in[4]); + DefaultFuncZero(a,b,c,d,e); +} +void DefaultFuncVector_12(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("DefaultFuncVector",nargout,nargin,2); + std::vector& i = *unwrap_shared_ptr< std::vector >(in[0], "ptr_stdvectorint"); + std::vector& s = *unwrap_shared_ptr< std::vector >(in[1], "ptr_stdvectorstring"); + DefaultFuncVector(i,s); +} +void setPose_13(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("setPose",nargout,nargin,1); + gtsam::Pose3& pose = *unwrap_shared_ptr< gtsam::Pose3 >(in[0], "ptr_gtsamPose3"); + setPose(pose); +} +void TemplatedFunctionRot3_14(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("TemplatedFunctionRot3",nargout,nargin,1); gtsam::Rot3& t = *unwrap_shared_ptr< gtsam::Rot3 >(in[0], "ptr_gtsamRot3"); @@ -267,7 +299,16 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) DefaultFuncObj_10(nargout, out, nargin-1, in+1); break; case 11: - TemplatedFunctionRot3_11(nargout, out, nargin-1, in+1); + DefaultFuncZero_11(nargout, out, nargin-1, in+1); + break; + case 12: + DefaultFuncVector_12(nargout, out, nargin-1, in+1); + break; + case 13: + setPose_13(nargout, out, nargin-1, in+1); + break; + case 14: + TemplatedFunctionRot3_14(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 766c64bb3..4d8a7c789 100644 --- a/wrap/tests/expected/matlab/geometry_wrapper.cpp +++ b/wrap/tests/expected/matlab/geometry_wrapper.cpp @@ -36,6 +36,8 @@ typedef std::set*> Collector_Multi static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; typedef std::set*> Collector_MultipleTemplatesIntFloat; static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; +typedef std::set*> Collector_ForwardKinematics; +static Collector_ForwardKinematics collector_ForwardKinematics; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; typedef std::set*> Collector_gtsamPoint2; @@ -97,6 +99,12 @@ void _deleteAllObjects() collector_MultipleTemplatesIntFloat.erase(iter++); anyDeleted = true; } } + { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); + iter != collector_ForwardKinematics.end(); ) { + delete *iter; + collector_ForwardKinematics.erase(iter++); + anyDeleted = true; + } } { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); iter != collector_MyFactorPosePoint2.end(); ) { delete *iter; diff --git a/wrap/tests/expected/matlab/inheritance_wrapper.cpp b/wrap/tests/expected/matlab/inheritance_wrapper.cpp index e68b3a6e4..077df4830 100644 --- a/wrap/tests/expected/matlab/inheritance_wrapper.cpp +++ b/wrap/tests/expected/matlab/inheritance_wrapper.cpp @@ -38,6 +38,8 @@ typedef std::set*> Collector_Multi static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; typedef std::set*> Collector_MultipleTemplatesIntFloat; static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; +typedef std::set*> Collector_ForwardKinematics; +static Collector_ForwardKinematics collector_ForwardKinematics; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; typedef std::set*> Collector_gtsamPoint2; @@ -107,6 +109,12 @@ void _deleteAllObjects() collector_MultipleTemplatesIntFloat.erase(iter++); anyDeleted = true; } } + { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); + iter != collector_ForwardKinematics.end(); ) { + delete *iter; + collector_ForwardKinematics.erase(iter++); + anyDeleted = true; + } } { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); iter != collector_MyFactorPosePoint2.end(); ) { delete *iter; @@ -564,12 +572,12 @@ void MyTemplateMatrix_Level_34(int nargout, mxArray *out[], int nargin, const mx out[0] = wrap_shared_ptr(boost::make_shared>(MyTemplate::Level(K)),"MyTemplateMatrix", false); } -void Test_set_container_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_vector2_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("set_container",nargout,nargin-1,1); + checkArguments("return_vector2",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); + Vector value = unwrap< Vector >(in[1]); + out[0] = wrap< Vector >(obj->return_vector2(value)); } void ForwardKinematicsFactor_collectorInsertAndMakeBase_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) @@ -716,7 +724,7 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) MyTemplateMatrix_Level_34(nargout, out, nargin-1, in+1); break; case 35: - Test_set_container_35(nargout, out, nargin-1, in+1); + Test_return_vector2_35(nargout, out, nargin-1, in+1); break; case 36: ForwardKinematicsFactor_collectorInsertAndMakeBase_35(nargout, out, nargin-1, in+1); diff --git a/wrap/tests/expected/matlab/namespaces_wrapper.cpp b/wrap/tests/expected/matlab/namespaces_wrapper.cpp index aba5c49ea..8f6e415e2 100644 --- a/wrap/tests/expected/matlab/namespaces_wrapper.cpp +++ b/wrap/tests/expected/matlab/namespaces_wrapper.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -43,6 +44,8 @@ typedef std::set*> Collector_Multi static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; typedef std::set*> Collector_MultipleTemplatesIntFloat; static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; +typedef std::set*> Collector_ForwardKinematics; +static Collector_ForwardKinematics collector_ForwardKinematics; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; typedef std::set*> Collector_gtsamPoint2; @@ -69,6 +72,8 @@ typedef std::set*> Collector_ns2ClassC; static Collector_ns2ClassC collector_ns2ClassC; typedef std::set*> Collector_ClassD; static Collector_ClassD collector_ClassD; +typedef std::set*> Collector_gtsamValues; +static Collector_gtsamValues collector_gtsamValues; void _deleteAllObjects() { @@ -124,6 +129,12 @@ void _deleteAllObjects() collector_MultipleTemplatesIntFloat.erase(iter++); anyDeleted = true; } } + { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); + iter != collector_ForwardKinematics.end(); ) { + delete *iter; + collector_ForwardKinematics.erase(iter++); + anyDeleted = true; + } } { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); iter != collector_MyFactorPosePoint2.end(); ) { delete *iter; @@ -202,6 +213,12 @@ void _deleteAllObjects() collector_ClassD.erase(iter++); anyDeleted = true; } } + { for(Collector_gtsamValues::iterator iter = collector_gtsamValues.begin(); + iter != collector_gtsamValues.end(); ) { + delete *iter; + collector_gtsamValues.erase(iter++); + anyDeleted = true; + } } if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -491,6 +508,69 @@ void ClassD_deconstructor_25(int nargout, mxArray *out[], int nargin, const mxAr } } +void gtsamValues_collectorInsertAndMakeBase_26(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_gtsamValues.insert(self); +} + +void gtsamValues_constructor_27(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new gtsam::Values()); + collector_gtsamValues.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void gtsamValues_constructor_28(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + gtsam::Values& other = *unwrap_shared_ptr< gtsam::Values >(in[0], "ptr_gtsamValues"); + Shared *self = new Shared(new gtsam::Values(other)); + collector_gtsamValues.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void gtsamValues_deconstructor_29(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_gtsamValues",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_gtsamValues::iterator item; + item = collector_gtsamValues.find(self); + if(item != collector_gtsamValues.end()) { + delete self; + collector_gtsamValues.erase(item); + } +} + +void gtsamValues_insert_30(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("insert",nargout,nargin-1,2); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamValues"); + size_t j = unwrap< size_t >(in[1]); + Vector vector = unwrap< Vector >(in[2]); + obj->insert(j,vector); +} + +void gtsamValues_insert_31(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("insert",nargout,nargin-1,2); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamValues"); + size_t j = unwrap< size_t >(in[1]); + Matrix matrix = unwrap< Matrix >(in[2]); + obj->insert(j,matrix); +} + void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { @@ -581,6 +661,24 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) case 25: ClassD_deconstructor_25(nargout, out, nargin-1, in+1); break; + case 26: + gtsamValues_collectorInsertAndMakeBase_26(nargout, out, nargin-1, in+1); + break; + case 27: + gtsamValues_constructor_27(nargout, out, nargin-1, in+1); + break; + case 28: + gtsamValues_constructor_28(nargout, out, nargin-1, in+1); + break; + case 29: + gtsamValues_deconstructor_29(nargout, out, nargin-1, in+1); + break; + case 30: + gtsamValues_insert_30(nargout, out, nargin-1, in+1); + break; + case 31: + gtsamValues_insert_31(nargout, out, nargin-1, in+1); + break; } } catch(const std::exception& e) { mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str()); diff --git a/wrap/tests/expected/matlab/special_cases_wrapper.cpp b/wrap/tests/expected/matlab/special_cases_wrapper.cpp index d79a41ace..056ce8097 100644 --- a/wrap/tests/expected/matlab/special_cases_wrapper.cpp +++ b/wrap/tests/expected/matlab/special_cases_wrapper.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -46,6 +47,8 @@ typedef std::set*> Collector_Multi static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; typedef std::set*> Collector_MultipleTemplatesIntFloat; static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; +typedef std::set*> Collector_ForwardKinematics; +static Collector_ForwardKinematics collector_ForwardKinematics; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; typedef std::set*> Collector_gtsamPoint2; @@ -72,6 +75,8 @@ typedef std::set*> Collector_ns2ClassC; static Collector_ns2ClassC collector_ns2ClassC; typedef std::set*> Collector_ClassD; static Collector_ClassD collector_ClassD; +typedef std::set*> Collector_gtsamValues; +static Collector_gtsamValues collector_gtsamValues; typedef std::set*> Collector_gtsamNonlinearFactorGraph; static Collector_gtsamNonlinearFactorGraph collector_gtsamNonlinearFactorGraph; typedef std::set*> Collector_gtsamSfmTrack; @@ -135,6 +140,12 @@ void _deleteAllObjects() collector_MultipleTemplatesIntFloat.erase(iter++); anyDeleted = true; } } + { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); + iter != collector_ForwardKinematics.end(); ) { + delete *iter; + collector_ForwardKinematics.erase(iter++); + anyDeleted = true; + } } { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); iter != collector_MyFactorPosePoint2.end(); ) { delete *iter; @@ -213,6 +224,12 @@ void _deleteAllObjects() collector_ClassD.erase(iter++); anyDeleted = true; } } + { for(Collector_gtsamValues::iterator iter = collector_gtsamValues.begin(); + iter != collector_gtsamValues.end(); ) { + delete *iter; + collector_gtsamValues.erase(iter++); + anyDeleted = true; + } } { for(Collector_gtsamNonlinearFactorGraph::iterator iter = collector_gtsamNonlinearFactorGraph.begin(); iter != collector_gtsamNonlinearFactorGraph.end(); ) { delete *iter; diff --git a/wrap/tests/expected/python/class_pybind.cpp b/wrap/tests/expected/python/class_pybind.cpp index 961daeffe..4c2371a42 100644 --- a/wrap/tests/expected/python/class_pybind.cpp +++ b/wrap/tests/expected/python/class_pybind.cpp @@ -55,13 +55,14 @@ PYBIND11_MODULE(class_py, m_) { .def("create_ptrs",[](Test* self){return self->create_ptrs();}) .def("create_MixedPtrs",[](Test* self){return self->create_MixedPtrs();}) .def("return_ptrs",[](Test* self, std::shared_ptr p1, std::shared_ptr p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2")) - .def("print_",[](Test* self){ py::scoped_ostream_redirect output; self->print();}) + .def("print",[](Test* self){ py::scoped_ostream_redirect output; self->print();}) .def("__repr__", [](const Test& self){ gtsam::RedirectCout redirect; self.print(); return redirect.str(); }) + .def("lambda_",[](Test* self){ self->lambda();}) .def("set_container",[](Test* self, std::vector container){ self->set_container(container);}, py::arg("container")) .def("set_container",[](Test* self, std::vector> container){ self->set_container(container);}, py::arg("container")) .def("set_container",[](Test* self, std::vector container){ self->set_container(container);}, py::arg("container")) @@ -82,9 +83,12 @@ PYBIND11_MODULE(class_py, m_) { py::class_, std::shared_ptr>>(m_, "MultipleTemplatesIntFloat"); + py::class_>(m_, "ForwardKinematics") + .def(py::init(), py::arg("robot"), py::arg("start_link_name"), py::arg("end_link_name"), py::arg("joint_angles"), py::arg("l2Tp") = gtsam::Pose3()); + py::class_, std::shared_ptr>>(m_, "MyFactorPosePoint2") .def(py::init>(), py::arg("key1"), py::arg("key2"), py::arg("measured"), py::arg("noiseModel")) - .def("print_",[](MyFactor* self, const string& s, const gtsam::KeyFormatter& keyFormatter){ py::scoped_ostream_redirect output; self->print(s, keyFormatter);}, py::arg("s") = "factor: ", py::arg("keyFormatter") = gtsam::DefaultKeyFormatter) + .def("print",[](MyFactor* self, const string& s, const gtsam::KeyFormatter& keyFormatter){ py::scoped_ostream_redirect output; self->print(s, keyFormatter);}, py::arg("s") = "factor: ", py::arg("keyFormatter") = gtsam::DefaultKeyFormatter) .def("__repr__", [](const MyFactor& self, const string& s, const gtsam::KeyFormatter& keyFormatter){ gtsam::RedirectCout redirect; @@ -92,6 +96,7 @@ PYBIND11_MODULE(class_py, m_) { return redirect.str(); }, py::arg("s") = "factor: ", py::arg("keyFormatter") = gtsam::DefaultKeyFormatter); + py::class_, std::shared_ptr>>(m_, "SuperCoolFactorPose3") #include "python/specializations.h" diff --git a/wrap/tests/expected/python/functions_pybind.cpp b/wrap/tests/expected/python/functions_pybind.cpp index 47c540bc0..bee95ec03 100644 --- a/wrap/tests/expected/python/functions_pybind.cpp +++ b/wrap/tests/expected/python/functions_pybind.cpp @@ -30,9 +30,12 @@ PYBIND11_MODULE(functions_py, m_) { m_.def("overloadedGlobalFunction",[](int a, double b){return ::overloadedGlobalFunction(a, b);}, py::arg("a"), py::arg("b")); m_.def("MultiTemplatedFunctionStringSize_tDouble",[](const T& x, size_t y){return ::MultiTemplatedFunction(x, y);}, py::arg("x"), py::arg("y")); m_.def("MultiTemplatedFunctionDoubleSize_tDouble",[](const T& x, size_t y){return ::MultiTemplatedFunction(x, y);}, py::arg("x"), py::arg("y")); - m_.def("DefaultFuncInt",[](int a){ ::DefaultFuncInt(a);}, py::arg("a") = 123); + m_.def("DefaultFuncInt",[](int a, int b){ ::DefaultFuncInt(a, b);}, py::arg("a") = 123, py::arg("b") = 0); m_.def("DefaultFuncString",[](const string& s, const string& name){ ::DefaultFuncString(s, name);}, py::arg("s") = "hello", py::arg("name") = ""); m_.def("DefaultFuncObj",[](const gtsam::KeyFormatter& keyFormatter){ ::DefaultFuncObj(keyFormatter);}, py::arg("keyFormatter") = gtsam::DefaultKeyFormatter); + m_.def("DefaultFuncZero",[](int a, int b, double c, bool d, bool e){ ::DefaultFuncZero(a, b, c, d, e);}, py::arg("a") = 0, py::arg("b"), py::arg("c") = 0.0, py::arg("d") = false, py::arg("e")); + m_.def("DefaultFuncVector",[](const std::vector& i, const std::vector& s){ ::DefaultFuncVector(i, s);}, py::arg("i") = {1, 2, 3}, py::arg("s") = {"borglab", "gtsam"}); + m_.def("setPose",[](const gtsam::Pose3& pose){ ::setPose(pose);}, py::arg("pose") = gtsam::Pose3()); m_.def("TemplatedFunctionRot3",[](const gtsam::Rot3& t){ ::TemplatedFunction(t);}, py::arg("t")); #include "python/specializations.h" diff --git a/wrap/tests/expected/python/namespaces_pybind.cpp b/wrap/tests/expected/python/namespaces_pybind.cpp index b09fe36eb..53e9d186a 100644 --- a/wrap/tests/expected/python/namespaces_pybind.cpp +++ b/wrap/tests/expected/python/namespaces_pybind.cpp @@ -11,6 +11,7 @@ #include "path/to/ns2.h" #include "path/to/ns2/ClassA.h" #include "path/to/ns3.h" +#include "gtsam/nonlinear/Values.h" #include "wrap/serialization.h" #include @@ -57,7 +58,16 @@ PYBIND11_MODULE(namespaces_py, m_) { py::class_>(m_, "ClassD") .def(py::init<>()); - m_.attr("aGlobalVar") = aGlobalVar; + m_.attr("aGlobalVar") = aGlobalVar; pybind11::module m_gtsam = m_.def_submodule("gtsam", "gtsam submodule"); + + py::class_>(m_gtsam, "Values") + .def(py::init<>()) + .def(py::init(), py::arg("other")) + .def("insert_vector",[](gtsam::Values* self, size_t j, const gtsam::Vector& vector){ self->insert(j, vector);}, py::arg("j"), py::arg("vector")) + .def("insert",[](gtsam::Values* self, size_t j, const gtsam::Vector& vector){ self->insert(j, vector);}, py::arg("j"), py::arg("vector")) + .def("insert_matrix",[](gtsam::Values* self, size_t j, const gtsam::Matrix& matrix){ self->insert(j, matrix);}, py::arg("j"), py::arg("matrix")) + .def("insert",[](gtsam::Values* self, size_t j, const gtsam::Matrix& matrix){ self->insert(j, matrix);}, py::arg("j"), py::arg("matrix")); + #include "python/specializations.h" diff --git a/wrap/tests/fixtures/class.i b/wrap/tests/fixtures/class.i index f49725ffa..9e30b17b5 100644 --- a/wrap/tests/fixtures/class.i +++ b/wrap/tests/fixtures/class.i @@ -61,7 +61,10 @@ class Test { pair create_MixedPtrs () const; pair return_ptrs (Test* p1, Test* p2) const; + // This should be callable as .print() in python void print() const; + // Since this is a reserved keyword, it should be updated to `lambda_` + void lambda() const; void set_container(std::vector container); void set_container(std::vector container); @@ -106,3 +109,14 @@ class MyVector { // Class with multiple instantiated templates template class MultipleTemplates {}; + +// Test for default args in constructor +class ForwardKinematics { + ForwardKinematics(const gtdynamics::Robot& robot, + const string& start_link_name, const string& end_link_name, + const gtsam::Values& joint_angles, + const gtsam::Pose3& l2Tp = gtsam::Pose3()); +}; + +class SuperCoolFactor; +typedef SuperCoolFactor SuperCoolFactorPose3; diff --git a/wrap/tests/fixtures/functions.i b/wrap/tests/fixtures/functions.i index 298028691..0852a3e1e 100644 --- a/wrap/tests/fixtures/functions.i +++ b/wrap/tests/fixtures/functions.i @@ -28,6 +28,11 @@ void TemplatedFunction(const T& t); typedef TemplatedFunction TemplatedFunctionRot3; // Check default arguments -void DefaultFuncInt(int a = 123); +void DefaultFuncInt(int a = 123, int b = 0); void DefaultFuncString(const string& s = "hello", const string& name = ""); void DefaultFuncObj(const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); +void DefaultFuncZero(int a = 0, int b, double c = 0.0, bool d = false, bool e); +void DefaultFuncVector(const std::vector &i = {1, 2, 3}, const std::vector &s = {"borglab", "gtsam"}); + +// Test for non-trivial default constructor +void setPose(const gtsam::Pose3& pose = gtsam::Pose3()); diff --git a/wrap/tests/fixtures/namespaces.i b/wrap/tests/fixtures/namespaces.i index 5c277801d..871087a37 100644 --- a/wrap/tests/fixtures/namespaces.i +++ b/wrap/tests/fixtures/namespaces.i @@ -60,3 +60,14 @@ class ClassD { }; int aGlobalVar; + +namespace gtsam { + #include +class Values { + Values(); + Values(const gtsam::Values& other); + + void insert(size_t j, Vector vector); + void insert(size_t j, Matrix matrix); +}; +} \ No newline at end of file diff --git a/wrap/tests/test_docs.py b/wrap/tests/test_docs.py index 622d6d14f..ca8cdbdde 100644 --- a/wrap/tests/test_docs.py +++ b/wrap/tests/test_docs.py @@ -41,7 +41,6 @@ class TestDocument(unittest.TestCase): OUTPUT_XML_DIR_PATH = path.abspath(path.join(DIR_NAME, OUTPUT_XML_DIR)) EXPECTED_XML_DIR_PATH = path.abspath(path.join(DIR_NAME, EXPECTED_XML_DIR)) - @unittest.skip("DOC_DIR_PATH doesn't exist") def test_generate_xml(self): """Test parse_xml.generate_xml""" if path.exists(self.OUTPUT_XML_DIR_PATH): @@ -65,7 +64,6 @@ class TestDocument(unittest.TestCase): self.assertTrue(not dircmp.diff_files and not dircmp.funny_files) - @unittest.skip("DOC_DIR_PATH doesn't exist") def test_parse(self): """Test the parsing of the XML generated by Doxygen.""" docs = parser.ParseDoxygenXML(self.DOC_DIR_PATH, diff --git a/wrap/tests/test_interface_parser.py b/wrap/tests/test_interface_parser.py index 70f044f04..95987f42f 100644 --- a/wrap/tests/test_interface_parser.py +++ b/wrap/tests/test_interface_parser.py @@ -142,9 +142,9 @@ class TestInterfaceParser(unittest.TestCase): "const C6* c6" args = ArgumentList.rule.parseString(arg_string)[0] - self.assertEqual(7, len(args.args_list)) + self.assertEqual(7, len(args.list())) self.assertEqual(['a', 'c1', 'c2', 'c3', 'c4', 'c5', 'c6'], - args.args_names()) + args.names()) def test_argument_list_qualifiers(self): """ @@ -153,7 +153,7 @@ class TestInterfaceParser(unittest.TestCase): """ arg_string = "double x1, double* x2, double& x3, double@ x4, " \ "const double x5, const double* x6, const double& x7, const double@ x8" - args = ArgumentList.rule.parseString(arg_string)[0].args_list + args = ArgumentList.rule.parseString(arg_string)[0].list() self.assertEqual(8, len(args)) self.assertFalse(args[1].ctype.is_ptr and args[1].ctype.is_shared_ptr and args[1].ctype.is_ref) @@ -169,7 +169,7 @@ class TestInterfaceParser(unittest.TestCase): """Test arguments list where the arguments can be templated.""" arg_string = "std::pair steps, vector vector_of_pointers" args = ArgumentList.rule.parseString(arg_string)[0] - args_list = args.args_list + args_list = args.list() self.assertEqual(2, len(args_list)) self.assertEqual("std::pair", args_list[0].ctype.to_cpp(False)) @@ -180,30 +180,62 @@ class TestInterfaceParser(unittest.TestCase): def test_default_arguments(self): """Tests any expression that is a valid default argument""" - args = ArgumentList.rule.parseString( - "string c = \"\", string s=\"hello\", int a=3, " - "int b, double pi = 3.1415, " - "gtsam::KeyFormatter kf = gtsam::DefaultKeyFormatter, " - "std::vector p = std::vector(), " - "std::vector l = (1, 2, 'name', \"random\", 3.1415)" - )[0].args_list + args = ArgumentList.rule.parseString(""" + string c = "", int z = 0, double z2 = 0.0, bool f = false, + string s="hello"+"goodbye", char c='a', int a=3, + int b, double pi = 3.1415""")[0].list() # Test for basic types - self.assertEqual(args[0].default, "") - self.assertEqual(args[1].default, "hello") - self.assertEqual(args[2].default, 3) + self.assertEqual(args[0].default, '""') + self.assertEqual(args[1].default, '0') + self.assertEqual(args[2].default, '0.0') + self.assertEqual(args[3].default, "false") + self.assertEqual(args[4].default, '"hello"+"goodbye"') + self.assertEqual(args[5].default, "'a'") + self.assertEqual(args[6].default, '3') # No default argument should set `default` to None - self.assertIsNone(args[3].default) + self.assertIsNone(args[7].default) + self.assertEqual(args[8].default, '3.1415') - self.assertEqual(args[4].default, 3.1415) + arg0 = 'gtsam::DefaultKeyFormatter' + arg1 = 'std::vector()' + arg2 = '{1, 2}' + arg3 = '[&c1, &c2](string s=5, int a){return s+"hello"+a+c1+c2;}' + arg4 = 'gtsam::Pose3()' + arg5 = 'Factor()' + arg6 = 'gtsam::Point3(1, 2, 3)' + arg7 = 'ns::Class(3, 2, 1, "name")' + + argument_list = """ + gtsam::KeyFormatter kf = {arg0}, + std::vector v = {arg1}, + std::vector l = {arg2}, + gtsam::KeyFormatter lambda = {arg3}, + gtsam::Pose3 p = {arg4}, + Factor x = {arg5}, + gtsam::Point3 x = {arg6}, + ns::Class obj = {arg7} + """.format(arg0=arg0, + arg1=arg1, + arg2=arg2, + arg3=arg3, + arg4=arg4, + arg5=arg5, + arg6=arg6, + arg7=arg7) + args = ArgumentList.rule.parseString(argument_list)[0].list() # Test non-basic type - self.assertEqual(repr(args[5].default.typename), - 'gtsam::DefaultKeyFormatter') + self.assertEqual(args[0].default, arg0) # Test templated type - self.assertEqual(repr(args[6].default.typename), 'std::vector') - # Test for allowing list as default argument - self.assertEqual(args[7].default, (1, 2, 'name', "random", 3.1415)) + self.assertEqual(args[1].default, arg1) + self.assertEqual(args[2].default, arg2) + self.assertEqual(args[3].default, arg3) + self.assertEqual(args[4].default, arg4) + self.assertEqual(args[5].default, arg5) + self.assertEqual(args[6].default, arg6) + # Test for default argument with multiple templates and params + self.assertEqual(args[7].default, arg7) def test_return_type(self): """Test ReturnType""" @@ -273,6 +305,15 @@ class TestInterfaceParser(unittest.TestCase): self.assertEqual("f", ret.name) self.assertEqual(3, len(ret.args)) + ret = Constructor.rule.parseString( + """ForwardKinematics(const gtdynamics::Robot& robot, + const string& start_link_name, const string& end_link_name, + const gtsam::Values& joint_angles, + const gtsam::Pose3& l2Tp = gtsam::Pose3());""")[0] + self.assertEqual("ForwardKinematics", ret.name) + self.assertEqual(5, len(ret.args)) + self.assertEqual("gtsam::Pose3()", ret.args.list()[4].default) + def test_operator_overload(self): """Test for operator overloading.""" # Unary operator @@ -296,7 +337,7 @@ class TestInterfaceParser(unittest.TestCase): ret.return_type.type1.typename.to_cpp()) self.assertTrue(len(ret.args) == 1) self.assertEqual("const gtsam::Vector2 &", - repr(ret.args.args_list[0].ctype)) + repr(ret.args.list()[0].ctype)) self.assertTrue(not ret.is_unary) def test_typedef_template_instantiation(self): @@ -392,6 +433,16 @@ class TestInterfaceParser(unittest.TestCase): self.assertEqual(0, len(ret.properties)) self.assertTrue(not ret.is_virtual) + def test_templated_class(self): + """Test a templated class.""" + ret = Class.rule.parseString(""" + template + class MyFactor {}; + """)[0] + + self.assertEqual("MyFactor", ret.name) + self.assertEqual("", repr(ret.template)) + def test_class_inheritance(self): """Test for class inheritance.""" ret = Class.rule.parseString(""" @@ -446,8 +497,7 @@ class TestInterfaceParser(unittest.TestCase): fwd = ForwardDeclaration.rule.parseString( "virtual class Test:gtsam::Point3;")[0] - fwd_name = fwd.name - self.assertEqual("Test", fwd_name.name) + self.assertEqual("Test", fwd.name) self.assertTrue(fwd.is_virtual) def test_function(self): @@ -469,14 +519,26 @@ class TestInterfaceParser(unittest.TestCase): variable = Variable.rule.parseString("string kGravity = 9.81;")[0] self.assertEqual(variable.name, "kGravity") self.assertEqual(variable.ctype.typename.name, "string") - self.assertEqual(variable.default, 9.81) + self.assertEqual(variable.default, "9.81") variable = Variable.rule.parseString( "const string kGravity = 9.81;")[0] self.assertEqual(variable.name, "kGravity") self.assertEqual(variable.ctype.typename.name, "string") self.assertTrue(variable.ctype.is_const) - self.assertEqual(variable.default, 9.81) + self.assertEqual(variable.default, "9.81") + + variable = Variable.rule.parseString( + "gtsam::Pose3 wTc = gtsam::Pose3();")[0] + self.assertEqual(variable.name, "wTc") + self.assertEqual(variable.ctype.typename.name, "Pose3") + self.assertEqual(variable.default, "gtsam::Pose3()") + + variable = Variable.rule.parseString( + "gtsam::Pose3 wTc = gtsam::Pose3(1, 2, 0);")[0] + self.assertEqual(variable.name, "wTc") + self.assertEqual(variable.ctype.typename.name, "Pose3") + self.assertEqual(variable.default, "gtsam::Pose3(1, 2, 0)") def test_enumerator(self): """Test for enumerator.""" diff --git a/wrap/tests/test_matlab_wrapper.py b/wrap/tests/test_matlab_wrapper.py index 02d40cb45..b321c4e15 100644 --- a/wrap/tests/test_matlab_wrapper.py +++ b/wrap/tests/test_matlab_wrapper.py @@ -15,8 +15,6 @@ from loguru import logger sys.path.append(osp.dirname(osp.dirname(osp.abspath(__file__)))) -import gtwrap.interface_parser as parser -import gtwrap.template_instantiator as instantiator from gtwrap.matlab_wrapper import MatlabWrapper @@ -117,19 +115,14 @@ class TestWrap(unittest.TestCase): if not osp.exists(self.MATLAB_ACTUAL_DIR): os.mkdir(self.MATLAB_ACTUAL_DIR) - module = parser.Module.parseString(content) - - instantiator.instantiate_namespace_inplace(module) - # Create MATLAB wrapper instance wrapper = MatlabWrapper( - module=module, module_name='geometry', top_module_namespace=['gtsam'], ignore_classes=[''], ) - cc_content = wrapper.wrap() + cc_content = wrapper.wrap(content) self.generate_content(cc_content) @@ -148,18 +141,13 @@ class TestWrap(unittest.TestCase): if not osp.exists(self.MATLAB_ACTUAL_DIR): os.mkdir(self.MATLAB_ACTUAL_DIR) - module = parser.Module.parseString(content) - - instantiator.instantiate_namespace_inplace(module) - wrapper = MatlabWrapper( - module=module, module_name='functions', top_module_namespace=['gtsam'], ignore_classes=[''], ) - cc_content = wrapper.wrap() + cc_content = wrapper.wrap(content) self.generate_content(cc_content) @@ -181,18 +169,13 @@ class TestWrap(unittest.TestCase): if not osp.exists(self.MATLAB_ACTUAL_DIR): os.mkdir(self.MATLAB_ACTUAL_DIR) - module = parser.Module.parseString(content) - - instantiator.instantiate_namespace_inplace(module) - wrapper = MatlabWrapper( - module=module, module_name='class', top_module_namespace=['gtsam'], ignore_classes=[''], ) - cc_content = wrapper.wrap() + cc_content = wrapper.wrap(content) self.generate_content(cc_content) @@ -214,18 +197,13 @@ class TestWrap(unittest.TestCase): if not osp.exists(self.MATLAB_ACTUAL_DIR): os.mkdir(self.MATLAB_ACTUAL_DIR) - module = parser.Module.parseString(content) - - instantiator.instantiate_namespace_inplace(module) - wrapper = MatlabWrapper( - module=module, module_name='inheritance', top_module_namespace=['gtsam'], ignore_classes=[''], ) - cc_content = wrapper.wrap() + cc_content = wrapper.wrap(content) self.generate_content(cc_content) @@ -237,7 +215,7 @@ class TestWrap(unittest.TestCase): for file in files: self.compare_and_diff(file) - def test_namspaces(self): + def test_namespaces(self): """ Test interface file with full namespace definition. """ @@ -247,18 +225,13 @@ class TestWrap(unittest.TestCase): if not osp.exists(self.MATLAB_ACTUAL_DIR): os.mkdir(self.MATLAB_ACTUAL_DIR) - module = parser.Module.parseString(content) - - instantiator.instantiate_namespace_inplace(module) - wrapper = MatlabWrapper( - module=module, module_name='namespaces', top_module_namespace=['gtsam'], ignore_classes=[''], ) - cc_content = wrapper.wrap() + cc_content = wrapper.wrap(content) self.generate_content(cc_content) @@ -282,29 +255,25 @@ class TestWrap(unittest.TestCase): if not osp.exists(self.MATLAB_ACTUAL_DIR): os.mkdir(self.MATLAB_ACTUAL_DIR) - module = parser.Module.parseString(content) - - instantiator.instantiate_namespace_inplace(module) - wrapper = MatlabWrapper( - module=module, module_name='special_cases', top_module_namespace=['gtsam'], ignore_classes=[''], ) - cc_content = wrapper.wrap() + cc_content = wrapper.wrap(content) self.generate_content(cc_content) files = [ 'special_cases_wrapper.cpp', '+gtsam/PinholeCameraCal3Bundler.m', - '+gtsam/NonlinearFactorGraph.m', + '+gtsam/NonlinearFactorGraph.m', ] for file in files: self.compare_and_diff(file) + if __name__ == '__main__': unittest.main() diff --git a/wrap/tests/test_pybind_wrapper.py b/wrap/tests/test_pybind_wrapper.py index fe5e1950e..77c884b62 100644 --- a/wrap/tests/test_pybind_wrapper.py +++ b/wrap/tests/test_pybind_wrapper.py @@ -16,8 +16,6 @@ sys.path.append(osp.dirname(osp.dirname(osp.abspath(__file__)))) sys.path.append( osp.normpath(osp.abspath(osp.join(__file__, '../../../build/wrap')))) -import gtwrap.interface_parser as parser -import gtwrap.template_instantiator as instantiator from gtwrap.pybind_wrapper import PybindWrapper sys.path.append(osp.dirname(osp.dirname(osp.abspath(__file__)))) @@ -37,23 +35,18 @@ class TestWrap(unittest.TestCase): """ Common function to wrap content. """ - module = parser.Module.parseString(content) - - instantiator.instantiate_namespace_inplace(module) - with open(osp.join(self.TEST_DIR, "pybind_wrapper.tpl")) as template_file: module_template = template_file.read() # Create Pybind wrapper instance - wrapper = PybindWrapper(module=module, - module_name=module_name, + wrapper = PybindWrapper(module_name=module_name, use_boost=False, top_module_namespaces=[''], ignore_classes=[''], module_template=module_template) - cc_content = wrapper.wrap() + cc_content = wrapper.wrap(content) output = osp.join(self.TEST_DIR, output_dir, module_name + ".cpp") @@ -165,10 +158,10 @@ class TestWrap(unittest.TestCase): with open(osp.join(self.INTERFACE_DIR, 'enum.i'), 'r') as f: content = f.read() - output = self.wrap_content(content, 'enum_py', - self.PYTHON_ACTUAL_DIR) + output = self.wrap_content(content, 'enum_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('enum_pybind.cpp', output) + if __name__ == '__main__': unittest.main()