diff --git a/.cproject b/.cproject index c89bab640..af6b32cd2 100644 --- a/.cproject +++ b/.cproject @@ -840,6 +840,102 @@ true true + + make + -j5 + testGaussianFactorGraphUnordered.run + true + true + true + + + make + -j5 + testGaussianBayesNetUnordered.run + true + true + true + + + make + -j5 + testGaussianConditional.run + true + true + true + + + make + -j5 + testGaussianDensity.run + true + true + true + + + make + -j5 + testGaussianJunctionTree.run + true + true + true + + + make + -j5 + testHessianFactor.run + true + true + true + + + make + -j5 + testJacobianFactor.run + true + true + true + + + make + -j5 + testKalmanFilter.run + true + true + true + + + make + -j5 + testNoiseModel.run + true + true + true + + + make + -j5 + testSampler.run + true + true + true + + + make + -j5 + testSerializationLinear.run + true + true + true + + + make + -j5 + testVectorValues.run + true + true + true + make -j5 @@ -904,6 +1000,14 @@ true true + + make + -j5 + testTSAMFactors.run + true + true + true + make -j5 @@ -1407,92 +1511,44 @@ true true - + make -j5 - testVectorValues.run + testEliminationTree.run true true true - + make -j5 - testNoiseModel.run + testInference.run true true true - + make -j5 - testHessianFactor.run + testKey.run true true true - + make - -j5 - testGaussianConditional.run + -j1 + testSymbolicBayesTree.run true - true + false true - + make - -j5 - testGaussianFactorGraphUnordered.run + -j1 + testSymbolicSequentialSolver.run true - true - true - - - make - -j5 - testGaussianJunctionTree.run - true - true - true - - - make - -j5 - testKalmanFilter.run - true - true - true - - - make - -j5 - testGaussianDensity.run - true - true - true - - - make - -j5 - testSerializationLinear.run - true - true - true - - - make - -j5 - testJacobianFactor.run - true - true - true - - - make - -j5 - testSampler.run - true - true + false true @@ -1953,6 +2009,134 @@ true true + + make + -j5 + testCal3Bundler.run + true + true + true + + + make + -j5 + testCal3DS2.run + true + true + true + + + make + -j5 + testCalibratedCamera.run + true + true + true + + + make + -j5 + testEssentialMatrix.run + true + true + true + + + make + -j1 VERBOSE=1 + testHomography2.run + true + false + true + + + make + -j5 + testPinholeCamera.run + true + true + true + + + make + -j5 + testPoint2.run + true + true + true + + + make + -j5 + testPoint3.run + true + true + true + + + make + -j5 + testPose2.run + true + true + true + + + make + -j5 + testPose3.run + true + true + true + + + make + -j5 + testRot3M.run + true + true + true + + + make + -j5 + testSphere2.run + true + true + true + + + make + -j5 + testStereoCamera.run + true + true + true + + + make + -j5 + timeCalibratedCamera.run + true + true + true + + + make + -j5 + timePinholeCamera.run + true + true + true + + + make + -j5 + timeStereoCamera.run + true + true + true + make -j2 @@ -2073,6 +2257,14 @@ true true + + make + -j5 + testDataset.run + true + true + true + make -j5 @@ -2121,6 +2313,14 @@ true true + + make + -j5 + testWrap.run + true + true + true + make -j5 @@ -2169,38 +2369,6 @@ true true - - make - -j5 - testInference.run - true - true - true - - - make - -j1 - testSymbolicSequentialSolver.run - true - false - true - - - make - -j5 - testEliminationTree.run - true - true - true - - - make - -j1 - testSymbolicBayesTree.run - true - false - true - make -j2 @@ -2225,6 +2393,22 @@ true true + + make + -j5 + testMatrix.run + true + true + true + + + make + -j5 + testVector.run + true + true + true + make -j5 @@ -2494,158 +2678,6 @@ true true - - make - -j5 - testDataset.run - true - true - true - - - make - -j5 - testStereoCamera.run - true - true - true - - - make - -j5 - testRot3M.run - true - true - true - - - make - -j5 - testPoint3.run - true - true - true - - - make - -j5 - testCalibratedCamera.run - true - true - true - - - make - -j5 - timeStereoCamera.run - true - true - true - - - make - -j1 VERBOSE=1 - testHomography2.run - true - false - true - - - make - -j5 - testPoint2.run - true - true - true - - - make - -j5 - testPose2.run - true - true - true - - - make - -j5 - testPose3.run - true - true - true - - - make - -j5 - timeCalibratedCamera.run - true - true - true - - - make - -j5 - testPinholeCamera.run - true - true - true - - - make - -j5 - timePinholeCamera.run - true - true - true - - - make - -j5 - testCal3DS2.run - true - true - true - - - make - -j5 - testCal3Bundler.run - true - true - true - - - make - -j5 - testSphere2.run - true - true - true - - - make - -j5 - testEssentialMatrix.run - true - true - true - - - make - -j5 - testVector.run - true - true - true - - - make - -j5 - testMatrix.run - true - true - true - make -j2 @@ -2798,14 +2830,6 @@ true true - - make - -j5 - testLagoInitializer.run - true - true - true - make -j4 @@ -2934,14 +2958,6 @@ true true - - make - -j5 - testWrap.run - true - true - true - make -j5 diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index ee2bbd42a..9a0b297ab 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -56,7 +56,7 @@ endif() # Clang on Mac uses a template depth that is less than standard and is too small if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") if(NOT "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0") - add_definitions(-ftemplate-depth=1024) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024") endif() endif() diff --git a/gtsam.h b/gtsam.h index 2d181d350..96a778acf 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2363,6 +2363,12 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor { namespace utilities { #include + gtsam::KeyList createKeyList(Vector I); + gtsam::KeyList createKeyList(string s, Vector I); + gtsam::KeyVector createKeyVector(Vector I); + gtsam::KeyVector createKeyVector(string s, Vector I); + gtsam::KeySet createKeySet(Vector I); + gtsam::KeySet createKeySet(string s, Vector I); Matrix extractPoint2(const gtsam::Values& values); Matrix extractPoint3(const gtsam::Values& values); Matrix extractPose2(const gtsam::Values& values); @@ -2375,6 +2381,8 @@ namespace utilities { void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); + gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base); + gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys); } //\namespace utilities diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 5822a51f5..576da93bd 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -16,13 +16,30 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) endif() endforeach(eigen_dir) + # do the same for the unsupported eigen folder + file(GLOB_RECURSE unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h") + + file(GLOB unsupported_eigen_dir_headers_all "Eigen/unsupported/Eigen/*") + foreach(unsupported_eigen_dir ${unsupported_eigen_dir_headers_all}) + get_filename_component(filename ${unsupported_eigen_dir} NAME) + if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES ".svn"))) + list(APPEND unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/${filename}") + install(FILES Eigen/unsupported/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/unsupported/Eigen) + endif() + endforeach(unsupported_eigen_dir) + # Add to project source set(eigen_headers ${eigen_headers} PARENT_SCOPE) + # set(unsupported_eigen_headers ${unsupported_eigen_headers} PARENT_SCOPE) # install Eigen - only the headers in our 3rdparty directory install(DIRECTORY Eigen/Eigen DESTINATION include/gtsam/3rdparty/Eigen FILES_MATCHING PATTERN "*.h") + + install(DIRECTORY Eigen/unsupported/Eigen + DESTINATION include/gtsam/3rdparty/Eigen/unsupported/ + FILES_MATCHING PATTERN "*.h") endif() option(GTSAM_BUILD_METIS "Build metis library" ON) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index d9ab7d71c..f51197cf2 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -591,41 +591,17 @@ Matrix3 skewSymmetric(double wx, double wy, double wz) } /* ************************************************************************* */ -/** Numerical Recipes in C wrappers - * create Numerical Recipes in C structure - * pointers are subtracted by one to provide base 1 access - */ -/* ************************************************************************* */ -// FIXME: assumes row major, rather than column major -//double** createNRC(Matrix& A) { -// const size_t m=A.rows(); -// double** a = new double* [m]; -// for(size_t i = 0; i < m; i++) -// a[i] = &A(i,0)-1; -// return a; -//} - -/* ****************************************** - * - * Modified from Justin's codebase - * - * Idea came from other public domain code. Takes a S.P.D. matrix - * and computes the LL^t decomposition. returns L, which is lower - * triangular. Note this is the opposite convention from Matlab, - * which calculates Q'Q where Q is upper triangular. - * - * ******************************************/ Matrix LLt(const Matrix& A) { - Matrix L = zeros(A.rows(), A.rows()); - Eigen::LLT llt; - llt.compute(A); + Eigen::LLT llt(A); return llt.matrixL(); } +/* ************************************************************************* */ Matrix RtR(const Matrix &A) { - return LLt(A).transpose(); + Eigen::LLT llt(A); + return llt.matrixU(); } /* @@ -633,13 +609,10 @@ Matrix RtR(const Matrix &A) */ Matrix cholesky_inverse(const Matrix &A) { - // FIXME: replace with real algorithm - return A.inverse(); - -// Matrix L = LLt(A); -// Matrix inv(eye(A.rows())); -// inplace_solve (L, inv, BNU::lower_tag ()); -// return BNU::prod(trans(inv), inv); + Eigen::LLT llt(A); + Matrix inv = eye(A.rows()); + llt.matrixU().solveInPlace(inv); + return inv*inv.transpose(); } /* ************************************************************************* */ @@ -648,9 +621,9 @@ Matrix cholesky_inverse(const Matrix &A) // inv(B) * inv(B)' == A // inv(B' * B) == A Matrix inverse_square_root(const Matrix& A) { - Matrix R = RtR(A); + Eigen::LLT llt(A); Matrix inv = eye(A.rows()); - R.triangularView().solveInPlace(inv); + llt.matrixU().solveInPlace(inv); return inv.transpose(); } diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 9d84e5ab7..4e857b143 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -1022,22 +1022,32 @@ TEST( matrix, inverse_square_root ) /* *********************************************************************** */ // M was generated as the covariance of a set of random numbers. L that // we are checking against was generated via chol(M)' on octave +namespace cholesky { +Matrix M = (Matrix(5, 5) << 0.0874197, -0.0030860, 0.0116969, 0.0081463, + 0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363, + 0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463, + 0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363, + -0.0021113, 0.0036415, 0.0909464); + +Matrix expected = (Matrix(5, 5) << + 0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, + -0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000, + 0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000, + 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, + 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247); +} TEST( matrix, LLt ) { - Matrix M = (Matrix(5, 5) << 0.0874197, -0.0030860, 0.0116969, 0.0081463, - 0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363, - 0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463, - 0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363, - -0.0021113, 0.0036415, 0.0909464); + EQUALITY(cholesky::expected, LLt(cholesky::M)); +} +TEST( matrix, RtR ) +{ + EQUALITY(cholesky::expected.transpose(), RtR(cholesky::M)); +} - Matrix expected = (Matrix(5, 5) << - 0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, - -0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000, - 0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000, - 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, - 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247); - - EQUALITY(expected, LLt(M)); +TEST( matrix, cholesky_inverse ) +{ + EQUALITY(cholesky::M.inverse(), cholesky_inverse(cholesky::M)); } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 543822ce0..8c8e2cb2b 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -130,6 +130,9 @@ namespace gtsam { /// A'*b for Jacobian, eta for Hessian virtual VectorValues gradientAtZero() const = 0; + /// A'*b for Jacobian, eta for Hessian (raw memory version) + virtual void gradientAtZero(double* d) const = 0; + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index b56270a55..d6e4663e3 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -75,6 +75,13 @@ namespace gtsam { return dims_accumulated; } + /* ************************************************************************* */ + GaussianFactorGraph::shared_ptr GaussianFactorGraph::cloneToPtr() const { + gtsam::GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); + *result = *this; + return result; + } + /* ************************************************************************* */ GaussianFactorGraph GaussianFactorGraph::clone() const { GaussianFactorGraph result; diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 50442ac6b..692310f26 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -160,7 +160,13 @@ namespace gtsam { * Cloning preserves null factors so indices for the original graph are still * valid for the cloned graph. */ - GaussianFactorGraph clone() const; + virtual GaussianFactorGraph clone() const; + + /** + * CloneToPtr() performs a simple assignment to a new graph and returns it. + * There is no preservation of null factors! + */ + virtual GaussianFactorGraph::shared_ptr cloneToPtr() const; /** * Returns the negation of all factors in this graph - corresponds to antifactors. @@ -257,7 +263,7 @@ namespace gtsam { * @param [output] g A VectorValues to store the gradient, which must be preallocated, * see allocateVectorValues * @return The gradient as a VectorValues */ - VectorValues gradientAtZero() const; + virtual VectorValues gradientAtZero() const; /** Optimize along the gradient direction, with a closed-form computation to perform the line * search. The gradient is computed about \f$ \delta x=0 \f$. diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index e62af9a05..6f40b9bea 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -599,6 +599,23 @@ VectorValues HessianFactor::gradientAtZero() const { return g; } +/* ************************************************************************* */ +// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n +void HessianFactor::gradientAtZero(double* d) const { + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + + // Loop over all variables in the factor + for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { + Key j = keys_[pos]; + // Get the diagonal block, and insert its diagonal + DVector dj = -info_(pos,size()).knownOffDiagonal(); + DMap(d + 9 * j) += dj; + } +} + /* ************************************************************************* */ std::pair, boost::shared_ptr > EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index faf2ccd33..62d84925c 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -387,6 +387,8 @@ namespace gtsam { /// eta for Hessian VectorValues gradientAtZero() const; + virtual void gradientAtZero(double* d) const; + /** * Densely partially eliminate with Cholesky factorization. JacobianFactors are * left-multiplied with their transpose to form the Hessian using the conversion constructor diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index c080f75cb..a63bbf473 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -573,6 +573,11 @@ VectorValues JacobianFactor::gradientAtZero() const { return g; } +/* ************************************************************************* */ +void JacobianFactor::gradientAtZero(double* d) const { + //throw std::runtime_error("gradientAtZero not implemented for Jacobian factor"); +} + /* ************************************************************************* */ pair JacobianFactor::jacobian() const { pair result = jacobianUnweighted(); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 5a567c2c7..b90012822 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -286,6 +286,9 @@ namespace gtsam { /// A'*b for Jacobian VectorValues gradientAtZero() const; + /* ************************************************************************* */ + virtual void gradientAtZero(double* d) const; + /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */ JacobianFactor whiten() const; diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 641b47640..4bce597a1 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -47,22 +47,50 @@ void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) { Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose(); } +/* ************************************************************************* */ +// check *above the diagonal* for non-zero entries +static boost::optional checkIfDiagonal(const Matrix M) { + size_t m = M.rows(), n = M.cols(); + // check all non-diagonal entries + bool full = false; + size_t i, j; + for (i = 0; i < m; i++) + if (!full) + for (j = i + 1; j < n; j++) + if (fabs(M(i, j)) > 1e-9) { + full = true; + break; + } + if (full) { + return boost::none; + } else { + Vector diagonal(n); + for (j = 0; j < n; j++) + diagonal(j) = M(j, j); + return diagonal; + } +} + +/* ************************************************************************* */ +Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { + size_t m = R.rows(), n = R.cols(); + if (m != n) throw invalid_argument("Gaussian::SqrtInformation: R not square"); + boost::optional diagonal = boost::none; + if (smart) + diagonal = checkIfDiagonal(R); + if (diagonal) return Diagonal::Sigmas(reciprocal(*diagonal),true); + else return shared_ptr(new Gaussian(R.rows(),R)); +} /* ************************************************************************* */ Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, bool smart) { size_t m = covariance.rows(), n = covariance.cols(); if (m != n) throw invalid_argument("Gaussian::Covariance: covariance not square"); - if (smart) { - // check all non-diagonal entries - size_t i,j; - for (i = 0; i < m; i++) - for (j = 0; j < n; j++) - if (i != j && fabs(covariance(i, j)) > 1e-9) goto full; - Vector variances(n); - for (j = 0; j < n; j++) variances(j) = covariance(j,j); - return Diagonal::Variances(variances,true); - } - full: return shared_ptr(new Gaussian(n, inverse_square_root(covariance))); + boost::optional variances = boost::none; + if (smart) + variances = checkIfDiagonal(covariance); + if (variances) return Diagonal::Variances(*variances,true); + else return shared_ptr(new Gaussian(n, inverse_square_root(covariance))); } /* ************************************************************************* */ @@ -166,7 +194,7 @@ void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const // Diagonal /* ************************************************************************* */ Diagonal::Diagonal() : - Gaussian(1)//, sigmas_(ones(1)), invsigmas_(ones(1)), precisions_(ones(1)) + Gaussian(1) // TODO: Frank asks: really sure about this? { } @@ -191,12 +219,17 @@ Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) { /* ************************************************************************* */ Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) { if (smart) { + DenseIndex j, n = sigmas.size(); // look for zeros to make a constraint - for (size_t i=0; i< (size_t) sigmas.size(); ++i) - if (sigmas(i)<1e-8) + for (j=0; j< n; ++j) + if (sigmas(j)<1e-8) return Constrained::MixedSigmas(sigmas); + // check whether all the same entry + for (j = 1; j < n; j++) + if (sigmas(j) != sigmas(0)) goto full; + return Isotropic::Sigma(n, sigmas(0), true); } - return Diagonal::shared_ptr(new Diagonal(sigmas)); + full: return Diagonal::shared_ptr(new Diagonal(sigmas)); } /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index a87f426fa..e709ea543 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -159,10 +159,10 @@ namespace gtsam { /** * A Gaussian noise model created by specifying a square root information matrix. + * @param R The (upper-triangular) square root information matrix + * @param smart check if can be simplified to derived class */ - static shared_ptr SqrtInformation(const Matrix& R) { - return shared_ptr(new Gaussian(R.rows(),R)); - } + static shared_ptr SqrtInformation(const Matrix& R, bool smart = true); /** * A Gaussian noise model created by specifying a covariance matrix. diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 9d87a163b..d68caeabe 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -267,6 +267,24 @@ TEST(NoiseModel, QRNan ) EXPECT(assert_equal(expectedAb,Ab)); } +/* ************************************************************************* */ +TEST(NoiseModel, SmartSqrtInformation ) +{ + bool smart = true; + gtsam::SharedGaussian expected = Unit::Create(3); + gtsam::SharedGaussian actual = Gaussian::SqrtInformation(eye(3), smart); + EXPECT(assert_equal(*expected,*actual)); +} + +/* ************************************************************************* */ +TEST(NoiseModel, SmartSqrtInformation2 ) +{ + bool smart = true; + gtsam::SharedGaussian expected = Unit::Isotropic::Sigma(3,2); + gtsam::SharedGaussian actual = Gaussian::SqrtInformation(0.5*eye(3), smart); + EXPECT(assert_equal(*expected,*actual)); +} + /* ************************************************************************* */ TEST(NoiseModel, SmartCovariance ) { diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 559568c84..06e79324d 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -34,6 +34,11 @@ namespace gtsam { * * @addtogroup SLAM * + * If you are using the factor, please cite: + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally + * independent sets in factor graphs: a unifying perspective based on smart factors, + * Int. Conf. on Robotics and Automation (ICRA), 2014. + * * REFERENCES: * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index b1e20297e..f5b5293f8 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -33,7 +33,13 @@ namespace gtsam { /** * * @addtogroup SLAM - * * REFERENCES: + * + * If you are using the factor, please cite: + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally + * independent sets in factor graphs: a unifying perspective based on smart factors, + * Int. Conf. on Robotics and Automation (ICRA), 2014. + * + ** REFERENCES: * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 6be69e710..2cde6768f 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -138,7 +138,7 @@ void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality) { } /* ************************************************************************* */ -GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem( +GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem( const GaussianFactorGraph& linear) { gttic(damp); @@ -159,7 +159,8 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem( // for each of the variables, add a prior double sigma = 1.0 / std::sqrt(state_.lambda); - GaussianFactorGraph damped = linear; + GaussianFactorGraph::shared_ptr dampedPtr = linear.cloneToPtr(); + GaussianFactorGraph &damped = (*dampedPtr); damped.reserve(damped.size() + state_.values.size()); if (params_.diagonalDamping) { BOOST_FOREACH(const VectorValues::KeyValuePair& key_vector, state_.hessianDiagonal) { @@ -188,7 +189,20 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem( } } gttoc(damp); - return damped; + return dampedPtr; +} + +/* ************************************************************************* */ +// Log current error/lambda to file +inline void LevenbergMarquardtOptimizer::writeLogFile(double currentError){ + if (!params_.logFile.empty()) { + ofstream os(params_.logFile.c_str(), ios::app); + boost::posix_time::ptime currentTime = boost::posix_time::microsec_clock::universal_time(); + os << /*inner iterations*/ state_.totalNumberInnerIterations << "," + << 1e-6 * (currentTime - state_.startTime).total_microseconds() << "," + << /*current error*/ currentError << "," << state_.lambda << "," + << /*outer iterations*/ state_.iterations << endl; + } } /* ************************************************************************* */ @@ -205,6 +219,9 @@ void LevenbergMarquardtOptimizer::iterate() { cout << "linearizing = " << endl; GaussianFactorGraph::shared_ptr linear = linearize(); + if(state_.totalNumberInnerIterations==0) // write initial error + writeLogFile(state_.error); + // Keep increasing lambda until we make make progress while (true) { @@ -212,21 +229,8 @@ void LevenbergMarquardtOptimizer::iterate() { cout << "trying lambda = " << state_.lambda << endl; // Build damped system for this lambda (adds prior factors that make it like gradient descent) - GaussianFactorGraph dampedSystem = buildDampedSystem(*linear); - - // Log current error/lambda to file - if (!params_.logFile.empty()) { - ofstream os(params_.logFile.c_str(), ios::app); - - boost::posix_time::ptime currentTime = - boost::posix_time::microsec_clock::universal_time(); - - os << state_.totalNumberInnerIterations << "," - << 1e-6 * (currentTime - state_.startTime).total_microseconds() << "," - << state_.error << "," << state_.lambda << endl; - } - - ++state_.totalNumberInnerIterations; + GaussianFactorGraph::shared_ptr dampedSystemPtr = buildDampedSystem(*linear); + GaussianFactorGraph &dampedSystem = (*dampedSystemPtr); // Try solving double modelFidelity = 0.0; @@ -256,6 +260,9 @@ void LevenbergMarquardtOptimizer::iterate() { double newlinearizedError = linear->error(delta); double linearizedCostChange = state_.error - newlinearizedError; + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "newlinearizedError = " << newlinearizedError << + " linearizedCostChange = " << linearizedCostChange << endl; if (linearizedCostChange >= 0) { // step is valid // update values @@ -266,50 +273,62 @@ void LevenbergMarquardtOptimizer::iterate() { // compute new error gttic(compute_error); if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) - cout << "calculating error" << endl; + cout << "calculating error:" << endl; newError = graph_.error(newValues); gttoc(compute_error); + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "old error (" << state_.error + << ") new (tentative) error (" << newError << ")" << endl; + // cost change in the original, nonlinear system (old - new) double costChange = state_.error - newError; - if (linearizedCostChange > 1e-15) { // the error has to decrease to satify this condition + if (linearizedCostChange > 1e-20) { // the (linear) error has to decrease to satisfy this condition // fidelity of linearized model VS original system between modelFidelity = costChange / linearizedCostChange; // if we decrease the error in the nonlinear system and modelFidelity is above threshold step_is_successful = modelFidelity > params_.minModelFidelity; - } else { - step_is_successful = true; // linearizedCostChange close to zero - } + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "modelFidelity: " << modelFidelity << endl; + } // else we consider the step non successful and we either increase lambda or stop if error change is small double minAbsoluteTolerance = params_.relativeErrorTol * state_.error; // if the change is small we terminate - if (fabs(costChange) < minAbsoluteTolerance) + if (fabs(costChange) < minAbsoluteTolerance){ + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "fabs(costChange)="<= LevenbergMarquardtParams::TRYLAMBDA) - cout << "increasing lambda: old error (" << state_.error - << ") new error (" << newError << ")" << endl; + cout << "increasing lambda" << endl; increaseLambda(); + writeLogFile(state_.error); // check if lambda is too big if (state_.lambda >= params_.lambdaUpperBound) { if (nloVerbosity >= NonlinearOptimizerParams::TERMINATION) - cout - << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" - << endl; + cout << "Warning: Levenberg-Marquardt giving up because " + "cannot decrease error with maximum lambda" << endl; break; } } else { // the change in the cost is very small and it is not worth trying bigger lambdas + writeLogFile(state_.error); + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "Levenberg-Marquardt: stopping as relative cost reduction is small" << endl; break; } } // end while diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 47952f9e4..f365fc524 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -113,7 +113,6 @@ public: inline void setDiagonalDamping(bool flag) { diagonalDamping = flag; } - inline void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor_ = flag; } @@ -255,9 +254,11 @@ public: } /** Build a damped system for a specific lambda */ - GaussianFactorGraph buildDampedSystem(const GaussianFactorGraph& linear); + GaussianFactorGraph::shared_ptr buildDampedSystem(const GaussianFactorGraph& linear); friend class ::NonlinearOptimizerMoreOptimizationTest; + void writeLogFile(double currentError); + /// @} protected: diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 0d2f9a604..f381a0786 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -112,14 +112,14 @@ pair load2D( // Create a sampler with random number generator Sampler sampler(42u); - // load the factors + // Parse the pose constraints + int id1, id2; bool haveLandmark = false; while (is) { if(! (is >> tag)) break; if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) { - int id1, id2; double x, y, yaw; double v1, v2, v3, v4, v5, v6; @@ -168,68 +168,75 @@ pair load2D( new BetweenFactor(id1, id2, l1Xl2, *model)); graph->push_back(factor); } + + // Parse measurements + double bearing, range, bearing_std, range_std; + + // A bearing-range measurement if (tag == "BR") { - int id1, id2; - double bearing, range, bearing_std, range_std; - is >> id1 >> id2 >> bearing >> range >> bearing_std >> range_std; - - // optional filter - if (maxID && (id1 >= maxID || id2 >= maxID)) - continue; - - noiseModel::Diagonal::shared_ptr measurementNoise = - noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); - *graph += BearingRangeFactor(id1, id2, bearing, range, measurementNoise); - - // Insert poses or points if they do not exist yet - if (!initial->exists(id1)) - initial->insert(id1, Pose2()); - if (!initial->exists(id2)) { - Pose2 pose = initial->at(id1); - Point2 local(cos(bearing)*range,sin(bearing)*range); - Point2 global = pose.transform_from(local); - initial->insert(id2, global); - } } + + // A landmark measurement, TODO Frank says: don't know why is converted to bearing-range if (tag == "LANDMARK") { - int id1, id2; double lmx, lmy; double v1, v2, v3; is >> id1 >> id2 >> lmx >> lmy >> v1 >> v2 >> v3; // Convert x,y to bearing,range - double bearing = std::atan2(lmy, lmx); - double range = std::sqrt(lmx*lmx + lmy*lmy); + bearing = std::atan2(lmy, lmx); + range = std::sqrt(lmx*lmx + lmy*lmy); // In our experience, the x-y covariance on landmark sightings is not very good, so assume - // that it describes the uncertainty at a range of 10m, and convert that to bearing/range - // uncertainty. - SharedDiagonal measurementNoise; + // it describes the uncertainty at a range of 10m, and convert that to bearing/range uncertainty. if(std::abs(v1 - v3) < 1e-4) { - double rangeVar = v1; - double bearingVar = v1 / 10.0; - measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << bearingVar, rangeVar)); + bearing_std = sqrt(v1 / 10.0); + range_std = sqrt(v1); } else { + bearing_std = 1; + range_std = 1; if(!haveLandmark) { cout << "Warning: load2D is a very simple dataset loader and is ignoring the\n" "non-uniform covariance on LANDMARK measurements in this file." << endl; haveLandmark = true; } } + } + + // Do some common stuff for bearing-range measurements + if (tag == "LANDMARK" || tag == "BR") { + + // optional filter + if (maxID && id1 >= maxID) + continue; + + // Create noise model + noiseModel::Diagonal::shared_ptr measurementNoise = + noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); // Add to graph - *graph += BearingRangeFactor(id1, L(id2), bearing, range, measurementNoise); + *graph += BearingRangeFactor(id1, L(id2), bearing, range, + measurementNoise); + + // Insert poses or points if they do not exist yet + if (!initial->exists(id1)) + initial->insert(id1, Pose2()); + if (!initial->exists(L(id2))) { + Pose2 pose = initial->at(id1); + Point2 local(cos(bearing)*range,sin(bearing)*range); + Point2 global = pose.transform_from(local); + initial->insert(L(id2), global); + } } is.ignore(LINESIZE, '\n'); } cout << "load2D read a graph file with " << initial->size() - << " vertices and " << graph->nrFactors() << " factors" << endl; + << " vertices and " << graph->nrFactors() << " factors" << endl; return make_pair(graph, initial); } @@ -817,7 +824,7 @@ bool writeBALfromValues(const string& filename, const SfM_data &data, Values& va } for (size_t j = 0; j < dataValues.number_tracks(); j++){ // for each point - Key pointKey = symbol('l',j); + Key pointKey = P(j); if(values.exists(pointKey)){ Point3 point = values.at(pointKey); dataValues.tracks[j].p = point; diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index f4bcdded0..add0db29d 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -19,12 +19,14 @@ #include +#include #include #include #include #include +using namespace gtsam::symbol_shorthand; using namespace std; using namespace gtsam; @@ -303,7 +305,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ value.insert(poseKey, pose); } for(size_t j=0; j < readData.number_tracks(); j++){ // for each point - Key pointKey = symbol('l',j); + Key pointKey = P(j); Point3 point = poseChange.transform_from( readData.tracks[j].p ); value.insert(pointKey, point); } @@ -335,7 +337,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ EXPECT(assert_equal(expectedPose,actualPose, 1e-7)); Point3 expectedPoint = track0.p; - Key pointKey = symbol('l',0); + Key pointKey = P(0); Point3 actualPoint = value.at(pointKey); EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6)); } diff --git a/gtsam_unstable/slam/ImplicitSchurFactor.h b/gtsam_unstable/slam/ImplicitSchurFactor.h index 234d13f1f..c0f339b30 100644 --- a/gtsam_unstable/slam/ImplicitSchurFactor.h +++ b/gtsam_unstable/slam/ImplicitSchurFactor.h @@ -24,7 +24,6 @@ class ImplicitSchurFactor: public GaussianFactor { public: typedef ImplicitSchurFactor This; ///< Typedef to this class - typedef JacobianFactor Base; ///< Typedef to base class typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class protected: @@ -87,7 +86,8 @@ public: } /// print - void print(const std::string& s, const KeyFormatter& formatter) const { + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << " ImplicitSchurFactor " << std::endl; Factor::print(s); std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl; @@ -188,19 +188,24 @@ public: /// Return the block diagonal of the Hessian for this factor virtual std::map hessianBlockDiagonal() const { std::map blocks; + // F'*(I - E*P*E')*F for (size_t pos = 0; pos < size(); ++pos) { Key j = keys_[pos]; - const Matrix2D& Fj = Fblocks_[pos].second; // F'*F - F'*E*P*E'*F (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9) - Eigen::Matrix FtE = Fj.transpose() - * E_.block<2, 3>(2 * pos, 0); - blocks[j] = Fj.transpose() * Fj - - FtE * PointCovariance_ * FtE.transpose(); + const Matrix2D& Fj = Fblocks_[pos].second; + // Eigen::Matrix FtE = Fj.transpose() + // * E_.block<2, 3>(2 * pos, 0); + // blocks[j] = Fj.transpose() * Fj + // - FtE * PointCovariance_ * FtE.transpose(); + + const Matrix23& Ej = E_.block<2, 3>(2 * pos, 0); + blocks[j] = Fj.transpose() * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj); + // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-( -// static const Eigen::Matrix I2 = eye(2); -// Eigen::Matrix Q = // -// I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose(); -// blocks[j] = Fj.transpose() * Q * Fj; + // static const Eigen::Matrix I2 = eye(2); + // Eigen::Matrix Q = // + // I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose(); + // blocks[j] = Fj.transpose() * Q * Fj; } return blocks; } @@ -235,25 +240,33 @@ public: typedef std::vector Error2s; /** - * @brief Calculate corrected error Q*e = (I - E*P*E')*e + * @brief Calculate corrected error Q*(e-2*b) = (I - E*P*E')*(e-2*b) */ - void projectError(const Error2s& e1, Error2s& e2) const { + void projectError2(const Error2s& e1, Error2s& e2) const { - // d1 = E.transpose() * e1 = (3*2m)*2m + // d1 = E.transpose() * (e1-2*b) = (3*2m)*2m Vector3 d1; d1.setZero(); for (size_t k = 0; k < size(); k++) - d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * e1[k]; + d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * (e1[k] - 2 * b_.segment < 2 > (k * 2)); // d2 = E.transpose() * e1 = (3*2m)*2m Vector3 d2 = PointCovariance_ * d1; // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] for (size_t k = 0; k < size(); k++) - e2[k] = e1[k] - E_.block < 2, 3 > (2 * k, 0) * d2; + e2[k] = e1[k] - 2 * b_.segment < 2 > (k * 2) - E_.block < 2, 3 > (2 * k, 0) * d2; } - /// needed to be GaussianFactor - (I - E*P*E')*(F*x - b) + /* + * This definition matches the linearized error in the Hessian Factor: + * LinError(x) = x'*H*x - 2*x'*eta + f + * with: + * H = F' * (I-E'*P*E) * F = F' * Q * F + * eta = F' * (I-E'*P*E) * b = F' * Q * b + * f = nonlinear error + * (x'*H*x - 2*x'*eta + f) = x'*F'*Q*F*x - 2*x'*F'*Q *b + f = x'*F'*Q*(F*x - 2*b) + f + */ virtual double error(const VectorValues& x) const { // resize does not do malloc if correct size @@ -262,15 +275,56 @@ public: // e1 = F * x - b = (2m*dm)*dm for (size_t k = 0; k < size(); ++k) - e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2); - projectError(e1, e2); + e1[k] = Fblocks_[k].second * x.at(keys_[k]); + projectError2(e1, e2); double result = 0; for (size_t k = 0; k < size(); ++k) - result += dot(e2[k], e2[k]); - return 0.5 * result; + result += dot(e1[k], e2[k]); + + double f = b_.squaredNorm(); + return 0.5 * (result + f); } + /// needed to be GaussianFactor - (I - E*P*E')*(F*x - b) + // This is wrong and does not match the definition in Hessian + // virtual double error(const VectorValues& x) const { + // + // // resize does not do malloc if correct size + // e1.resize(size()); + // e2.resize(size()); + // + // // e1 = F * x - b = (2m*dm)*dm + // for (size_t k = 0; k < size(); ++k) + // e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2); + // projectError(e1, e2); + // + // double result = 0; + // for (size_t k = 0; k < size(); ++k) + // result += dot(e2[k], e2[k]); + // + // std::cout << "implicitFactor::error result " << result << std::endl; + // return 0.5 * result; + // } + /** + * @brief Calculate corrected error Q*e = (I - E*P*E')*e + */ + void projectError(const Error2s& e1, Error2s& e2) const { + + // d1 = E.transpose() * e1 = (3*2m)*2m + Vector3 d1; + d1.setZero(); + for (size_t k = 0; k < size(); k++) + d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * e1[k]; + + // d2 = E.transpose() * e1 = (3*2m)*2m + Vector3 d2 = PointCovariance_ * d1; + + // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] + for (size_t k = 0; k < size(); k++) + e2[k] = e1[k] - E_.block < 2, 3 > (2 * k, 0) * d2; + } + /// Scratch space for multiplyHessianAdd mutable Error2s e1, e2; @@ -377,6 +431,28 @@ public: return g; } + /** + * Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS + */ + void gradientAtZero(double* d) const { + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + + // calculate Q*b + e1.resize(size()); + e2.resize(size()); + for (size_t k = 0; k < size(); k++) + e1[k] = b_.segment < 2 > (2 * k); + projectError(e1, e2); + + for (size_t k = 0; k < size(); ++k) { // for each camera in the factor + Key j = keys_[k]; + DMap(d + D * j) += -Fblocks_[k].second.transpose() * e2[k]; + } + } + }; // ImplicitSchurFactor diff --git a/gtsam_unstable/slam/JacobianFactorQ.h b/gtsam_unstable/slam/JacobianFactorQ.h index fdbe0e231..f4dfb9422 100644 --- a/gtsam_unstable/slam/JacobianFactorQ.h +++ b/gtsam_unstable/slam/JacobianFactorQ.h @@ -23,6 +23,19 @@ public: JacobianFactorQ() { } + /// Empty constructor with keys + JacobianFactorQ(const FastVector& keys, + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + Matrix zeroMatrix = Matrix::Zero(0,D); + Vector zeroVector = Vector::Zero(0); + typedef std::pair KeyMatrix; + std::vector QF; + QF.reserve(keys.size()); + BOOST_FOREACH(const Key& key, keys) + QF.push_back(KeyMatrix(key, zeroMatrix)); + JacobianFactor::fillTerms(QF, zeroVector, model); + } + /// Constructor JacobianFactorQ(const std::vector& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b, diff --git a/gtsam_unstable/slam/JacobianFactorSVD.h b/gtsam_unstable/slam/JacobianFactorSVD.h index 752a9f48e..e8ade3b1b 100644 --- a/gtsam_unstable/slam/JacobianFactorSVD.h +++ b/gtsam_unstable/slam/JacobianFactorSVD.h @@ -18,10 +18,23 @@ public: typedef Eigen::Matrix Matrix2D; typedef std::pair KeyMatrix2D; + typedef std::pair KeyMatrix; /// Default constructor JacobianFactorSVD() {} + /// Empty constructor with keys + JacobianFactorSVD(const FastVector& keys, + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + Matrix zeroMatrix = Matrix::Zero(0,D); + Vector zeroVector = Vector::Zero(0); + std::vector QF; + QF.reserve(keys.size()); + BOOST_FOREACH(const Key& key, keys) + QF.push_back(KeyMatrix(key, zeroMatrix)); + JacobianFactor::fillTerms(QF, zeroVector, model); + } + /// Constructor JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { @@ -32,7 +45,6 @@ public: // BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); // JacobianFactor factor(QF, Q * b); - typedef std::pair KeyMatrix; std::vector QF; QF.reserve(numKeys); BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) diff --git a/gtsam_unstable/slam/SmartFactorBase.h b/gtsam_unstable/slam/SmartFactorBase.h index d24a878bb..93931549f 100644 --- a/gtsam_unstable/slam/SmartFactorBase.h +++ b/gtsam_unstable/slam/SmartFactorBase.h @@ -20,6 +20,7 @@ #pragma once #include "JacobianFactorQ.h" +#include "JacobianFactorSVD.h" #include "ImplicitSchurFactor.h" #include "RegularHessianFactor.h" @@ -135,12 +136,12 @@ public: } /** return the measurements */ - const Vector& measured() const { + const std::vector& measured() const { return measured_; } /** return the noise model */ - const SharedNoiseModel& noise() const { + const std::vector& noise() const { return noise_; } @@ -192,7 +193,7 @@ public: b[2 * i + 1] = e.y(); } catch (CheiralityException& e) { std::cout << "Cheirality exception " << std::endl; - exit (EXIT_FAILURE); + exit(EXIT_FAILURE); } i += 1; } @@ -222,7 +223,7 @@ public: * this->noise_.at(i)->distance(reprojectionError.vector()); } catch (CheiralityException&) { std::cout << "Cheirality exception " << std::endl; - exit (EXIT_FAILURE); + exit(EXIT_FAILURE); } i += 1; } @@ -244,7 +245,7 @@ public: cameras[i].project(point, boost::none, Ei); } catch (CheiralityException& e) { std::cout << "Cheirality exception " << std::endl; - exit (EXIT_FAILURE); + exit(EXIT_FAILURE); } this->noise_.at(i)->WhitenSystem(Ei, b); E.block<2, 3>(2 * i, 0) = Ei; @@ -274,7 +275,7 @@ public: -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector(); } catch (CheiralityException&) { std::cout << "Cheirality exception " << std::endl; - exit (EXIT_FAILURE); + exit(EXIT_FAILURE); } this->noise_.at(i)->WhitenSystem(Fi, Ei, Hcali, bi); @@ -302,15 +303,18 @@ public: // Point covariance inv(E'*E) Matrix3 EtE = E.transpose() * E; - Matrix3 DMatrix = eye(E.cols()); // damping matrix if (diagonalDamping) { // diagonal of the hessian - DMatrix(0, 0) = EtE(0, 0); - DMatrix(1, 1) = EtE(1, 1); - DMatrix(2, 2) = EtE(2, 2); + EtE(0, 0) += lambda * EtE(0, 0); + EtE(1, 1) += lambda * EtE(1, 1); + EtE(2, 2) += lambda * EtE(2, 2); + }else{ + EtE(0, 0) += lambda; + EtE(1, 1) += lambda; + EtE(2, 2) += lambda; } - PointCov.noalias() = (EtE + lambda * DMatrix).inverse(); + PointCov.noalias() = (EtE).inverse(); return f; } @@ -321,8 +325,8 @@ public: const Cameras& cameras, const Point3& point, const double lambda = 0.0) const { - size_t numKeys = this->keys_.size(); - std::vector < KeyMatrix2D > Fblocks; + int numKeys = this->keys_.size(); + std::vector Fblocks; double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda); F = zeros(2 * numKeys, D * numKeys); @@ -345,7 +349,7 @@ public: diagonalDamping); // diagonalDamping should have no effect (only on PointCov) // Do SVD on A - Eigen::JacobiSVD < Matrix > svd(E, Eigen::ComputeFullU); + Eigen::JacobiSVD svd(E, Eigen::ComputeFullU); Vector s = svd.singularValues(); // Enull = zeros(2 * numKeys, 2 * numKeys - 3); int numKeys = this->keys_.size(); @@ -361,7 +365,7 @@ public: const Cameras& cameras, const Point3& point) const { int numKeys = this->keys_.size(); - std::vector < KeyMatrix2D > Fblocks; + std::vector Fblocks; double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point); F.resize(2 * numKeys, D * numKeys); F.setZero(); @@ -380,14 +384,14 @@ public: int numKeys = this->keys_.size(); - std::vector < KeyMatrix2D > Fblocks; + std::vector Fblocks; Matrix E; Matrix3 PointCov; Vector b; double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, diagonalDamping); -//#define HESSIAN_BLOCKS +//#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix #ifdef HESSIAN_BLOCKS // Create structures for Hessian Factors std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); @@ -400,46 +404,23 @@ public: //std::vector < Vector > gs2(gs.begin(), gs.end()); return boost::make_shared < RegularHessianFactor - > (this->keys_, Gs, gs, f); -#else + > (this->keys_, Gs, gs, f); +#else // we create directly a SymmetricBlockMatrix size_t n1 = D * numKeys + 1; std::vector dims(numKeys + 1); // this also includes the b term std::fill(dims.begin(), dims.end() - 1, D); dims.back() = 1; - SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1) + SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1) sparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... - augmentedHessian(numKeys,numKeys)(0,0) = f; - return boost::make_shared >( - this->keys_, augmentedHessian); - + augmentedHessian(numKeys, numKeys)(0, 0) = f; + return boost::make_shared >(this->keys_, + augmentedHessian); #endif } // **************************************************************************************************** - boost::shared_ptr > updateAugmentedHessian( - const Cameras& cameras, const Point3& point, const double lambda, - bool diagonalDamping, SymmetricBlockMatrix& augmentedHessian) const { - - int numKeys = this->keys_.size(); - - std::vector < KeyMatrix2D > Fblocks; - Matrix E; - Matrix3 PointCov; - Vector b; - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); - - std::vector dims(numKeys + 1); // this also includes the b term - std::fill(dims.begin(), dims.end() - 1, D); - dims.back() = 1; - - updateSparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... - std::cout << "f "<< f <& Fblocks, const Matrix& E, const Matrix& PointCov, const Vector& b, /*output ->*/std::vector& Gs, std::vector& gs) const { @@ -466,7 +447,7 @@ public: int GsCount2 = 0; for (DenseIndex i1 = 0; i1 < numKeys; i1++) { // for each camera DenseIndex i1D = i1 * D; - gs.at(i1) = gs_vector.segment < D > (i1D); + gs.at(i1) = gs_vector.segment(i1D); for (DenseIndex i2 = 0; i2 < numKeys; i2++) { if (i2 >= i1) { Gs.at(GsCount2) = H.block(i1D, i2 * D); @@ -476,53 +457,6 @@ public: } } - // **************************************************************************************************** - void updateSparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, - /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { - // Schur complement trick - // Gs = F' * F - F' * E * P * E' * F - // gs = F' * (b - E * P * E' * b) - - // a single point is observed in numKeys cameras - size_t numKeys = this->keys_.size(); // cameras observing current point - size_t aug_numKeys = augmentedHessian.rows() - 1; // all cameras in the group - - // Blockwise Schur complement - for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera - - const Matrix2D& Fi1 = Fblocks.at(i1).second; - const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P; - - // D = (Dx2) * (2) - // (augmentedHessian.matrix()).block (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b - size_t aug_i1 = this->keys_[i1]; - std::cout << "i1 "<< i1 < (2 * i1) // F' * b - - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) - - // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) - std::cout << "filled 1 " <(2 * i1, 0).transpose() * Fi1); - - // upper triangular part of the hessian - for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera - const Matrix2D& Fi2 = Fblocks.at(i2).second; - size_t aug_i2 = this->keys_[i2]; - std::cout << "i2 "<< i2 <(2 * i2, 0).transpose() * Fi2); - } - } // end of for over cameras - } - // **************************************************************************************************** void sparseSchurComplement(const std::vector& Fblocks, const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, @@ -542,20 +476,20 @@ public: // D = (Dx2) * (2) // (augmentedHessian.matrix()).block (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b - augmentedHessian(i1,numKeys) = Fi1.transpose() * b.segment < 2 > (2 * i1) // F' * b - - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) + augmentedHessian(i1, numKeys) = Fi1.transpose() * b.segment<2>(2 * i1) // F' * b + - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) - augmentedHessian(i1,i1) = - Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); + augmentedHessian(i1, i1) = Fi1.transpose() + * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); // upper triangular part of the hessian - for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera + for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera const Matrix2D& Fi2 = Fblocks.at(i2).second; // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - augmentedHessian(i1,i2) = - -Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); + augmentedHessian(i1, i2) = -Fi1.transpose() + * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); } } // end of for over cameras } @@ -586,24 +520,109 @@ public: { // for i1 = i2 // D = (Dx2) * (2) - gs.at(i1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b - // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) - gs.at(i1) -= Fi1.transpose() * (Ei1_P * (E.transpose() * b)); + gs.at(i1) = Fi1.transpose() * b.segment<2>(2 * i1) // F' * b + -Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) - Gs.at(GsIndex) = Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); + Gs.at(GsIndex) = Fi1.transpose() + * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); GsIndex++; } // upper triangular part of the hessian - for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera + for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera const Matrix2D& Fi2 = Fblocks.at(i2).second; // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - Gs.at(GsIndex) = -Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); + Gs.at(GsIndex) = -Fi1.transpose() + * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); GsIndex++; } } // end of for over cameras } + + // **************************************************************************************************** + void updateAugmentedHessian(const Cameras& cameras, const Point3& point, + const double lambda, bool diagonalDamping, + SymmetricBlockMatrix& augmentedHessian, + const FastVector allKeys) const { + + // int numKeys = this->keys_.size(); + + std::vector Fblocks; + Matrix E; + Matrix3 PointCov; + Vector b; + double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, + diagonalDamping); + + updateSparseSchurComplement(Fblocks, E, PointCov, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... + } + + // **************************************************************************************************** + void updateSparseSchurComplement(const std::vector& Fblocks, + const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, + const double f, const FastVector allKeys, + /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { + // Schur complement trick + // Gs = F' * F - F' * E * P * E' * F + // gs = F' * (b - E * P * E' * b) + + MatrixDD matrixBlock; + typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix + + FastMap KeySlotMap; + for (size_t slot=0; slot < allKeys.size(); slot++) + KeySlotMap.insert(std::make_pair(allKeys[slot],slot)); + + // a single point is observed in numKeys cameras + size_t numKeys = this->keys_.size(); // cameras observing current point + size_t aug_numKeys = (augmentedHessian.rows() - 1) / D; // all cameras in the group + + // Blockwise Schur complement + for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera in the current factor + + const Matrix2D& Fi1 = Fblocks.at(i1).second; + const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P; + + // D = (Dx2) * (2) + // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7) + // we should map those to a slot in the local (grouped) hessian (0,1,2,3,4) + // Key cameraKey_i1 = this->keys_[i1]; + DenseIndex aug_i1 = KeySlotMap[this->keys_[i1]]; + + // information vector - store previous vector + // vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal(); + // add contribution of current factor + augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal() + + Fi1.transpose() * b.segment<2>(2 * i1) // F' * b + - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) + + // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) + // main block diagonal - store previous block + matrixBlock = augmentedHessian(aug_i1, aug_i1); + // add contribution of current factor + augmentedHessian(aug_i1, aug_i1) = matrixBlock + + ( Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1) ); + + // upper triangular part of the hessian + for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera + const Matrix2D& Fi2 = Fblocks.at(i2).second; + + //Key cameraKey_i2 = this->keys_[i2]; + DenseIndex aug_i2 = KeySlotMap[this->keys_[i2]]; + + // (DxD) = (Dx2) * ( (2x2) * (2xD) ) + // off diagonal block - store previous block + // matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal(); + // add contribution of current factor + augmentedHessian(aug_i1, aug_i2) = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal() + - Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); + } + } // end of for over cameras + + augmentedHessian(aug_numKeys, aug_numKeys)(0, 0) += f; + } + // **************************************************************************************************** boost::shared_ptr > createImplicitSchurFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, @@ -620,13 +639,24 @@ public: boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { - std::vector < KeyMatrix2D > Fblocks; + std::vector Fblocks; Matrix E; Matrix3 PointCov; Vector b; computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, diagonalDamping); - return boost::make_shared < JacobianFactorQ > (Fblocks, E, PointCov, b); + return boost::make_shared >(Fblocks, E, PointCov, b); + } + + // **************************************************************************************************** + boost::shared_ptr createJacobianSVDFactor( + const Cameras& cameras, const Point3& point, double lambda = 0.0) const { + size_t numKeys = this->keys_.size(); + std::vector < KeyMatrix2D > Fblocks; + Vector b; + Matrix Enull(2*numKeys, 2*numKeys-3); + computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); + return boost::make_shared< JacobianFactorSVD<6> >(Fblocks, Enull, b); } private: diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index 59a841813..2124dd6f6 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -54,6 +54,10 @@ public: double f; }; +enum linearizationType { + HESSIAN, JACOBIAN_SVD, JACOBIAN_Q +}; + /** * SmartProjectionFactor: triangulates point * TODO: why LANDMARK parameter? @@ -91,6 +95,13 @@ protected: /// shorthand for base class type typedef SmartFactorBase Base; + double landmarkDistanceThreshold_; // if the landmark is triangulated at a + // distance larger than that the factor is considered degenerate + + double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the + // average reprojection error is smaller than this threshold after triangulation, + // and the factor is disregarded if the error is large + /// shorthand for this class typedef SmartProjectionFactor This; @@ -115,12 +126,15 @@ public: SmartProjectionFactor(const double rankTol, const double linThreshold, const bool manageDegeneracy, const bool enableEPI, boost::optional body_P_sensor = boost::none, - SmartFactorStatePtr state = SmartFactorStatePtr( - new SmartProjectionFactorState())) : + double landmarkDistanceThreshold = 1e10, + double dynamicOutlierRejectionThreshold = -1, + SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_( 1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_( linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_( - false), verboseCheirality_(false), state_(state) { + false), verboseCheirality_(false), state_(state), + landmarkDistanceThreshold_(landmarkDistanceThreshold), + dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) { } /** Virtual destructor */ @@ -234,6 +248,31 @@ public: rankTolerance_, enableEPI_); degenerate_ = false; cheiralityException_ = false; + + // Check landmark distance and reprojection errors to avoid outliers + double totalReprojError = 0.0; + size_t i=0; + BOOST_FOREACH(const Camera& camera, cameras) { + Point3 cameraTranslation = camera.pose().translation(); + // we discard smart factors corresponding to points that are far away + if(cameraTranslation.distance(point_) > landmarkDistanceThreshold_){ + degenerate_ = true; + break; + } + const Point2& zi = this->measured_.at(i); + try { + Point2 reprojectionError(camera.project(point_) - zi); + totalReprojError += reprojectionError.vector().norm(); + } catch (CheiralityException& e) { + cheiralityException_ = true; + } + i += 1; + } + // we discard smart factors that have large reprojection error + if(dynamicOutlierRejectionThreshold_ > 0 && + totalReprojError/m > dynamicOutlierRejectionThreshold_) + degenerate_ = true; + } catch (TriangulationUnderconstrainedException&) { // if TriangulationUnderconstrainedException can be // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before @@ -385,7 +424,7 @@ public: if (triangulateForLinearize(cameras)) return Base::createJacobianQFactor(cameras, point_, lambda); else - return boost::shared_ptr >(); + return boost::make_shared< JacobianFactorQ >(this->keys_); } /// Create a factor, takes values @@ -397,7 +436,16 @@ public: if (nonDegenerate) return createJacobianQFactor(myCameras, lambda); else - return boost::shared_ptr >(); + return boost::make_shared< JacobianFactorQ >(this->keys_); + } + + /// different (faster) way to compute Jacobian factor + boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras, + double lambda) const { + if (triangulateForLinearize(cameras)) + return Base::createJacobianSVDFactor(cameras, point_, lambda); + else + return boost::make_shared< JacobianFactorSVD >(this->keys_); } /// Returns true if nonDegenerate diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactor.h b/gtsam_unstable/slam/SmartProjectionPoseFactor.h index 879e5ab80..66497d28d 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactor.h @@ -22,6 +22,16 @@ #include "SmartProjectionFactor.h" namespace gtsam { +/** + * + * @addtogroup SLAM + * + * If you are using the factor, please cite: + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally + * independent sets in factor graphs: a unifying perspective based on smart factors, + * Int. Conf. on Robotics and Automation (ICRA), 2014. + * + */ /** * The calibration is known here. The factor only constraints poses (variable dimension is 6) @@ -31,6 +41,8 @@ template class SmartProjectionPoseFactor: public SmartProjectionFactor { protected: + linearizationType linearizeTo_; + // Known calibration std::vector > K_all_; ///< shared pointer to calibration object (one for each camera) @@ -56,8 +68,11 @@ public: */ SmartProjectionPoseFactor(const double rankTol = 1, const double linThreshold = -1, const bool manageDegeneracy = false, - const bool enableEPI = false, boost::optional body_P_sensor = boost::none) : - Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor) {} + const bool enableEPI = false, boost::optional body_P_sensor = boost::none, + linearizationType linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, + double dynamicOutlierRejectionThreshold = -1) : + Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor, + landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {} /** Virtual destructor */ virtual ~SmartProjectionPoseFactor() {} @@ -139,11 +154,22 @@ public: } /** - * linearize returns a Hessian factor contraining the poses + * linear factor on the poses */ virtual boost::shared_ptr linearize( const Values& values) const { - return this->createHessianFactor(cameras(values)); + // depending on flag set on construction we may linearize to different linear factors + switch(linearizeTo_){ + case JACOBIAN_SVD : + return this->createJacobianSVDFactor(cameras(values), 0.0); + break; + case JACOBIAN_Q : + return this->createJacobianQFactor(cameras(values), 0.0); + break; + default: + return this->createHessianFactor(cameras(values)); + break; + } } /** diff --git a/gtsam_unstable/slam/tests/testSmartProjectionHessianPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp similarity index 79% rename from gtsam_unstable/slam/tests/testSmartProjectionHessianPoseFactor.cpp rename to gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp index 9a3fe7f62..ee8db1267 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionHessianPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp @@ -369,6 +369,271 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ if(isDebugTest) tictoc_print_(); } +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, jacobianSVD ){ + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + smartFactor3->add(measurements_cam3, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3*noise_pose); + + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3,result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, landmarkDistance ){ + + double excludeLandmarksFutherThanDist = 2; + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + smartFactor3->add(measurements_cam3, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3*noise_pose); + + // All factors are disabled and pose should remain where it is + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(values.at(x3),result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ + + double excludeLandmarksFutherThanDist = 1e10; + double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + Point3 landmark4(5, -0.5, 1); + + vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); + measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10,10); // add outlier + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, + JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor3->add(measurements_cam3, views, model, K); + + SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor4->add(measurements_cam4, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(smartFactor4); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3); + + // All factors are disabled and pose should remain where it is + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3,result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, jacobianQ ){ + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + smartFactor3->add(measurements_cam3, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3*noise_pose); + + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3,result.at(x3))); +} + /* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){ // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; diff --git a/matlab.h b/matlab.h index 100f5a242..c4891a615 100644 --- a/matlab.h +++ b/matlab.h @@ -28,130 +28,228 @@ #include #include +#include + #include namespace gtsam { - namespace utilities { - - /// Extract all Point2 values into a single matrix [x y] - Matrix extractPoint2(const Values& values) { - size_t j=0; - Values::ConstFiltered points = values.filter(); - Matrix result(points.size(),2); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) - result.row(j++) = key_value.value.vector(); - return result; - } - - /// Extract all Point3 values into a single matrix [x y z] - Matrix extractPoint3(const Values& values) { - Values::ConstFiltered points = values.filter(); - Matrix result(points.size(),3); - size_t j=0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) - result.row(j++) = key_value.value.vector(); - return result; - } - - /// Extract all Pose2 values into a single matrix [x y theta] - Matrix extractPose2(const Values& values) { - Values::ConstFiltered poses = values.filter(); - Matrix result(poses.size(),3); - size_t j=0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) - result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta(); - return result; - } - - /// Extract all Pose3 values - Values allPose3s(const Values& values) { - return values.filter(); - } - - /// Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z] - Matrix extractPose3(const Values& values) { - Values::ConstFiltered poses = values.filter(); - Matrix result(poses.size(),12); - size_t j=0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) { - result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0); - result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1); - result.row(j).segment(6, 3) << key_value.value.rotation().matrix().row(2); - result.row(j).tail(3) = key_value.value.translation().vector(); - j++; - } - return result; - } - - /// Perturb all Point2 values using normally distributed noise - void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,sigma); - Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); - } - } - - /// Perturb all Pose2 values using normally distributed noise - void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = 42u) { - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas( - Vector3(sigmaT, sigmaT, sigmaR)); - Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); - } - } - - /// Perturb all Point3 values using normally distributed noise - void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma); - Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); - } - } - - /// Insert a number of initial point values by backprojecting - void insertBackprojections(Values& values, const SimpleCamera& camera, const Vector& J, const Matrix& Z, double depth) { - if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K"); - if (Z.cols() != J.size()) throw std::invalid_argument("insertBackProjections: J and Z must have same number of entries"); - for(int k=0;k > - (Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor)); - } - } - - /// Calculate the errors of all projection factors in a graph - Matrix reprojectionErrors(const NonlinearFactorGraph& graph, const Values& values) { - // first count - size_t K = 0, k=0; - BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) - if (boost::dynamic_pointer_cast >(f)) ++K; - // now fill - Matrix errors(2,K); - BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) { - boost::shared_ptr > p = boost::dynamic_pointer_cast >(f); - if (p) errors.col(k++) = p->unwhitenedError(values); - } - return errors; - } - - } +namespace utilities { +// Create a KeyList from indices +FastList createKeyList(const Vector& I) { + FastList set; + for (int i = 0; i < I.size(); i++) + set.push_back(I[i]); + return set; +} + +// Create a KeyList from indices using symbol +FastList createKeyList(string s, const Vector& I) { + FastList set; + char c = s[0]; + for (int i = 0; i < I.size(); i++) + set.push_back(Symbol(c, I[i])); + return set; +} + +// Create a KeyVector from indices +FastVector createKeyVector(const Vector& I) { + FastVector set; + for (int i = 0; i < I.size(); i++) + set.push_back(I[i]); + return set; +} + +// Create a KeyVector from indices using symbol +FastVector createKeyVector(string s, const Vector& I) { + FastVector set; + char c = s[0]; + for (int i = 0; i < I.size(); i++) + set.push_back(Symbol(c, I[i])); + return set; +} + +// Create a KeySet from indices +FastSet createKeySet(const Vector& I) { + FastSet set; + for (int i = 0; i < I.size(); i++) + set.insert(I[i]); + return set; +} + +// Create a KeySet from indices using symbol +FastSet createKeySet(string s, const Vector& I) { + FastSet set; + char c = s[0]; + for (int i = 0; i < I.size(); i++) + set.insert(symbol(c, I[i])); + return set; +} + +/// Extract all Point2 values into a single matrix [x y] +Matrix extractPoint2(const Values& values) { + size_t j = 0; + Values::ConstFiltered points = values.filter(); + Matrix result(points.size(), 2); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) + result.row(j++) = key_value.value.vector(); + return result; +} + +/// Extract all Point3 values into a single matrix [x y z] +Matrix extractPoint3(const Values& values) { + Values::ConstFiltered points = values.filter(); + Matrix result(points.size(), 3); + size_t j = 0; + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) + result.row(j++) = key_value.value.vector(); + return result; +} + +/// Extract all Pose2 values into a single matrix [x y theta] +Matrix extractPose2(const Values& values) { + Values::ConstFiltered poses = values.filter(); + Matrix result(poses.size(), 3); + size_t j = 0; + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) + result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta(); + return result; +} + +/// Extract all Pose3 values +Values allPose3s(const Values& values) { + return values.filter(); +} + +/// Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z] +Matrix extractPose3(const Values& values) { + Values::ConstFiltered poses = values.filter(); + Matrix result(poses.size(), 12); + size_t j = 0; + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) { + result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0); + result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1); + result.row(j).segment(6, 3) << key_value.value.rotation().matrix().row(2); + result.row(j).tail(3) = key_value.value.translation().vector(); + j++; + } + return result; +} + +/// Perturb all Point2 values using normally distributed noise +void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2, + sigma); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } +} + +/// Perturb all Pose2 values using normally distributed noise +void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = + 42u) { + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas( + Vector3(sigmaT, sigmaT, sigmaR)); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } +} + +/// Perturb all Point3 values using normally distributed noise +void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3, + sigma); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } +} + +/// Insert a number of initial point values by backprojecting +void insertBackprojections(Values& values, const SimpleCamera& camera, + const Vector& J, const Matrix& Z, double depth) { + if (Z.rows() != 2) + throw std::invalid_argument("insertBackProjections: Z must be 2*K"); + if (Z.cols() != J.size()) + throw std::invalid_argument( + "insertBackProjections: J and Z must have same number of entries"); + for (int k = 0; k < Z.cols(); k++) { + Point2 p(Z(0, k), Z(1, k)); + Point3 P = camera.backproject(p, depth); + values.insert(J(k), P); + } +} + +/// Insert multiple projection factors for a single pose key +void insertProjectionFactors(NonlinearFactorGraph& graph, Key i, + const Vector& J, const Matrix& Z, const SharedNoiseModel& model, + const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) { + if (Z.rows() != 2) + throw std::invalid_argument("addMeasurements: Z must be 2*K"); + if (Z.cols() != J.size()) + throw std::invalid_argument( + "addMeasurements: J and Z must have same number of entries"); + for (int k = 0; k < Z.cols(); k++) { + graph.push_back( + boost::make_shared >( + Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor)); + } +} + +/// Calculate the errors of all projection factors in a graph +Matrix reprojectionErrors(const NonlinearFactorGraph& graph, + const Values& values) { + // first count + size_t K = 0, k = 0; + BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) + if (boost::dynamic_pointer_cast >( + f)) + ++K; + // now fill + Matrix errors(2, K); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) { + boost::shared_ptr > p = + boost::dynamic_pointer_cast >( + f); + if (p) + errors.col(k++) = p->unwhitenedError(values); + } + return errors; +} + +/// Convert from local to world coordinates +Values localToWorld(const Values& local, const Pose2& base, + const FastVector user_keys = FastVector()) { + + Values world; + + // if no keys given, get all keys from local values + FastVector keys(user_keys); + if (keys.size()==0) + keys = FastVector(local.keys()); + + // Loop over all keys + BOOST_FOREACH(Key key, keys) { + try { + // if value is a Pose2, compose it with base pose + Pose2 pose = local.at(key); + world.insert(key, base.compose(pose)); + } catch (std::exception e1) { + try { + // if value is a Point2, transform it from base pose + Point2 point = local.at(key); + world.insert(key, base.transform_from(point)); + } catch (std::exception e2) { + // if not Pose2 or Point2, do nothing + } + } + } + return world; +} + +} } diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m index 5fec7721c..023c61dbe 100644 --- a/matlab/+gtsam/Contents.m +++ b/matlab/+gtsam/Contents.m @@ -175,6 +175,9 @@ % symbolIndex - get index from a symbol key % %% Wrapped C++ Convenience Functions for use within MATLAB +% utilities.createKeyList - Create KeyList from indices +% utilities.createKeyVector - Create KeyVector from indices +% utilities.createKeySet - Create KeySet from indices % utilities.extractPoint2 - Extract all Point2 values into a single matrix [x y] % utilities.extractPoint3 - Extract all Point3 values into a single matrix [x y z] % utilities.extractPose2 - Extract all Pose2 values into a single matrix [x y theta] @@ -186,3 +189,4 @@ % utilities.insertBackprojections - Insert a number of initial point values by backprojecting % utilities.insertProjectionFactors - Insert multiple projection factors for a single pose key % utilities.reprojectionErrors - Calculate the errors of all projection factors in a graph +% utilities.localToWorld - Convert from local to world coordinates diff --git a/matlab/+gtsam/plot2DTrajectory.m b/matlab/+gtsam/plot2DTrajectory.m index c6b0f85aa..45e7fe467 100644 --- a/matlab/+gtsam/plot2DTrajectory.m +++ b/matlab/+gtsam/plot2DTrajectory.m @@ -8,53 +8,41 @@ function plot2DTrajectory(values, linespec, marginals) import gtsam.* if ~exist('linespec', 'var') || isempty(linespec) - linespec = 'k*-'; + linespec = 'k*-'; end haveMarginals = exist('marginals', 'var'); -keys = KeyVector(values.keys); holdstate = ishold; hold on -% Plot poses and covariance matrices -lastIndex = []; -for i = 0:keys.size-1 +% Do something very efficient to draw trajectory +poses = utilities.extractPose2(values); +X = poses(:,1); +Y = poses(:,2); +theta = poses(:,3); +plot(X,Y,linespec); + +% Quiver can also be vectorized if no marginals asked +if ~haveMarginals + C = cos(theta); + S = sin(theta); + quiver(X,Y,C,S,0.1,linespec); +else + % plotPose2 does both quiver and covariance matrix + keys = KeyVector(values.keys); + for i = 0:keys.size-1 key = keys.at(i); x = values.at(key); if isa(x, 'gtsam.Pose2') - if ~isempty(lastIndex) - % Draw line from last pose then covariance ellipse on top of - % last pose. - lastKey = keys.at(lastIndex); - lastPose = values.at(lastKey); - plot([ x.x; lastPose.x ], [ x.y; lastPose.y ], linespec); - if haveMarginals - P = marginals.marginalCovariance(lastKey); - gtsam.plotPose2(lastPose, 'g', P); - else - gtsam.plotPose2(lastPose, 'g', []); - end - - end - lastIndex = i; - end -end - -% Draw final covariance ellipse -if ~isempty(lastIndex) - lastKey = keys.at(lastIndex); - lastPose = values.at(lastKey); - if haveMarginals - P = marginals.marginalCovariance(lastKey); - gtsam.plotPose2(lastPose, 'g', P); - else - gtsam.plotPose2(lastPose, 'g', []); + P = marginals.marginalCovariance(key); + gtsam.plotPose2(x,linespec(1), P); end + end end if ~holdstate - hold off + hold off end end diff --git a/matlab/gtsam_examples/Pose2SLAMExample_advanced.m b/matlab/gtsam_examples/Pose2SLAMExample_advanced.m index 343dee05b..64cda5afc 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample_advanced.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_advanced.m @@ -59,16 +59,16 @@ params.setAbsoluteErrorTol(1e-15); params.setRelativeErrorTol(1e-15); params.setVerbosity('ERROR'); params.setVerbosityDL('VERBOSE'); -params.setOrdering(graph.orderingCOLAMD(initialEstimate)); +params.setOrdering(graph.orderingCOLAMD()); optimizer = DoglegOptimizer(graph, initialEstimate, params); result = optimizer.optimizeSafely(); result.print('final result'); %% Get the corresponding dense matrix -ord = graph.orderingCOLAMD(result); -gfg = graph.linearize(result,ord); -denseAb = gfg.denseJacobian; +ord = graph.orderingCOLAMD(); +gfg = graph.linearize(result); +denseAb = gfg.augmentedJacobian; %% Get sparse matrix A and RHS b IJS = gfg.sparseJacobian_(); diff --git a/matlab/gtsam_examples/Pose2SLAMExample_graph.m b/matlab/gtsam_examples/Pose2SLAMExample_graph.m index b4957cce3..83ec949cc 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_graph.m @@ -36,7 +36,9 @@ toc hold on; plot2DTrajectory(result, 'b-*'); %% Plot Covariance Ellipses +tic marginals = Marginals(graph, result); +toc P={}; for i=1:result.size()-1 pose_i = result.at(i); diff --git a/matlab/gtsam_examples/Pose3SLAMExample_graph.m b/matlab/gtsam_examples/Pose3SLAMExample_graph.m index 01df4fc33..39e48c204 100644 --- a/matlab/gtsam_examples/Pose3SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose3SLAMExample_graph.m @@ -12,6 +12,8 @@ import gtsam.* +%% PLEASE NOTE THAT PLOTTING TAKES A VERY LONG TIME HERE + %% Find data file N = 2500; % dataset = 'sphere_smallnoise.graph'; diff --git a/matlab/gtsam_tests/testUtilities.m b/matlab/gtsam_tests/testUtilities.m new file mode 100644 index 000000000..da8dec789 --- /dev/null +++ b/matlab/gtsam_tests/testUtilities.m @@ -0,0 +1,47 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% GTSAM Copyright 2010, Georgia Tech Research Corporation, +% Atlanta, Georgia 30332-0415 +% All Rights Reserved +% Authors: Frank Dellaert, et al. (see THANKS for the full author list) +% +% See LICENSE for the license information +% +% @brief Checks for results of functions in utilities namespace +% @author Frank Dellaert +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +import gtsam.* + +%% Create keys for variables +x1 = symbol('x',1); x2 = symbol('x',2); x3 = symbol('x',3); + +actual = utilities.createKeyList([1;2;3]); +CHECK('KeyList', isa(actual,'gtsam.KeyList')); +CHECK('size==3', actual.size==3); +CHECK('actual.front==1', actual.front==1); + +actual = utilities.createKeyList('x',[1;2;3]); +CHECK('KeyList', isa(actual,'gtsam.KeyList')); +CHECK('size==3', actual.size==3); +CHECK('actual.front==x1', actual.front==x1); + +actual = utilities.createKeyVector([1;2;3]); +CHECK('KeyVector', isa(actual,'gtsam.KeyVector')); +CHECK('size==3', actual.size==3); +CHECK('actual.at(0)==1', actual.at(0)==1); + +actual = utilities.createKeyVector('x',[1;2;3]); +CHECK('KeyVector', isa(actual,'gtsam.KeyVector')); +CHECK('size==3', actual.size==3); +CHECK('actual.at(0)==x1', actual.at(0)==x1); + +actual = utilities.createKeySet([1;2;3]); +CHECK('KeySet', isa(actual,'gtsam.KeySet')); +CHECK('size==3', actual.size==3); +CHECK('actual.count(1)', actual.count(1)); + +actual = utilities.createKeySet('x',[1;2;3]); +CHECK('KeySet', isa(actual,'gtsam.KeySet')); +CHECK('size==3', actual.size==3); +CHECK('actual.count(x1)', actual.count(x1)); + diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index 2cad40df8..c379179c5 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -33,5 +33,8 @@ testVisualISAMExample display 'Starting: testSerialization' testSerialization +display 'Starting: testUtilities' +testUtilities + % end of tests display 'Tests complete!' diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index b976cca90..6d8a056fc 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -295,7 +295,7 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { // test the diagonal GaussianFactorGraph::shared_ptr linear = optimizer.linearize(); - GaussianFactorGraph damped = optimizer.buildDampedSystem(*linear); + GaussianFactorGraph damped = *optimizer.buildDampedSystem(*linear); VectorValues d = linear->hessianDiagonal(), // expectedDiagonal = d + params.lambdaInitial * d; EXPECT(assert_equal(expectedDiagonal, damped.hessianDiagonal())); @@ -309,7 +309,7 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { EXPECT(assert_equal(expectedGradient,linear->gradientAtZero())); // Check that the gradient is zero for damped system (it is not!) - damped = optimizer.buildDampedSystem(*linear); + damped = *optimizer.buildDampedSystem(*linear); VectorValues actualGradient = damped.gradientAtZero(); EXPECT(assert_equal(expectedGradient,actualGradient)); diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index c3798e5ce..d76556e4a 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -16,13 +16,14 @@ * @author Richard Roberts **/ -#include -#include -#include +#include "Argument.h" + #include #include -#include "Argument.h" +#include +#include +#include using namespace std; using namespace wrap; @@ -31,18 +32,24 @@ using namespace wrap; string Argument::matlabClass(const string& delim) const { string result; BOOST_FOREACH(const string& ns, namespaces) - result += ns + delim; - if (type=="string" || type=="unsigned char" || type=="char") + result += ns + delim; + if (type == "string" || type == "unsigned char" || type == "char") return result + "char"; - if (type=="Vector" || type=="Matrix") + if (type == "Vector" || type == "Matrix") return result + "double"; - if (type=="int" || type=="size_t") + if (type == "int" || type == "size_t") return result + "numeric"; - if (type=="bool") + if (type == "bool") return result + "logical"; return result + type; } +/* ************************************************************************* */ +bool Argument::isScalar() const { + return (type == "bool" || type == "char" || type == "unsigned char" + || type == "int" || type == "size_t" || type == "double"); +} + /* ************************************************************************* */ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << " "; @@ -52,7 +59,8 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { if (is_ptr) // A pointer: emit an "unwrap_shared_ptr" call which returns a pointer - file.oss << "boost::shared_ptr<" << cppType << "> " << name << " = unwrap_shared_ptr< "; + file.oss << "boost::shared_ptr<" << cppType << "> " << name + << " = unwrap_shared_ptr< "; else if (is_ref) // A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer file.oss << cppType << "& " << name << " = *unwrap_shared_ptr< "; @@ -65,23 +73,28 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << cppType << " " << name << " = unwrap< "; file.oss << cppType << " >(" << matlabName; - if (is_ptr || is_ref) file.oss << ", \"ptr_" << matlabUniqueType << "\""; + if (is_ptr || is_ref) + file.oss << ", \"ptr_" << matlabUniqueType << "\""; file.oss << ");" << endl; } /* ************************************************************************* */ string Argument::qualifiedType(const string& delim) const { string result; - BOOST_FOREACH(const string& ns, namespaces) result += ns + delim; + BOOST_FOREACH(const string& ns, namespaces) + result += ns + delim; return result + type; } /* ************************************************************************* */ string ArgumentList::types() const { string str; - bool first=true; + bool first = true; BOOST_FOREACH(Argument arg, *this) { - if (!first) str += ","; str += arg.type; first=false; + if (!first) + str += ","; + str += arg.type; + first = false; } return str; } @@ -89,16 +102,17 @@ string ArgumentList::types() const { /* ************************************************************************* */ string ArgumentList::signature() const { string sig; - bool cap=false; + bool cap = false; BOOST_FOREACH(Argument arg, *this) { BOOST_FOREACH(char ch, arg.type) - if(isupper(ch)) { - sig += ch; - //If there is a capital letter, we don't want to read it below - cap=true; - } - if(!cap) sig += arg.type[0]; + if (isupper(ch)) { + sig += ch; + //If there is a capital letter, we don't want to read it below + cap = true; + } + if (!cap) + sig += arg.type[0]; //Reset to default cap = false; } @@ -109,23 +123,77 @@ string ArgumentList::signature() const { /* ************************************************************************* */ string ArgumentList::names() const { string str; - bool first=true; + bool first = true; BOOST_FOREACH(Argument arg, *this) { - if (!first) str += ","; str += arg.name; first=false; + if (!first) + str += ","; + str += arg.name; + first = false; } return str; } +/* ************************************************************************* */ +bool ArgumentList::allScalar() const { + BOOST_FOREACH(Argument arg, *this) + if (!arg.isScalar()) return false; + return true; +} + /* ************************************************************************* */ void ArgumentList::matlab_unwrap(FileWriter& file, int start) const { int index = start; BOOST_FOREACH(Argument arg, *this) { stringstream buf; buf << "in[" << index << "]"; - arg.matlab_unwrap(file,buf.str()); + arg.matlab_unwrap(file, buf.str()); index++; } } /* ************************************************************************* */ +void ArgumentList::emit_prototype(FileWriter& file, const string& name) const { + file.oss << name << "("; + bool first = true; + BOOST_FOREACH(Argument arg, *this) { + if (!first) + file.oss << ", "; + file.oss << arg.type << " " << arg.name; + first = false; + } + file.oss << ")"; +} +/* ************************************************************************* */ +void ArgumentList::emit_call(FileWriter& file, const ReturnValue& returnVal, + const string& wrapperName, int id, bool staticMethod) const { + returnVal.emit_matlab(file); + file.oss << wrapperName << "(" << id; + if (!staticMethod) + file.oss << ", this"; + file.oss << ", varargin{:});\n"; +} +/* ************************************************************************* */ +void ArgumentList::emit_conditional_call(FileWriter& file, + const ReturnValue& returnVal, const string& wrapperName, int id, + bool staticMethod) const { + // Check nr of arguments + file.oss << "if length(varargin) == " << size(); + if (size() > 0) + file.oss << " && "; + // ...and their types + bool first = true; + for (size_t i = 0; i < size(); i++) { + if (!first) + file.oss << " && "; + file.oss << "isa(varargin{" << i + 1 << "},'" << (*this)[i].matlabClass(".") + << "')"; + first = false; + } + file.oss << "\n"; + + // output call to C++ wrapper + file.oss << " "; + emit_call(file, returnVal, wrapperName, id, staticMethod); +} +/* ************************************************************************* */ diff --git a/wrap/Argument.h b/wrap/Argument.h index f46eaa427..6f791978a 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -19,11 +19,12 @@ #pragma once +#include "FileWriter.h" +#include "ReturnValue.h" + #include #include -#include "FileWriter.h" - namespace wrap { /// Argument class @@ -40,6 +41,9 @@ struct Argument { /// return MATLAB class for use in isa(x,class) std::string matlabClass(const std::string& delim = "") const; + /// Check if will be unwrapped using scalar login in wrap/matlab.h + bool isScalar() const; + /// adds namespaces to type std::string qualifiedType(const std::string& delim = "") const; @@ -59,6 +63,9 @@ struct ArgumentList: public std::vector { /// create a comma-separated string listing all argument names, used in m-files std::string names() const; + /// Check if all arguments scalar + bool allScalar() const; + // MATLAB code generation: /** @@ -68,6 +75,32 @@ struct ArgumentList: public std::vector { */ void matlab_unwrap(FileWriter& file, int start = 0) const; // MATLAB to C++ + /** + * emit MATLAB prototype + * @param file output stream + * @param name of method or function + */ + void emit_prototype(FileWriter& file, const std::string& name) const; + + /** + * emit emit MATLAB call to wrapper + * @param file output stream + * @param returnVal the return value + * @param wrapperName of method or function + * @param staticMethod flag to emit "this" in call + */ + void emit_call(FileWriter& file, const ReturnValue& returnVal, + const std::string& wrapperName, int id, bool staticMethod = false) const; + + /** + * emit conditional MATLAB call to wrapper (checking arguments first) + * @param file output stream + * @param returnVal the return value + * @param wrapperName of method or function + * @param staticMethod flag to emit "this" in call + */ + void emit_conditional_call(FileWriter& file, const ReturnValue& returnVal, + const std::string& wrapperName, int id, bool staticMethod = false) const; }; } // \namespace wrap diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 3b8c41041..075c98811 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -7,62 +7,62 @@ * See LICENSE for the license information - * -------------------------------------------------------------------------- */ - + * -------------------------------------------------------------------------- */ + /** * @file Class.cpp * @author Frank Dellaert * @author Andrew Melim * @author Richard Roberts - **/ - + **/ + +#include "Class.h" +#include "utilities.h" +#include "Argument.h" + +#include +#include + #include #include #include //#include // on Linux GCC: fails with error regarding needing C++0x std flags //#include // same failure as above #include // works on Linux GCC - -#include -#include - -#include "Class.h" -#include "utilities.h" -#include "Argument.h" - -using namespace std; -using namespace wrap; - -/* ************************************************************************* */ -void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, - const TypeAttributesTable& typeAttributes, - FileWriter& wrapperFile, vector& functionNames) const { - +using namespace std; +using namespace wrap; + +/* ************************************************************************* */ +void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, + const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, + vector& functionNames) const { + // Create namespace folders - createNamespaceStructure(namespaces, toolboxPath); - + createNamespaceStructure(namespaces, toolboxPath); + // open destination classFile - string classFile = toolboxPath; - if(!namespaces.empty()) - classFile += "/+" + wrap::qualifiedName("/+", namespaces); - classFile += "/" + name + ".m"; - FileWriter proxyFile(classFile, verbose_, "%"); - + string classFile = toolboxPath; + if (!namespaces.empty()) + classFile += "/+" + wrap::qualifiedName("/+", namespaces); + classFile += "/" + name + ".m"; + FileWriter proxyFile(classFile, verbose_, "%"); + // get the name of actual matlab object - const string matlabQualName = qualifiedName("."), matlabUniqueName = qualifiedName(), cppName = qualifiedName("::"); - const string matlabBaseName = wrap::qualifiedName(".", qualifiedParent); - const string cppBaseName = wrap::qualifiedName("::", qualifiedParent); - + const string matlabQualName = qualifiedName("."), matlabUniqueName = + qualifiedName(), cppName = qualifiedName("::"); + const string matlabBaseName = wrap::qualifiedName(".", qualifiedParent); + const string cppBaseName = wrap::qualifiedName("::", qualifiedParent); + // emit class proxy code // we want our class to inherit the handle class for memory purposes - const string parent = qualifiedParent.empty() ? "handle" : matlabBaseName; + const string parent = qualifiedParent.empty() ? "handle" : matlabBaseName; comment_fragment(proxyFile); - proxyFile.oss << "classdef " << name << " < " << parent << endl; + proxyFile.oss << "classdef " << name << " < " << parent << endl; proxyFile.oss << " properties\n"; proxyFile.oss << " ptr_" << matlabUniqueName << " = 0\n"; proxyFile.oss << " end\n"; proxyFile.oss << " methods\n"; - + // Constructor proxyFile.oss << " function obj = " << name << "(varargin)\n"; // Special pointer constructors - one in MATLAB to create an object and @@ -72,284 +72,325 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, // other wrap modules - to add these to their collectors the pointer is // passed from one C++ module into matlab then back into the other C++ // module. - pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName, functionNames); - wrapperFile.oss << "\n"; + pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName, + functionNames); + wrapperFile.oss << "\n"; // Regular constructors - BOOST_FOREACH(ArgumentList a, constructor.args_list) - { - const int id = (int)functionNames.size(); - constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(), id, a); - const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile, - cppName, matlabUniqueName, cppBaseName, id, a); - wrapperFile.oss << "\n"; - functionNames.push_back(wrapFunctionName); - } - proxyFile.oss << " else\n"; - proxyFile.oss << " error('Arguments do not match any overload of " << matlabQualName << " constructor');\n"; - proxyFile.oss << " end\n"; - if(!qualifiedParent.empty()) - proxyFile.oss << " obj = obj@" << matlabBaseName << "(uint64(" << ptr_constructor_key << "), base_ptr);\n"; - proxyFile.oss << " obj.ptr_" << matlabUniqueName << " = my_ptr;\n"; - proxyFile.oss << " end\n\n"; - + BOOST_FOREACH(ArgumentList a, constructor.args_list) { + const int id = (int) functionNames.size(); + constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(), + id, a); + const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile, + cppName, matlabUniqueName, cppBaseName, id, a); + wrapperFile.oss << "\n"; + functionNames.push_back(wrapFunctionName); + } + proxyFile.oss << " else\n"; + proxyFile.oss << " error('Arguments do not match any overload of " + << matlabQualName << " constructor');\n"; + proxyFile.oss << " end\n"; + if (!qualifiedParent.empty()) + proxyFile.oss << " obj = obj@" << matlabBaseName << "(uint64(" + << ptr_constructor_key << "), base_ptr);\n"; + proxyFile.oss << " obj.ptr_" << matlabUniqueName << " = my_ptr;\n"; + proxyFile.oss << " end\n\n"; + // Deconstructor - { - const int id = (int)functionNames.size(); - deconstructor.proxy_fragment(proxyFile, wrapperName, matlabUniqueName, id); - proxyFile.oss << "\n"; - const string functionName = deconstructor.wrapper_fragment(wrapperFile, cppName, matlabUniqueName, id); - wrapperFile.oss << "\n"; - functionNames.push_back(functionName); - } - proxyFile.oss << " function display(obj), obj.print(''); end\n %DISPLAY Calls print on the object\n"; - proxyFile.oss << " function disp(obj), obj.display; end\n %DISP Calls print on the object\n"; - + { + const int id = (int) functionNames.size(); + deconstructor.proxy_fragment(proxyFile, wrapperName, matlabUniqueName, id); + proxyFile.oss << "\n"; + const string functionName = deconstructor.wrapper_fragment(wrapperFile, + cppName, matlabUniqueName, id); + wrapperFile.oss << "\n"; + functionNames.push_back(functionName); + } + proxyFile.oss + << " function display(obj), obj.print(''); end\n %DISPLAY Calls print on the object\n"; + proxyFile.oss + << " function disp(obj), obj.display; end\n %DISP Calls print on the object\n"; + // Methods - BOOST_FOREACH(const Methods::value_type& name_m, methods) { - const Method& m = name_m.second; - m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, typeAttributes, functionNames); - proxyFile.oss << "\n"; - wrapperFile.oss << "\n"; - } + BOOST_FOREACH(const Methods::value_type& name_m, methods) { + const Method& m = name_m.second; + m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, + matlabUniqueName, wrapperName, typeAttributes, functionNames); + proxyFile.oss << "\n"; + wrapperFile.oss << "\n"; + } if (hasSerialization) serialization_fragments(proxyFile, wrapperFile, wrapperName, functionNames); - - proxyFile.oss << " end\n"; - proxyFile.oss << "\n"; - proxyFile.oss << " methods(Static = true)\n"; - + + proxyFile.oss << " end\n"; + proxyFile.oss << "\n"; + proxyFile.oss << " methods(Static = true)\n"; + // Static methods - BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) { - const StaticMethod& m = name_m.second; - m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, typeAttributes, functionNames); - proxyFile.oss << "\n"; - wrapperFile.oss << "\n"; - } + BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) { + const StaticMethod& m = name_m.second; + m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, + matlabUniqueName, wrapperName, typeAttributes, functionNames); + proxyFile.oss << "\n"; + wrapperFile.oss << "\n"; + } if (hasSerialization) - deserialization_fragments(proxyFile, wrapperFile, wrapperName, functionNames); + deserialization_fragments(proxyFile, wrapperFile, wrapperName, + functionNames); proxyFile.oss << " end\n"; proxyFile.oss << "end\n"; - + // Close file - proxyFile.emit(true); -} - -/* ************************************************************************* */ -string Class::qualifiedName(const string& delim) const { - return ::wrap::qualifiedName(delim, namespaces, name); -} - -/* ************************************************************************* */ -void Class::pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, const string& wrapperName, vector& functionNames) const { - - const string matlabUniqueName = qualifiedName(), cppName = qualifiedName("::"); - const string baseCppName = wrap::qualifiedName("::", qualifiedParent); - - const int collectorInsertId = (int)functionNames.size(); - const string collectorInsertFunctionName = matlabUniqueName + "_collectorInsertAndMakeBase_" + boost::lexical_cast(collectorInsertId); - functionNames.push_back(collectorInsertFunctionName); - - int upcastFromVoidId; - string upcastFromVoidFunctionName; - if(isVirtual) { - upcastFromVoidId = (int)functionNames.size(); - upcastFromVoidFunctionName = matlabUniqueName + "_upcastFromVoid_" + boost::lexical_cast(upcastFromVoidId); - functionNames.push_back(upcastFromVoidFunctionName); - } - + proxyFile.emit(true); +} + +/* ************************************************************************* */ +string Class::qualifiedName(const string& delim) const { + return ::wrap::qualifiedName(delim, namespaces, name); +} + +/* ************************************************************************* */ +void Class::pointer_constructor_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, const string& wrapperName, + vector& functionNames) const { + + const string matlabUniqueName = qualifiedName(), cppName = qualifiedName( + "::"); + const string baseCppName = wrap::qualifiedName("::", qualifiedParent); + + const int collectorInsertId = (int) functionNames.size(); + const string collectorInsertFunctionName = matlabUniqueName + + "_collectorInsertAndMakeBase_" + + boost::lexical_cast(collectorInsertId); + functionNames.push_back(collectorInsertFunctionName); + + int upcastFromVoidId; + string upcastFromVoidFunctionName; + if (isVirtual) { + upcastFromVoidId = (int) functionNames.size(); + upcastFromVoidFunctionName = matlabUniqueName + "_upcastFromVoid_" + + boost::lexical_cast(upcastFromVoidId); + functionNames.push_back(upcastFromVoidFunctionName); + } + // MATLAB constructor that assigns pointer to matlab object then calls c++ // function to add the object to the collector. - if(isVirtual) { - proxyFile.oss << " if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void')))"; - } else { - proxyFile.oss << " if nargin == 2"; - } - proxyFile.oss << " && isa(varargin{1}, 'uint64') && varargin{1} == uint64(" << ptr_constructor_key << ")\n"; - if(isVirtual) { - proxyFile.oss << " if nargin == 2\n"; - proxyFile.oss << " my_ptr = varargin{2};\n"; - proxyFile.oss << " else\n"; - proxyFile.oss << " my_ptr = " << wrapperName << "(" << upcastFromVoidId << ", varargin{2});\n"; - proxyFile.oss << " end\n"; - } else { - proxyFile.oss << " my_ptr = varargin{2};\n"; - } - if(qualifiedParent.empty()) // If this class has a base class, we'll get a base class pointer back - proxyFile.oss << " "; - else - proxyFile.oss << " base_ptr = "; + if (isVirtual) { + proxyFile.oss + << " if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void')))"; + } else { + proxyFile.oss << " if nargin == 2"; + } + proxyFile.oss << " && isa(varargin{1}, 'uint64') && varargin{1} == uint64(" + << ptr_constructor_key << ")\n"; + if (isVirtual) { + proxyFile.oss << " if nargin == 2\n"; + proxyFile.oss << " my_ptr = varargin{2};\n"; + proxyFile.oss << " else\n"; + proxyFile.oss << " my_ptr = " << wrapperName << "(" + << upcastFromVoidId << ", varargin{2});\n"; + proxyFile.oss << " end\n"; + } else { + proxyFile.oss << " my_ptr = varargin{2};\n"; + } + if (qualifiedParent.empty()) // If this class has a base class, we'll get a base class pointer back + proxyFile.oss << " "; + else + proxyFile.oss << " base_ptr = "; proxyFile.oss << wrapperName << "(" << collectorInsertId << ", my_ptr);\n"; // Call collector insert and get base class ptr - + // C++ function to add pointer from MATLAB to collector. The pointer always // comes from a C++ return value; this mechanism allows the object to be added // to a collector in a different wrap module. If this class has a base class, // a new pointer to the base class is allocated and returned. - wrapperFile.oss << "void " << collectorInsertFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; - wrapperFile.oss << "{\n"; - wrapperFile.oss << " mexAtExit(&_deleteAllObjects);\n"; + wrapperFile.oss << "void " << collectorInsertFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + wrapperFile.oss << "{\n"; + wrapperFile.oss << " mexAtExit(&_deleteAllObjects);\n"; // Typedef boost::shared_ptr - wrapperFile.oss << " typedef boost::shared_ptr<" << cppName << "> Shared;\n"; - wrapperFile.oss << "\n"; + wrapperFile.oss << " typedef boost::shared_ptr<" << cppName << "> Shared;\n"; + wrapperFile.oss << "\n"; // Get self pointer passed in - wrapperFile.oss << " Shared *self = *reinterpret_cast (mxGetData(in[0]));\n"; + wrapperFile.oss + << " Shared *self = *reinterpret_cast (mxGetData(in[0]));\n"; // Add to collector - wrapperFile.oss << " collector_" << matlabUniqueName << ".insert(self);\n"; + wrapperFile.oss << " collector_" << matlabUniqueName << ".insert(self);\n"; // If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy) - if(!qualifiedParent.empty()) { - wrapperFile.oss << "\n"; - wrapperFile.oss << " typedef boost::shared_ptr<" << baseCppName << "> SharedBase;\n"; - wrapperFile.oss << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; - wrapperFile.oss << " *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self);\n"; - } - wrapperFile.oss << "}\n"; - + if (!qualifiedParent.empty()) { + wrapperFile.oss << "\n"; + wrapperFile.oss << " typedef boost::shared_ptr<" << baseCppName + << "> SharedBase;\n"; + wrapperFile.oss + << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; + wrapperFile.oss + << " *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self);\n"; + } + wrapperFile.oss << "}\n"; + // If this is a virtual function, C++ function to dynamic upcast it from a // shared_ptr. This mechanism allows automatic dynamic creation of the // real underlying derived-most class when a C++ method returns a virtual // base class. - if(isVirtual) - wrapperFile.oss << - "\n" - "void " << upcastFromVoidFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {\n" - " mexAtExit(&_deleteAllObjects);\n" - " typedef boost::shared_ptr<" << cppName << "> Shared;\n" - " boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0]));\n" - " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n" - " Shared *self = new Shared(boost::static_pointer_cast<" << cppName << ">(*asVoid));\n" - " *reinterpret_cast(mxGetData(out[0])) = self;\n" - "}\n"; -} - -/* ************************************************************************* */ -vector expandArgumentListsTemplate(const vector& argLists, const string& templateArg, const vector& instName, const std::vector& expandedClassNamespace, const string& expandedClassName) { - vector result; - BOOST_FOREACH(const ArgumentList& argList, argLists) { - ArgumentList instArgList; - BOOST_FOREACH(const Argument& arg, argList) { - Argument instArg = arg; - if(arg.type == templateArg) { - instArg.namespaces.assign(instName.begin(), instName.end()-1); - instArg.type = instName.back(); - } else if(arg.type == "This") { - instArg.namespaces.assign(expandedClassNamespace.begin(), expandedClassNamespace.end()); - instArg.type = expandedClassName; - } - instArgList.push_back(instArg); - } - result.push_back(instArgList); - } - return result; -} - -/* ************************************************************************* */ -template -map expandMethodTemplate(const map& methods, const string& templateArg, const vector& instName, const std::vector& expandedClassNamespace, const string& expandedClassName) { - map result; - typedef pair Name_Method; - BOOST_FOREACH(const Name_Method& name_method, methods) { - const METHOD& method = name_method.second; - METHOD instMethod = method; - instMethod.argLists = expandArgumentListsTemplate(method.argLists, templateArg, instName, expandedClassNamespace, expandedClassName); - instMethod.returnVals.clear(); - BOOST_FOREACH(const ReturnValue& retVal, method.returnVals) { - ReturnValue instRetVal = retVal; - if(retVal.type1 == templateArg) { - instRetVal.namespaces1.assign(instName.begin(), instName.end()-1); - instRetVal.type1 = instName.back(); - } else if(retVal.type1 == "This") { - instRetVal.namespaces1.assign(expandedClassNamespace.begin(), expandedClassNamespace.end()); - instRetVal.type1 = expandedClassName; - } - if(retVal.type2 == templateArg) { - instRetVal.namespaces2.assign(instName.begin(), instName.end()-1); - instRetVal.type2 = instName.back(); - } else if(retVal.type1 == "This") { - instRetVal.namespaces2.assign(expandedClassNamespace.begin(), expandedClassNamespace.end()); - instRetVal.type2 = expandedClassName; - } - instMethod.returnVals.push_back(instRetVal); - } - result.insert(make_pair(name_method.first, instMethod)); - } - return result; -} - -/* ************************************************************************* */ -Class expandClassTemplate(const Class& cls, const string& templateArg, const vector& instName, const std::vector& expandedClassNamespace, const string& expandedClassName) { - Class inst; - inst.name = cls.name; - inst.templateArgs = cls.templateArgs; - inst.typedefName = cls.typedefName; - inst.isVirtual = cls.isVirtual; + if (isVirtual) + wrapperFile.oss << "\n" + "void " << upcastFromVoidFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {\n" + " mexAtExit(&_deleteAllObjects);\n" + " typedef boost::shared_ptr<" << cppName + << "> Shared;\n" + " boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0]));\n" + " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n" + " Shared *self = new Shared(boost::static_pointer_cast<" << cppName + << ">(*asVoid));\n" + " *reinterpret_cast(mxGetData(out[0])) = self;\n" + "}\n"; +} + +/* ************************************************************************* */ +vector expandArgumentListsTemplate( + const vector& argLists, const string& templateArg, + const vector& instName, + const std::vector& expandedClassNamespace, + const string& expandedClassName) { + vector result; + BOOST_FOREACH(const ArgumentList& argList, argLists) { + ArgumentList instArgList; + BOOST_FOREACH(const Argument& arg, argList) { + Argument instArg = arg; + if (arg.type == templateArg) { + instArg.namespaces.assign(instName.begin(), instName.end() - 1); + instArg.type = instName.back(); + } else if (arg.type == "This") { + instArg.namespaces.assign(expandedClassNamespace.begin(), + expandedClassNamespace.end()); + instArg.type = expandedClassName; + } + instArgList.push_back(instArg); + } + result.push_back(instArgList); + } + return result; +} + +/* ************************************************************************* */ +template +map expandMethodTemplate(const map& methods, + const string& templateArg, const vector& instName, + const std::vector& expandedClassNamespace, + const string& expandedClassName) { + map result; + typedef pair Name_Method; + BOOST_FOREACH(const Name_Method& name_method, methods) { + const METHOD& method = name_method.second; + METHOD instMethod = method; + instMethod.argLists = expandArgumentListsTemplate(method.argLists, + templateArg, instName, expandedClassNamespace, expandedClassName); + instMethod.returnVals.clear(); + BOOST_FOREACH(const ReturnValue& retVal, method.returnVals) { + ReturnValue instRetVal = retVal; + if (retVal.type1 == templateArg) { + instRetVal.namespaces1.assign(instName.begin(), instName.end() - 1); + instRetVal.type1 = instName.back(); + } else if (retVal.type1 == "This") { + instRetVal.namespaces1.assign(expandedClassNamespace.begin(), + expandedClassNamespace.end()); + instRetVal.type1 = expandedClassName; + } + if (retVal.type2 == templateArg) { + instRetVal.namespaces2.assign(instName.begin(), instName.end() - 1); + instRetVal.type2 = instName.back(); + } else if (retVal.type1 == "This") { + instRetVal.namespaces2.assign(expandedClassNamespace.begin(), + expandedClassNamespace.end()); + instRetVal.type2 = expandedClassName; + } + instMethod.returnVals.push_back(instRetVal); + } + result.insert(make_pair(name_method.first, instMethod)); + } + return result; +} + +/* ************************************************************************* */ +Class expandClassTemplate(const Class& cls, const string& templateArg, + const vector& instName, + const std::vector& expandedClassNamespace, + const string& expandedClassName) { + Class inst; + inst.name = cls.name; + inst.templateArgs = cls.templateArgs; + inst.typedefName = cls.typedefName; + inst.isVirtual = cls.isVirtual; inst.isSerializable = cls.isSerializable; - inst.qualifiedParent = cls.qualifiedParent; - inst.methods = expandMethodTemplate(cls.methods, templateArg, instName, expandedClassNamespace, expandedClassName); - inst.static_methods = expandMethodTemplate(cls.static_methods, templateArg, instName, expandedClassNamespace, expandedClassName); - inst.namespaces = cls.namespaces; - inst.constructor = cls.constructor; - inst.constructor.args_list = expandArgumentListsTemplate(cls.constructor.args_list, templateArg, instName, expandedClassNamespace, expandedClassName); - inst.constructor.name = inst.name; - inst.deconstructor = cls.deconstructor; - inst.deconstructor.name = inst.name; - inst.verbose_ = cls.verbose_; - return inst; -} - -/* ************************************************************************* */ -vector Class::expandTemplate(const string& templateArg, const vector >& instantiations) const { - vector result; - BOOST_FOREACH(const vector& instName, instantiations) { - const string expandedName = name + instName.back(); - Class inst = expandClassTemplate(*this, templateArg, instName, this->namespaces, expandedName); - inst.name = expandedName; - inst.templateArgs.clear(); - inst.typedefName = qualifiedName("::") + "<" + wrap::qualifiedName("::", instName) + ">"; - result.push_back(inst); - } - return result; -} - -/* ************************************************************************* */ -Class Class::expandTemplate(const string& templateArg, const vector& instantiation, const std::vector& expandedClassNamespace, const string& expandedClassName) const { - return expandClassTemplate(*this, templateArg, instantiation, expandedClassNamespace, expandedClassName); -} - -/* ************************************************************************* */ -std::string Class::getTypedef() const { - string result; - BOOST_FOREACH(const string& namesp, namespaces) { - result += ("namespace " + namesp + " { "); - } - result += ("typedef " + typedefName + " " + name + ";"); - for (size_t i = 0; i Class::expandTemplate(const string& templateArg, + const vector >& instantiations) const { + vector result; + BOOST_FOREACH(const vector& instName, instantiations) { + const string expandedName = name + instName.back(); + Class inst = expandClassTemplate(*this, templateArg, instName, + this->namespaces, expandedName); + inst.name = expandedName; + inst.templateArgs.clear(); + inst.typedefName = qualifiedName("::") + "<" + + wrap::qualifiedName("::", instName) + ">"; + result.push_back(inst); + } + return result; +} + +/* ************************************************************************* */ +Class Class::expandTemplate(const string& templateArg, + const vector& instantiation, + const std::vector& expandedClassNamespace, + const string& expandedClassName) const { + return expandClassTemplate(*this, templateArg, instantiation, + expandedClassNamespace, expandedClassName); +} + +/* ************************************************************************* */ +std::string Class::getTypedef() const { + string result; + BOOST_FOREACH(const string& namesp, namespaces) { + result += ("namespace " + namesp + " { "); + } + result += ("typedef " + typedefName + " " + name + ";"); + for (size_t i = 0; i < namespaces.size(); ++i) { + result += " }"; + } + return result; +} + +/* ************************************************************************* */ void Class::comment_fragment(FileWriter& proxyFile) const { - proxyFile.oss << "%class " << name - << ", see Doxygen page for details\n"; + proxyFile.oss << "%class " << name << ", see Doxygen page for details\n"; proxyFile.oss << "%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; if (!constructor.args_list.empty()) proxyFile.oss << "%\n%-------Constructors-------\n"; BOOST_FOREACH(ArgumentList argList, constructor.args_list) { - string up_name = boost::to_upper_copy(name); - proxyFile.oss << "%" << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ")\n"; + proxyFile.oss << "%"; + argList.emit_prototype(proxyFile, name); + proxyFile.oss << "\n"; } if (!methods.empty()) @@ -357,17 +398,9 @@ void Class::comment_fragment(FileWriter& proxyFile) const { BOOST_FOREACH(const Methods::value_type& name_m, methods) { const Method& m = name_m.second; BOOST_FOREACH(ArgumentList argList, m.argLists) { - string up_name = boost::to_upper_copy(m.name); - proxyFile.oss << "%" << m.name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ") : returns " + proxyFile.oss << "%"; + argList.emit_prototype(proxyFile, m.name); + proxyFile.oss << " : returns " << m.returnVals[0].return_type(false, m.returnVals[0].pair) << endl; } } @@ -377,18 +410,9 @@ void Class::comment_fragment(FileWriter& proxyFile) const { BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) { const StaticMethod& m = name_m.second; BOOST_FOREACH(ArgumentList argList, m.argLists) { - string up_name = boost::to_upper_copy(m.name); - proxyFile.oss << "%" << m.name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - - proxyFile.oss << ") : returns " + proxyFile.oss << "%"; + argList.emit_prototype(proxyFile, m.name); + proxyFile.oss << " : returns " << m.returnVals[0].return_type(false, m.returnVals[0].pair) << endl; } } @@ -396,15 +420,17 @@ void Class::comment_fragment(FileWriter& proxyFile) const { if (hasSerialization) { proxyFile.oss << "%\n%-------Serialization Interface-------\n"; proxyFile.oss << "%string_serialize() : returns string\n"; - proxyFile.oss << "%string_deserialize(string serialized) : returns " << this->name << "\n"; + proxyFile.oss << "%string_deserialize(string serialized) : returns " + << this->name << "\n"; } proxyFile.oss << "%\n"; } -/* ************************************************************************* */ -void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const std::string& wrapperName, std::vector& functionNames) const { +/* ************************************************************************* */ +void Class::serialization_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, const std::string& wrapperName, + std::vector& functionNames) const { //void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) //{ @@ -418,30 +444,34 @@ void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFi //} int serialize_id = functionNames.size(); - const string - matlabQualName = qualifiedName("."), - matlabUniqueName = qualifiedName(), - cppClassName = qualifiedName("::"); - const string wrapFunctionNameSerialize = matlabUniqueName + "_string_serialize_" + boost::lexical_cast(serialize_id); + const string matlabQualName = qualifiedName("."), matlabUniqueName = + qualifiedName(), cppClassName = qualifiedName("::"); + const string wrapFunctionNameSerialize = matlabUniqueName + + "_string_serialize_" + boost::lexical_cast(serialize_id); functionNames.push_back(wrapFunctionNameSerialize); // call //void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) - wrapperFile.oss << "void " << wrapFunctionNameSerialize << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + wrapperFile.oss << "void " << wrapFunctionNameSerialize + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; wrapperFile.oss << "{\n"; - wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; + wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName + << "> Shared;" << endl; // check arguments - for serialize, no arguments // example: checkArguments("string_serialize",nargout,nargin-1,0); - wrapperFile.oss << " checkArguments(\"string_serialize\",nargout,nargin-1,0);\n"; + wrapperFile.oss + << " checkArguments(\"string_serialize\",nargout,nargin-1,0);\n"; // get class pointer // example: Shared obj = unwrap_shared_ptr(in[0], "ptr_Point3"); - wrapperFile.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; + wrapperFile.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName + << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; // Serialization boilerplate wrapperFile.oss << " std::ostringstream out_archive_stream;\n"; - wrapperFile.oss << " boost::archive::text_oarchive out_archive(out_archive_stream);\n"; + wrapperFile.oss + << " boost::archive::text_oarchive out_archive(out_archive_stream);\n"; wrapperFile.oss << " out_archive << *obj;\n"; wrapperFile.oss << " out[0] = wrap< string >(out_archive_stream.str());\n"; @@ -459,13 +489,19 @@ void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFi // end // end - proxyFile.oss << " function varargout = string_serialize(this, varargin)\n"; - proxyFile.oss << " % STRING_SERIALIZE usage: string_serialize() : returns string\n"; - proxyFile.oss << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; + proxyFile.oss + << " function varargout = string_serialize(this, varargin)\n"; + proxyFile.oss + << " % STRING_SERIALIZE usage: string_serialize() : returns string\n"; + proxyFile.oss + << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; proxyFile.oss << " if length(varargin) == 0\n"; - proxyFile.oss << " varargout{1} = " << wrapperName << "(" << boost::lexical_cast(serialize_id) << ", this, varargin{:});\n"; + proxyFile.oss << " varargout{1} = " << wrapperName << "(" + << boost::lexical_cast(serialize_id) << ", this, varargin{:});\n"; proxyFile.oss << " else\n"; - proxyFile.oss << " error('Arguments do not match any overload of function " << matlabQualName << ".string_serialize');\n"; + proxyFile.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << ".string_serialize');\n"; proxyFile.oss << " end\n"; proxyFile.oss << " end\n\n"; @@ -476,14 +512,16 @@ void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFi // end proxyFile.oss << " function sobj = saveobj(obj)\n"; - proxyFile.oss << " % SAVEOBJ Saves the object to a matlab-readable format\n"; + proxyFile.oss + << " % SAVEOBJ Saves the object to a matlab-readable format\n"; proxyFile.oss << " sobj = obj.string_serialize();\n"; proxyFile.oss << " end\n"; } /* ************************************************************************* */ -void Class::deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const std::string& wrapperName, std::vector& functionNames) const { +void Class::deserialization_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, const std::string& wrapperName, + std::vector& functionNames) const { //void Point3_string_deserialize_18(int nargout, mxArray *out[], int nargin, const mxArray *in[]) //{ // typedef boost::shared_ptr Shared; @@ -495,32 +533,36 @@ void Class::deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapper // in_archive >> *output; // out[0] = wrap_shared_ptr(output,"Point3", false); //} - int deserialize_id = functionNames.size(); - const string - matlabQualName = qualifiedName("."), - matlabUniqueName = qualifiedName(), - cppClassName = qualifiedName("::"); - const string wrapFunctionNameDeserialize = matlabUniqueName + "_string_deserialize_" + boost::lexical_cast(deserialize_id); - functionNames.push_back(wrapFunctionNameDeserialize); + int deserialize_id = functionNames.size(); + const string matlabQualName = qualifiedName("."), matlabUniqueName = + qualifiedName(), cppClassName = qualifiedName("::"); + const string wrapFunctionNameDeserialize = matlabUniqueName + + "_string_deserialize_" + boost::lexical_cast(deserialize_id); + functionNames.push_back(wrapFunctionNameDeserialize); - // call - wrapperFile.oss << "void " << wrapFunctionNameDeserialize << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; - wrapperFile.oss << "{\n"; - wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; + // call + wrapperFile.oss << "void " << wrapFunctionNameDeserialize + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + wrapperFile.oss << "{\n"; + wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName + << "> Shared;" << endl; - // check arguments - for deserialize, 1 string argument - wrapperFile.oss << " checkArguments(\"" << matlabUniqueName << ".string_deserialize\",nargout,nargin,1);\n"; + // check arguments - for deserialize, 1 string argument + wrapperFile.oss << " checkArguments(\"" << matlabUniqueName + << ".string_deserialize\",nargout,nargin,1);\n"; - // string argument with deserialization boilerplate - wrapperFile.oss << " string serialized = unwrap< string >(in[0]);\n"; - wrapperFile.oss << " std::istringstream in_archive_stream(serialized);\n"; - wrapperFile.oss << " boost::archive::text_iarchive in_archive(in_archive_stream);\n"; - wrapperFile.oss << " Shared output(new " << cppClassName << "());\n"; - wrapperFile.oss << " in_archive >> *output;\n"; - wrapperFile.oss << " out[0] = wrap_shared_ptr(output,\"" << matlabQualName << "\", false);\n"; - wrapperFile.oss << "}\n"; + // string argument with deserialization boilerplate + wrapperFile.oss << " string serialized = unwrap< string >(in[0]);\n"; + wrapperFile.oss << " std::istringstream in_archive_stream(serialized);\n"; + wrapperFile.oss + << " boost::archive::text_iarchive in_archive(in_archive_stream);\n"; + wrapperFile.oss << " Shared output(new " << cppClassName << "());\n"; + wrapperFile.oss << " in_archive >> *output;\n"; + wrapperFile.oss << " out[0] = wrap_shared_ptr(output,\"" << matlabQualName + << "\", false);\n"; + wrapperFile.oss << "}\n"; - // Generate matlab function + // Generate matlab function // function varargout = string_deserialize(varargin) // % STRING_DESERIALIZE usage: string_deserialize() : returns Point3 // % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html @@ -531,32 +573,40 @@ void Class::deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapper // end // end - proxyFile.oss << " function varargout = string_deserialize(varargin)\n"; - proxyFile.oss << " % STRING_DESERIALIZE usage: string_deserialize() : returns " << matlabQualName << "\n"; - proxyFile.oss << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; - proxyFile.oss << " if length(varargin) == 1\n"; - proxyFile.oss << " varargout{1} = " << wrapperName << "(" << boost::lexical_cast(deserialize_id) << ", varargin{:});\n"; - proxyFile.oss << " else\n"; - proxyFile.oss << " error('Arguments do not match any overload of function " << matlabQualName << ".string_deserialize');\n"; - proxyFile.oss << " end\n"; - proxyFile.oss << " end\n\n"; + proxyFile.oss << " function varargout = string_deserialize(varargin)\n"; + proxyFile.oss + << " % STRING_DESERIALIZE usage: string_deserialize() : returns " + << matlabQualName << "\n"; + proxyFile.oss + << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; + proxyFile.oss << " if length(varargin) == 1\n"; + proxyFile.oss << " varargout{1} = " << wrapperName << "(" + << boost::lexical_cast(deserialize_id) << ", varargin{:});\n"; + proxyFile.oss << " else\n"; + proxyFile.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << ".string_deserialize');\n"; + proxyFile.oss << " end\n"; + proxyFile.oss << " end\n\n"; - - // Generate matlab load function + // Generate matlab load function // function obj = loadobj(sobj) // % LOADOBJ Saves the object to a matlab-readable format // obj = Point3.string_deserialize(sobj); // end - proxyFile.oss << " function obj = loadobj(sobj)\n"; - proxyFile.oss << " % LOADOBJ Saves the object to a matlab-readable format\n"; - proxyFile.oss << " obj = " << matlabQualName << ".string_deserialize(sobj);\n"; - proxyFile.oss << " end" << endl; + proxyFile.oss << " function obj = loadobj(sobj)\n"; + proxyFile.oss + << " % LOADOBJ Saves the object to a matlab-readable format\n"; + proxyFile.oss << " obj = " << matlabQualName + << ".string_deserialize(sobj);\n"; + proxyFile.oss << " end" << endl; } /* ************************************************************************* */ std::string Class::getSerializationExport() const { //BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsamSharedDiagonal"); - return "BOOST_CLASS_EXPORT_GUID(" + qualifiedName("::") + ", \"" + qualifiedName() + "\");"; + return "BOOST_CLASS_EXPORT_GUID(" + qualifiedName("::") + ", \"" + + qualifiedName() + "\");"; } /* ************************************************************************* */ diff --git a/wrap/Class.h b/wrap/Class.h index f5267cee2..2ca976f66 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -27,7 +27,6 @@ #include "Method.h" #include "StaticMethod.h" #include "TypeAttributesTable.h" -#include namespace wrap { diff --git a/wrap/Constructor.h b/wrap/Constructor.h index 96d1030a1..5438c515c 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -18,11 +18,11 @@ #pragma once +#include "Argument.h" + #include #include -#include "Argument.h" - namespace wrap { // Constructor class @@ -34,7 +34,7 @@ struct Constructor { } // Then the instance variables are set directly by the Module constructor - std::vector args_list; + std::vector args_list; std::string name; bool verbose_; @@ -50,21 +50,18 @@ struct Constructor { * if nargin == 2, obj.self = new_Pose3_RP(varargin{1},varargin{2}); end */ void proxy_fragment(FileWriter& file, const std::string& wrapperName, - bool hasParent, const int id, const ArgumentList args) const; + bool hasParent, const int id, const ArgumentList args) const; /// cpp wrapper std::string wrapper_fragment(FileWriter& file, - const std::string& cppClassName, - const std::string& matlabUniqueName, - const std::string& cppBaseClassName, - int id, - const ArgumentList& al) const; + const std::string& cppClassName, const std::string& matlabUniqueName, + const std::string& cppBaseClassName, int id, + const ArgumentList& al) const; /// constructor function void generate_construct(FileWriter& file, const std::string& cppClassName, - std::vector& args_list) const; - + std::vector& args_list) const; + }; - } // \namespace wrap diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp index 16f6d48f1..25e1dcedb 100644 --- a/wrap/GlobalFunction.cpp +++ b/wrap/GlobalFunction.cpp @@ -17,7 +17,8 @@ using namespace std; /* ************************************************************************* */ void GlobalFunction::addOverload(bool verbose, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal, const StrVec& ns_stack) { + const ArgumentList& args, const ReturnValue& retVal, + const StrVec& ns_stack) { this->verbose_ = verbose; this->name = name; this->argLists.push_back(args); @@ -26,16 +27,16 @@ void GlobalFunction::addOverload(bool verbose, const std::string& name, } /* ************************************************************************* */ -void GlobalFunction::matlab_proxy(const std::string& toolboxPath, const std::string& wrapperName, - const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, - std::vector& functionNames) const { +void GlobalFunction::matlab_proxy(const std::string& toolboxPath, + const std::string& wrapperName, const TypeAttributesTable& typeAttributes, + FileWriter& file, std::vector& functionNames) const { // cluster overloads with same namespace // create new GlobalFunction structures around namespaces - same namespaces and names are overloads // map of namespace to global function typedef map GlobalFunctionMap; GlobalFunctionMap grouped_functions; - for (size_t i=0; i& functionNames) const { +void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, + const std::string& wrapperName, const TypeAttributesTable& typeAttributes, + FileWriter& file, std::vector& functionNames) const { // create the folder for the namespace const StrVec& ns = namespaces.front(); @@ -68,84 +70,68 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, cons // open destination mfunctionFileName string mfunctionFileName = toolboxPath; - if(!ns.empty()) + if (!ns.empty()) mfunctionFileName += "/+" + wrap::qualifiedName("/+", ns); mfunctionFileName += "/" + name + ".m"; FileWriter mfunctionFile(mfunctionFileName, verbose_, "%"); // get the name of actual matlab object - const string - matlabQualName = qualifiedName(".", ns, name), - matlabUniqueName = qualifiedName("", ns, name), - cppName = qualifiedName("::", ns, name); + const string matlabQualName = qualifiedName(".", ns, name), matlabUniqueName = + qualifiedName("", ns, name), cppName = qualifiedName("::", ns, name); mfunctionFile.oss << "function varargout = " << name << "(varargin)\n"; - for(size_t overload = 0; overload < argLists.size(); ++overload) { + for (size_t overload = 0; overload < argLists.size(); ++overload) { const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; - size_t nrArgs = args.size(); const int id = functionNames.size(); // Output proxy matlab code - - // check for number of arguments... - mfunctionFile.oss << (overload==0?"":"else") << "if length(varargin) == " << nrArgs; - if (nrArgs>0) mfunctionFile.oss << " && "; - // ...and their types - bool first = true; - for(size_t i=0;i(id); + const string wrapFunctionName = matlabUniqueName + "_" + + boost::lexical_cast(id); // call - wrapperFile.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + file.oss << "void " << wrapFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; // start - wrapperFile.oss << "{\n"; + file.oss << "{\n"; - returnVal.wrapTypeUnwrap(wrapperFile); + returnVal.wrapTypeUnwrap(file); // check arguments // NOTE: for static functions, there is no object passed - wrapperFile.oss << " checkArguments(\"" << matlabUniqueName << "\",nargout,nargin," << args.size() << ");\n"; + file.oss << " checkArguments(\"" << matlabUniqueName + << "\",nargout,nargin," << args.size() << ");\n"; // unwrap arguments, see Argument.cpp - args.matlab_unwrap(wrapperFile,0); // We start at 0 because there is no self object + args.matlab_unwrap(file, 0); // We start at 0 because there is no self object // call method with default type and wrap result - if (returnVal.type1!="void") - returnVal.wrap_result(cppName+"("+args.names()+")", wrapperFile, typeAttributes); + if (returnVal.type1 != "void") + returnVal.wrap_result(cppName + "(" + args.names() + ")", file, + typeAttributes); else - wrapperFile.oss << cppName+"("+args.names()+");\n"; + file.oss << cppName + "(" + args.names() + ");\n"; // finish - wrapperFile.oss << "}\n"; + file.oss << "}\n"; // Add to function list functionNames.push_back(wrapFunctionName); } - mfunctionFile.oss << "else\n"; - mfunctionFile.oss << " error('Arguments do not match any overload of function " << matlabQualName << "');" << endl; - mfunctionFile.oss << "end" << endl; + mfunctionFile.oss << " else\n"; + mfunctionFile.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << "');" << endl; + mfunctionFile.oss << " end" << endl; // Close file mfunctionFile.emit(true); @@ -153,9 +139,5 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, cons /* ************************************************************************* */ - } // \namespace wrap - - - diff --git a/wrap/GlobalFunction.h b/wrap/GlobalFunction.h index 2ecaf4998..6c8ad0c86 100644 --- a/wrap/GlobalFunction.h +++ b/wrap/GlobalFunction.h @@ -22,37 +22,38 @@ struct GlobalFunction { std::string name; // each overload, regardless of namespace - std::vector argLists; ///< arugments for each overload - std::vector returnVals; ///< returnVals for each overload - std::vector namespaces; ///< Stack of namespaces + std::vector argLists; ///< arugments for each overload + std::vector returnVals; ///< returnVals for each overload + std::vector namespaces; ///< Stack of namespaces // Constructor only used in Module - GlobalFunction(bool verbose = true) : verbose_(verbose) {} + GlobalFunction(bool verbose = true) : + verbose_(verbose) { + } // Used to reconstruct - GlobalFunction(const std::string& name_, bool verbose = true) - : verbose_(verbose), name(name_) {} + GlobalFunction(const std::string& name_, bool verbose = true) : + verbose_(verbose), name(name_) { + } // adds an overloaded version of this function void addOverload(bool verbose, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal, const StrVec& ns_stack); + const ArgumentList& args, const ReturnValue& retVal, + const StrVec& ns_stack); // codegen function called from Module to build the cpp and matlab versions of the function - void matlab_proxy(const std::string& toolboxPath, const std::string& wrapperName, - const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, - std::vector& functionNames) const; + void matlab_proxy(const std::string& toolboxPath, + const std::string& wrapperName, const TypeAttributesTable& typeAttributes, + FileWriter& file, std::vector& functionNames) const; private: // Creates a single global function - all in same namespace - void generateSingleFunction(const std::string& toolboxPath, const std::string& wrapperName, - const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, - std::vector& functionNames) const; + void generateSingleFunction(const std::string& toolboxPath, + const std::string& wrapperName, const TypeAttributesTable& typeAttributes, + FileWriter& file, std::vector& functionNames) const; }; } // \namespace wrap - - - diff --git a/wrap/Method.cpp b/wrap/Method.cpp index b327ac7dc..7b88b9cdc 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -15,14 +15,15 @@ * @author Richard Roberts **/ -#include -#include +#include "Method.h" +#include "utilities.h" #include #include +#include -#include "Method.h" -#include "utilities.h" +#include +#include using namespace std; using namespace wrap; @@ -38,155 +39,140 @@ void Method::addOverload(bool verbose, bool is_const, const std::string& name, } /* ************************************************************************* */ -void Method::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const string& cppClassName, - const std::string& matlabQualName, - const std::string& matlabUniqueName, - const string& wrapperName, - const TypeAttributesTable& typeAttributes, - vector& functionNames) const { +void Method::proxy_wrapper_fragments(FileWriter& file, FileWriter& wrapperFile, + const string& cppClassName, const std::string& matlabQualName, + const std::string& matlabUniqueName, const string& wrapperName, + const TypeAttributesTable& typeAttributes, + vector& functionNames) const { - proxyFile.oss << " function varargout = " << name << "(this, varargin)\n"; - //Comments for documentation - string up_name = boost::to_upper_copy(name); - proxyFile.oss << " % " << up_name << " usage:"; + // Create function header + file.oss << " function varargout = " << name << "(this, varargin)\n"; + + // Emit comments for documentation + string up_name = boost::to_upper_copy(name); + file.oss << " % " << up_name << " usage: "; unsigned int argLCount = 0; - BOOST_FOREACH(ArgumentList argList, argLists) - { - proxyFile.oss << " " << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) - { - if(i != argList.size()-1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - if(argLCount != argLists.size()-1) - proxyFile.oss << "), "; - else - proxyFile.oss << ") : returns " << returnVals[0].return_type(false, returnVals[0].pair) << endl; - argLCount++; - } - - proxyFile.oss << " % " << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" << endl; - proxyFile.oss << " % " << "" << endl; - proxyFile.oss << " % " << "Method Overloads" << endl; - BOOST_FOREACH(ArgumentList argList, argLists) - { - proxyFile.oss << " % " << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) - { - if(i != argList.size()-1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ")" << endl; - } - - for(size_t overload = 0; overload < argLists.size(); ++overload) { - const ArgumentList& args = argLists[overload]; - const ReturnValue& returnVal = returnVals[overload]; - size_t nrArgs = args.size(); - - const int id = functionNames.size(); - - // Output proxy matlab code - - // check for number of arguments... - proxyFile.oss << " " << (overload==0?"":"else") << "if length(varargin) == " << nrArgs; - if (nrArgs>0) proxyFile.oss << " && "; - // ...and their types - bool first = true; - for(size_t i=0;i 1) { + file.oss << " % " << "" << endl; + file.oss << " % " << "Method Overloads" << endl; + BOOST_FOREACH(ArgumentList argList, argLists) { + file.oss << " % "; + argList.emit_prototype(file, name); + file.oss << endl; + } + } + + // Handle special case of single overload with all numeric arguments + if (argLists.size() == 1 && argLists[0].allScalar()) { + // Output proxy matlab code + file.oss << " "; + const int id = (int) functionNames.size(); + argLists[0].emit_call(file, returnVals[0], wrapperName, id); // Output C++ wrapper code - - const string wrapFunctionName = wrapper_fragment( - wrapperFile, cppClassName, matlabUniqueName, overload, id, typeAttributes); + const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName, + matlabUniqueName, 0, id, typeAttributes); // Add to function list functionNames.push_back(wrapFunctionName); + } else { + // Check arguments for all overloads + for (size_t overload = 0; overload < argLists.size(); ++overload) { + // Output proxy matlab code + file.oss << " " << (overload == 0 ? "" : "else"); + const int id = (int) functionNames.size(); + argLists[overload].emit_conditional_call(file, returnVals[overload], + wrapperName, id); + + // Output C++ wrapper code + const string wrapFunctionName = wrapper_fragment(wrapperFile, + cppClassName, matlabUniqueName, overload, id, typeAttributes); + + // Add to function list + functionNames.push_back(wrapFunctionName); + } + file.oss << " else\n"; + file.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << "." << name << "');" << endl; + file.oss << " end\n"; } - - proxyFile.oss << " else\n"; - proxyFile.oss << " error('Arguments do not match any overload of function " << - matlabQualName << "." << name << "');" << endl; - proxyFile.oss << " end\n"; - proxyFile.oss << " end\n"; + file.oss << " end\n"; } /* ************************************************************************* */ -string Method::wrapper_fragment(FileWriter& file, - const string& cppClassName, - const string& matlabUniqueName, - int overload, - int id, - const TypeAttributesTable& typeAttributes) const { +string Method::wrapper_fragment(FileWriter& file, const string& cppClassName, + const string& matlabUniqueName, int overload, int id, + const TypeAttributesTable& typeAttributes) const { // generate code - const string wrapFunctionName = matlabUniqueName + "_" + name + "_" + boost::lexical_cast(id); + const string wrapFunctionName = matlabUniqueName + "_" + name + "_" + + boost::lexical_cast(id); const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; // call - file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + file.oss << "void " << wrapFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; // start file.oss << "{\n"; - if(returnVal.isPair) - { - if(returnVal.category1 == ReturnValue::CLASS) - file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 << ";"<< endl; - if(returnVal.category2 == ReturnValue::CLASS) - file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType2("::") << "> Shared" << returnVal.type2 << ";"<< endl; - } - else - if(returnVal.category1 == ReturnValue::CLASS) - file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 << ";"<< endl; + if (returnVal.isPair) { + if (returnVal.category1 == ReturnValue::CLASS) + file.oss << " typedef boost::shared_ptr<" + << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 + << ";" << endl; + if (returnVal.category2 == ReturnValue::CLASS) + file.oss << " typedef boost::shared_ptr<" + << returnVal.qualifiedType2("::") << "> Shared" << returnVal.type2 + << ";" << endl; + } else if (returnVal.category1 == ReturnValue::CLASS) + file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") + << "> Shared" << returnVal.type1 << ";" << endl; - file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; + file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" + << endl; // check arguments // extra argument obj -> nargin-1 is passed ! // example: checkArguments("equals",nargout,nargin-1,2); - file.oss << " checkArguments(\"" << name << "\",nargout,nargin-1," << args.size() << ");\n"; + file.oss << " checkArguments(\"" << name << "\",nargout,nargin-1," + << args.size() << ");\n"; // get class pointer // example: shared_ptr = unwrap_shared_ptr< Test >(in[0], "Test"); - file.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; + file.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName + << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; // unwrap arguments, see Argument.cpp - args.matlab_unwrap(file,1); + args.matlab_unwrap(file, 1); // call method and wrap result // example: out[0]=wrap(self->return_field(t)); - if (returnVal.type1!="void") - returnVal.wrap_result("obj->"+name+"("+args.names()+")", file, typeAttributes); + if (returnVal.type1 != "void") + returnVal.wrap_result("obj->" + name + "(" + args.names() + ")", file, + typeAttributes); else - file.oss << " obj->"+name+"("+args.names()+");\n"; + file.oss << " obj->" + name + "(" + args.names() + ");\n"; // finish file.oss << "}\n"; diff --git a/wrap/Method.h b/wrap/Method.h index f2a7ed190..9926a5179 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -18,13 +18,12 @@ #pragma once -#include -#include - #include "Argument.h" #include "ReturnValue.h" #include "TypeAttributesTable.h" -#include + +#include +#include namespace wrap { diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 6870d5178..2cb5ea7ed 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -98,7 +98,7 @@ void Module::parseMarkup(const std::string& data) { // The one with postfix 0 are used to reset the variables after parse. string methodName, methodName0; bool isConst, isConst0 = false; - ReturnValue retVal0(verbose), retVal(verbose); + ReturnValue retVal0, retVal; Argument arg0, arg; ArgumentList args0, args; vector arg_dup; ///keep track of duplicates diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index 3cb68181b..78e36d4da 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -141,5 +141,13 @@ void ReturnValue::wrapTypeUnwrap(FileWriter& wrapperFile) const { } } /* ************************************************************************* */ +void ReturnValue::emit_matlab(FileWriter& file) const { + string output; + if (isPair) + file.oss << "[ varargout{1} varargout{2} ] = "; + else if (category1 != ReturnValue::VOID) + file.oss << "varargout{1} = "; +} +/* ************************************************************************* */ diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index 7a677532f..989f81109 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -8,36 +8,36 @@ * @author Richard Roberts */ -#include -#include - #include "FileWriter.h" #include "TypeAttributesTable.h" +#include + #pragma once namespace wrap { +/** + * Encapsulates return value of a method or function + */ struct ReturnValue { + /// the different supported return value categories typedef enum { - CLASS = 1, - EIGEN = 2, - BASIS = 3, - VOID = 4 + CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4 } return_category; - ReturnValue(bool enable_verbosity = true) - : verbose(enable_verbosity), isPtr1(false), isPtr2(false), - isPair(false), category1(CLASS), category2(CLASS) - {} - - bool verbose; bool isPtr1, isPtr2, isPair; return_category category1, category2; std::string type1, type2; std::vector namespaces1, namespaces2; + /// Constructor + ReturnValue() : + isPtr1(false), isPtr2(false), isPair(false), category1(CLASS), category2( + CLASS) { + } + typedef enum { arg1, arg2, pair } pairing; @@ -49,10 +49,12 @@ struct ReturnValue { std::string matlab_returnType() const; - void wrap_result(const std::string& result, FileWriter& file, const TypeAttributesTable& typeAttributes) const; + void wrap_result(const std::string& result, FileWriter& file, + const TypeAttributesTable& typeAttributes) const; void wrapTypeUnwrap(FileWriter& wrapperFile) const; + void emit_matlab(FileWriter& file) const; }; } // \namespace wrap diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index e5c10b4c8..0c4cc7d75 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -16,19 +16,19 @@ * @author Richard Roberts **/ -#include -#include - -#include -#include - #include "StaticMethod.h" #include "utilities.h" +#include +#include +#include + +#include +#include + using namespace std; using namespace wrap; - /* ************************************************************************* */ void StaticMethod::addOverload(bool verbose, const std::string& name, const ArgumentList& args, const ReturnValue& retVal) { @@ -39,144 +39,103 @@ void StaticMethod::addOverload(bool verbose, const std::string& name, } /* ************************************************************************* */ -void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const string& cppClassName, - const std::string& matlabQualName, - const std::string& matlabUniqueName, - const string& wrapperName, - const TypeAttributesTable& typeAttributes, - vector& functionNames) const { +void StaticMethod::proxy_wrapper_fragments(FileWriter& file, + FileWriter& wrapperFile, const string& cppClassName, + const std::string& matlabQualName, const std::string& matlabUniqueName, + const string& wrapperName, const TypeAttributesTable& typeAttributes, + vector& functionNames) const { - string upperName = name; upperName[0] = std::toupper(upperName[0], std::locale()); + string upperName = name; + upperName[0] = std::toupper(upperName[0], std::locale()); - proxyFile.oss << " function varargout = " << upperName << "(varargin)\n"; + file.oss << " function varargout = " << upperName << "(varargin)\n"; //Comments for documentation - string up_name = boost::to_upper_copy(name); - proxyFile.oss << " % " << up_name << " usage:"; + string up_name = boost::to_upper_copy(name); + file.oss << " % " << up_name << " usage: "; unsigned int argLCount = 0; - BOOST_FOREACH(ArgumentList argList, argLists) - { - proxyFile.oss << " " << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) - { - if(i != argList.size()-1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - if(argLCount != argLists.size()-1) - proxyFile.oss << "), "; - else - proxyFile.oss << ") : returns " << returnVals[0].return_type(false, returnVals[0].pair) << endl; - argLCount++; - } + BOOST_FOREACH(ArgumentList argList, argLists) { + argList.emit_prototype(file, name); + if (argLCount != argLists.size() - 1) + file.oss << ", "; + else + file.oss << " : returns " + << returnVals[0].return_type(false, returnVals[0].pair) << endl; + argLCount++; + } - proxyFile.oss << " % " << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" << endl; - proxyFile.oss << " % " << "" << endl; - proxyFile.oss << " % " << "Usage" << endl; - BOOST_FOREACH(ArgumentList argList, argLists) - { - proxyFile.oss << " % " << up_name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) - { - if(i != argList.size()-1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ")" << endl; - } + file.oss << " % " + << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" + << endl; + file.oss << " % " << "" << endl; + file.oss << " % " << "Usage" << endl; + BOOST_FOREACH(ArgumentList argList, argLists) { + file.oss << " % "; + argList.emit_prototype(file, up_name); + file.oss << endl; + } - - for(size_t overload = 0; overload < argLists.size(); ++overload) { - const ArgumentList& args = argLists[overload]; - const ReturnValue& returnVal = returnVals[overload]; - size_t nrArgs = args.size(); - - const int id = (int)functionNames.size(); + // Check arguments for all overloads + for (size_t overload = 0; overload < argLists.size(); ++overload) { // Output proxy matlab code - - // check for number of arguments... - proxyFile.oss << " " << (overload==0?"":"else") << "if length(varargin) == " << nrArgs; - if (nrArgs>0) proxyFile.oss << " && "; - // ...and their types - bool first = true; - for(size_t i=0;i(id); + const string wrapFunctionName = matlabUniqueName + "_" + name + "_" + + boost::lexical_cast(id); const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; // call - file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + file.oss << "void " << wrapFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; // start file.oss << "{\n"; returnVal.wrapTypeUnwrap(file); - file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; + file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" + << endl; // check arguments // NOTE: for static functions, there is no object passed - file.oss << " checkArguments(\"" << matlabUniqueName << "." << name << "\",nargout,nargin," << args.size() << ");\n"; + file.oss << " checkArguments(\"" << matlabUniqueName << "." << name + << "\",nargout,nargin," << args.size() << ");\n"; // unwrap arguments, see Argument.cpp - args.matlab_unwrap(file,0); // We start at 0 because there is no self object + args.matlab_unwrap(file, 0); // We start at 0 because there is no self object // call method with default type and wrap result - if (returnVal.type1!="void") - returnVal.wrap_result(cppClassName+"::"+name+"("+args.names()+")", file, typeAttributes); + if (returnVal.type1 != "void") + returnVal.wrap_result(cppClassName + "::" + name + "(" + args.names() + ")", + file, typeAttributes); else - file.oss << cppClassName+"::"+name+"("+args.names()+");\n"; + file.oss << cppClassName + "::" + name + "(" + args.names() + ");\n"; // finish file.oss << "}\n"; diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index 3e8dc08cf..e1855f4c2 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -19,13 +19,9 @@ #pragma once -#include -#include - #include "Argument.h" #include "ReturnValue.h" #include "TypeAttributesTable.h" -#include namespace wrap { @@ -34,7 +30,8 @@ struct StaticMethod { /// Constructor creates empty object StaticMethod(bool verbosity = true) : - verbose(verbosity) {} + verbose(verbosity) { + } // Then the instance variables are set directly by the Module constructor bool verbose; @@ -46,22 +43,20 @@ struct StaticMethod { // with those in rhs, but in subsequent calls it adds additional argument // lists as function overloads. void addOverload(bool verbose, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal); + const ArgumentList& args, const ReturnValue& retVal); // MATLAB code generation // classPath is class directory, e.g., ../matlab/@Point2 void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const std::string& cppClassName, const std::string& matlabQualName, const std::string& matlabUniqueName, - const std::string& wrapperName, const TypeAttributesTable& typeAttributes, - std::vector& functionNames) const; + const std::string& cppClassName, const std::string& matlabQualName, + const std::string& matlabUniqueName, const std::string& wrapperName, + const TypeAttributesTable& typeAttributes, + std::vector& functionNames) const; private: std::string wrapper_fragment(FileWriter& file, - const std::string& cppClassName, - const std::string& matlabUniqueName, - int overload, - int id, - const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper + const std::string& cppClassName, const std::string& matlabUniqueName, + int overload, int id, const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper }; } // \namespace wrap diff --git a/wrap/tests/expected/.gitignore b/wrap/tests/expected/.gitignore new file mode 100644 index 000000000..6d725d9bc --- /dev/null +++ b/wrap/tests/expected/.gitignore @@ -0,0 +1 @@ +*.m~ diff --git a/wrap/tests/expected/Point2.m b/wrap/tests/expected/Point2.m index 22dec9641..9f64f2d10 100644 --- a/wrap/tests/expected/Point2.m +++ b/wrap/tests/expected/Point2.m @@ -44,92 +44,43 @@ classdef Point2 < handle function varargout = argChar(this, varargin) % ARGCHAR usage: argChar(char a) : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % argChar(char a) - if length(varargin) == 1 && isa(varargin{1},'char') - geometry_wrapper(4, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.argChar'); - end + geometry_wrapper(4, this, varargin{:}); end function varargout = argUChar(this, varargin) % ARGUCHAR usage: argUChar(unsigned char a) : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % argUChar(unsigned char a) - if length(varargin) == 1 && isa(varargin{1},'char') - geometry_wrapper(5, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.argUChar'); - end + geometry_wrapper(5, this, varargin{:}); end function varargout = dim(this, varargin) % DIM usage: dim() : returns int % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % dim() - if length(varargin) == 0 - varargout{1} = geometry_wrapper(6, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.dim'); - end + varargout{1} = geometry_wrapper(6, this, varargin{:}); end function varargout = returnChar(this, varargin) % RETURNCHAR usage: returnChar() : returns char % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % returnChar() - if length(varargin) == 0 - varargout{1} = geometry_wrapper(7, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.returnChar'); - end + varargout{1} = geometry_wrapper(7, this, varargin{:}); end function varargout = vectorConfusion(this, varargin) % VECTORCONFUSION usage: vectorConfusion() : returns VectorNotEigen % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % vectorConfusion() - if length(varargin) == 0 - varargout{1} = geometry_wrapper(8, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.vectorConfusion'); - end + varargout{1} = geometry_wrapper(8, this, varargin{:}); end function varargout = x(this, varargin) % X usage: x() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % x() - if length(varargin) == 0 - varargout{1} = geometry_wrapper(9, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.x'); - end + varargout{1} = geometry_wrapper(9, this, varargin{:}); end function varargout = y(this, varargin) % Y usage: y() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % y() - if length(varargin) == 0 - varargout{1} = geometry_wrapper(10, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.y'); - end + varargout{1} = geometry_wrapper(10, this, varargin{:}); end end diff --git a/wrap/tests/expected/Point3.m b/wrap/tests/expected/Point3.m index 94d9c25b8..8a43987b9 100644 --- a/wrap/tests/expected/Point3.m +++ b/wrap/tests/expected/Point3.m @@ -43,14 +43,7 @@ classdef Point3 < handle function varargout = norm(this, varargin) % NORM usage: norm() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % norm() - if length(varargin) == 0 - varargout{1} = geometry_wrapper(14, this, varargin{:}); - else - error('Arguments do not match any overload of function Point3.norm'); - end + varargout{1} = geometry_wrapper(14, this, varargin{:}); end function varargout = string_serialize(this, varargin) diff --git a/wrap/tests/expected/Test.m b/wrap/tests/expected/Test.m index 8e56df6fc..1afd15efe 100644 --- a/wrap/tests/expected/Test.m +++ b/wrap/tests/expected/Test.m @@ -56,9 +56,6 @@ classdef Test < handle function varargout = arg_EigenConstRef(this, varargin) % ARG_EIGENCONSTREF usage: arg_EigenConstRef(Matrix value) : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % arg_EigenConstRef(Matrix value) if length(varargin) == 1 && isa(varargin{1},'double') geometry_wrapper(23, this, varargin{:}); else @@ -69,61 +66,30 @@ classdef Test < handle function varargout = create_MixedPtrs(this, varargin) % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Test, SharedTest > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % create_MixedPtrs() - if length(varargin) == 0 - [ varargout{1} varargout{2} ] = geometry_wrapper(24, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.create_MixedPtrs'); - end + [ varargout{1} varargout{2} ] = geometry_wrapper(24, this, varargin{:}); end function varargout = create_ptrs(this, varargin) % CREATE_PTRS usage: create_ptrs() : returns pair< SharedTest, SharedTest > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % create_ptrs() - if length(varargin) == 0 - [ varargout{1} varargout{2} ] = geometry_wrapper(25, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.create_ptrs'); - end + [ varargout{1} varargout{2} ] = geometry_wrapper(25, this, varargin{:}); end function varargout = print(this, varargin) % PRINT usage: print() : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % print() - if length(varargin) == 0 - geometry_wrapper(26, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.print'); - end + geometry_wrapper(26, this, varargin{:}); end function varargout = return_Point2Ptr(this, varargin) % RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns Point2 % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_Point2Ptr(bool value) - if length(varargin) == 1 && isa(varargin{1},'logical') - varargout{1} = geometry_wrapper(27, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_Point2Ptr'); - end + varargout{1} = geometry_wrapper(27, this, varargin{:}); end function varargout = return_Test(this, varargin) % RETURN_TEST usage: return_Test(Test value) : returns Test % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_Test(Test value) if length(varargin) == 1 && isa(varargin{1},'Test') varargout{1} = geometry_wrapper(28, this, varargin{:}); else @@ -134,9 +100,6 @@ classdef Test < handle function varargout = return_TestPtr(this, varargin) % RETURN_TESTPTR usage: return_TestPtr(Test value) : returns Test % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_TestPtr(Test value) if length(varargin) == 1 && isa(varargin{1},'Test') varargout{1} = geometry_wrapper(29, this, varargin{:}); else @@ -147,35 +110,18 @@ classdef Test < handle function varargout = return_bool(this, varargin) % RETURN_BOOL usage: return_bool(bool value) : returns bool % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_bool(bool value) - if length(varargin) == 1 && isa(varargin{1},'logical') - varargout{1} = geometry_wrapper(30, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_bool'); - end + varargout{1} = geometry_wrapper(30, this, varargin{:}); end function varargout = return_double(this, varargin) % RETURN_DOUBLE usage: return_double(double value) : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_double(double value) - if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = geometry_wrapper(31, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_double'); - end + varargout{1} = geometry_wrapper(31, this, varargin{:}); end function varargout = return_field(this, varargin) % RETURN_FIELD usage: return_field(Test t) : returns bool % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_field(Test t) if length(varargin) == 1 && isa(varargin{1},'Test') varargout{1} = geometry_wrapper(32, this, varargin{:}); else @@ -186,22 +132,12 @@ classdef Test < handle function varargout = return_int(this, varargin) % RETURN_INT usage: return_int(int value) : returns int % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_int(int value) - if length(varargin) == 1 && isa(varargin{1},'numeric') - varargout{1} = geometry_wrapper(33, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_int'); - end + varargout{1} = geometry_wrapper(33, this, varargin{:}); end function varargout = return_matrix1(this, varargin) % RETURN_MATRIX1 usage: return_matrix1(Matrix value) : returns Matrix % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_matrix1(Matrix value) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = geometry_wrapper(34, this, varargin{:}); else @@ -212,9 +148,6 @@ classdef Test < handle function varargout = return_matrix2(this, varargin) % RETURN_MATRIX2 usage: return_matrix2(Matrix value) : returns Matrix % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_matrix2(Matrix value) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = geometry_wrapper(35, this, varargin{:}); else @@ -225,9 +158,6 @@ classdef Test < handle function varargout = return_pair(this, varargin) % RETURN_PAIR usage: return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_pair(Vector v, Matrix A) if length(varargin) == 2 && isa(varargin{1},'double') && isa(varargin{2},'double') [ varargout{1} varargout{2} ] = geometry_wrapper(36, this, varargin{:}); else @@ -238,9 +168,6 @@ classdef Test < handle function varargout = return_ptrs(this, varargin) % RETURN_PTRS usage: return_ptrs(Test p1, Test p2) : returns pair< SharedTest, SharedTest > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_ptrs(Test p1, Test p2) if length(varargin) == 2 && isa(varargin{1},'Test') && isa(varargin{2},'Test') [ varargout{1} varargout{2} ] = geometry_wrapper(37, this, varargin{:}); else @@ -251,22 +178,12 @@ classdef Test < handle function varargout = return_size_t(this, varargin) % RETURN_SIZE_T usage: return_size_t(size_t value) : returns size_t % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_size_t(size_t value) - if length(varargin) == 1 && isa(varargin{1},'numeric') - varargout{1} = geometry_wrapper(38, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_size_t'); - end + varargout{1} = geometry_wrapper(38, this, varargin{:}); end function varargout = return_string(this, varargin) % RETURN_STRING usage: return_string(string value) : returns string % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_string(string value) if length(varargin) == 1 && isa(varargin{1},'char') varargout{1} = geometry_wrapper(39, this, varargin{:}); else @@ -277,9 +194,6 @@ classdef Test < handle function varargout = return_vector1(this, varargin) % RETURN_VECTOR1 usage: return_vector1(Vector value) : returns Vector % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_vector1(Vector value) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = geometry_wrapper(40, this, varargin{:}); else @@ -290,9 +204,6 @@ classdef Test < handle function varargout = return_vector2(this, varargin) % RETURN_VECTOR2 usage: return_vector2(Vector value) : returns Vector % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_vector2(Vector value) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = geometry_wrapper(41, this, varargin{:}); else diff --git a/wrap/tests/expected/aGlobalFunction.m b/wrap/tests/expected/aGlobalFunction.m index 09ece0b83..94e9b4a67 100644 --- a/wrap/tests/expected/aGlobalFunction.m +++ b/wrap/tests/expected/aGlobalFunction.m @@ -1,6 +1,6 @@ function varargout = aGlobalFunction(varargin) -if length(varargin) == 0 - varargout{1} = geometry_wrapper(42, varargin{:}); -else - error('Arguments do not match any overload of function aGlobalFunction'); -end + if length(varargin) == 0 + varargout{1} = geometry_wrapper(42, varargin{:}); + else + error('Arguments do not match any overload of function aGlobalFunction'); + end diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp index e00004d57..b34112718 100644 --- a/wrap/tests/expected/geometry_wrapper.cpp +++ b/wrap/tests/expected/geometry_wrapper.cpp @@ -502,6 +502,19 @@ void aGlobalFunction_42(int nargout, mxArray *out[], int nargin, const mxArray * checkArguments("aGlobalFunction",nargout,nargin,0); out[0] = wrap< Vector >(aGlobalFunction()); } +void overloadedGlobalFunction_43(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("overloadedGlobalFunction",nargout,nargin,1); + int a = unwrap< int >(in[0]); + out[0] = wrap< Vector >(overloadedGlobalFunction(a)); +} +void overloadedGlobalFunction_44(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("overloadedGlobalFunction",nargout,nargin,2); + int a = unwrap< int >(in[0]); + double b = unwrap< double >(in[1]); + out[0] = wrap< Vector >(overloadedGlobalFunction(a,b)); +} void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { @@ -643,6 +656,12 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) case 42: aGlobalFunction_42(nargout, out, nargin-1, in+1); break; + case 43: + overloadedGlobalFunction_43(nargout, out, nargin-1, in+1); + break; + case 44: + overloadedGlobalFunction_44(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/overloadedGlobalFunction.m b/wrap/tests/expected/overloadedGlobalFunction.m new file mode 100644 index 000000000..5b086b15e --- /dev/null +++ b/wrap/tests/expected/overloadedGlobalFunction.m @@ -0,0 +1,8 @@ +function varargout = overloadedGlobalFunction(varargin) + if length(varargin) == 1 && isa(varargin{1},'numeric') + varargout{1} = geometry_wrapper(43, varargin{:}); + elseif length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double') + varargout{1} = geometry_wrapper(44, varargin{:}); + else + error('Arguments do not match any overload of function overloadedGlobalFunction'); + end diff --git a/wrap/tests/expected_namespaces/+ns2/ClassA.m b/wrap/tests/expected_namespaces/+ns2/ClassA.m index c7ab901ae..9c064734e 100644 --- a/wrap/tests/expected_namespaces/+ns2/ClassA.m +++ b/wrap/tests/expected_namespaces/+ns2/ClassA.m @@ -40,22 +40,12 @@ classdef ClassA < handle function varargout = memberFunction(this, varargin) % MEMBERFUNCTION usage: memberFunction() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % memberFunction() - if length(varargin) == 0 - varargout{1} = testNamespaces_wrapper(9, this, varargin{:}); - else - error('Arguments do not match any overload of function ns2.ClassA.memberFunction'); - end + varargout{1} = testNamespaces_wrapper(9, this, varargin{:}); end function varargout = nsArg(this, varargin) % NSARG usage: nsArg(ClassB arg) : returns int % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % nsArg(ClassB arg) if length(varargin) == 1 && isa(varargin{1},'ns1.ClassB') varargout{1} = testNamespaces_wrapper(10, this, varargin{:}); else @@ -66,14 +56,7 @@ classdef ClassA < handle function varargout = nsReturn(this, varargin) % NSRETURN usage: nsReturn(double q) : returns ns2::ns3::ClassB % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % nsReturn(double q) - if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = testNamespaces_wrapper(11, this, varargin{:}); - else - error('Arguments do not match any overload of function ns2.ClassA.nsReturn'); - end + varargout{1} = testNamespaces_wrapper(11, this, varargin{:}); end end diff --git a/wrap/tests/expected_namespaces/+ns2/overloadedGlobalFunction.m b/wrap/tests/expected_namespaces/+ns2/overloadedGlobalFunction.m new file mode 100644 index 000000000..84f3b8f47 --- /dev/null +++ b/wrap/tests/expected_namespaces/+ns2/overloadedGlobalFunction.m @@ -0,0 +1,8 @@ +function varargout = overloadedGlobalFunction(varargin) + if length(varargin) == 1 && isa(varargin{1},'ns1.ClassA') + varargout{1} = testNamespaces_wrapper(24, varargin{:}); + elseif length(varargin) == 2 && isa(varargin{1},'ns1.ClassA') && isa(varargin{2},'double') + varargout{1} = testNamespaces_wrapper(25, varargin{:}); + else + error('Arguments do not match any overload of function ns2.overloadedGlobalFunction'); + end diff --git a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp index 29739a2f6..6d22e1625 100644 --- a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp +++ b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp @@ -342,6 +342,21 @@ void ns2aGlobalFunction_23(int nargout, mxArray *out[], int nargin, const mxArra checkArguments("ns2aGlobalFunction",nargout,nargin,0); out[0] = wrap< Vector >(ns2::aGlobalFunction()); } +void ns2overloadedGlobalFunction_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedClassA; + checkArguments("ns2overloadedGlobalFunction",nargout,nargin,1); + ns1::ClassA& a = *unwrap_shared_ptr< ns1::ClassA >(in[0], "ptr_ns1ClassA"); + out[0] = wrap_shared_ptr(SharedClassA(new ns1::ClassA(ns2::overloadedGlobalFunction(a))),"ns1.ClassA", false); +} +void ns2overloadedGlobalFunction_25(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedClassA; + checkArguments("ns2overloadedGlobalFunction",nargout,nargin,2); + ns1::ClassA& a = *unwrap_shared_ptr< ns1::ClassA >(in[0], "ptr_ns1ClassA"); + double b = unwrap< double >(in[1]); + out[0] = wrap_shared_ptr(SharedClassA(new ns1::ClassA(ns2::overloadedGlobalFunction(a,b))),"ns1.ClassA", false); +} void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { @@ -426,6 +441,12 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) case 23: ns2aGlobalFunction_23(nargout, out, nargin-1, in+1); break; + case 24: + ns2overloadedGlobalFunction_24(nargout, out, nargin-1, in+1); + break; + case 25: + ns2overloadedGlobalFunction_25(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/geometry.h b/wrap/tests/geometry.h index bdced45ed..bc233763d 100644 --- a/wrap/tests/geometry.h +++ b/wrap/tests/geometry.h @@ -87,6 +87,10 @@ class Test { Vector aGlobalFunction(); +// An overloaded global function +Vector overloadedGlobalFunction(int a); +Vector overloadedGlobalFunction(int a, double b); + // comments at the end! // even more comments at the end! diff --git a/wrap/tests/testNamespaces.h b/wrap/tests/testNamespaces.h index 8e6ef51d6..a9b23cad1 100644 --- a/wrap/tests/testNamespaces.h +++ b/wrap/tests/testNamespaces.h @@ -47,6 +47,10 @@ class ClassC { // separate namespace global function, same name Vector aGlobalFunction(); +// An overloaded global function +ns1::ClassA overloadedGlobalFunction(const ns1::ClassA& a); +ns1::ClassA overloadedGlobalFunction(const ns1::ClassA& a, double b); + } //\namespace ns2 class ClassD { diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 8bf2c1412..c6ce0903a 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -15,16 +15,18 @@ * @author Frank Dellaert **/ +#include +#include + +#include + +#include +#include + #include #include #include #include -#include -#include -#include - -#include -#include using namespace std; using namespace boost::assign; @@ -305,8 +307,8 @@ TEST( wrap, parse_geometry ) { } // evaluate global functions -// Vector aGlobalFunction(); - LONGS_EQUAL(1, module.global_functions.size()); + // Vector aGlobalFunction(); + LONGS_EQUAL(2, module.global_functions.size()); CHECK(module.global_functions.find("aGlobalFunction") != module.global_functions.end()); { GlobalFunction gfunc = module.global_functions.at("aGlobalFunction"); @@ -380,7 +382,7 @@ TEST( wrap, parse_namespaces ) { // evaluate global functions // Vector ns1::aGlobalFunction(); // Vector ns2::aGlobalFunction(); - LONGS_EQUAL(1, module.global_functions.size()); + LONGS_EQUAL(2, module.global_functions.size()); CHECK(module.global_functions.find("aGlobalFunction") != module.global_functions.end()); { GlobalFunction gfunc = module.global_functions.at("aGlobalFunction"); @@ -415,13 +417,17 @@ TEST( wrap, matlab_code_namespaces ) { module.matlab_code("actual_namespaces", headerPath); - EXPECT(files_equal(exp_path + "ClassD.m" , act_path + "ClassD.m" )); - EXPECT(files_equal(exp_path + "+ns1/ClassA.m" , act_path + "+ns1/ClassA.m" )); - EXPECT(files_equal(exp_path + "+ns1/ClassB.m" , act_path + "+ns1/ClassB.m" )); - EXPECT(files_equal(exp_path + "+ns2/ClassA.m" , act_path + "+ns2/ClassA.m" )); - EXPECT(files_equal(exp_path + "+ns2/ClassC.m" , act_path + "+ns2/ClassC.m" )); - EXPECT(files_equal(exp_path + "+ns2/+ns3/ClassB.m" , act_path + "+ns2/+ns3/ClassB.m" )); - EXPECT(files_equal(exp_path + "testNamespaces_wrapper.cpp" , act_path + "testNamespaces_wrapper.cpp" )); + EXPECT(files_equal(exp_path + "ClassD.m", act_path + "ClassD.m" )); + EXPECT(files_equal(exp_path + "+ns1/ClassA.m", act_path + "+ns1/ClassA.m" )); + EXPECT(files_equal(exp_path + "+ns1/ClassB.m", act_path + "+ns1/ClassB.m" )); + EXPECT(files_equal(exp_path + "+ns2/ClassA.m", act_path + "+ns2/ClassA.m" )); + EXPECT(files_equal(exp_path + "+ns2/ClassC.m", act_path + "+ns2/ClassC.m" )); + EXPECT( + files_equal(exp_path + "+ns2/overloadedGlobalFunction.m", exp_path + "+ns2/overloadedGlobalFunction.m" )); + EXPECT( + files_equal(exp_path + "+ns2/+ns3/ClassB.m", act_path + "+ns2/+ns3/ClassB.m" )); + EXPECT( + files_equal(exp_path + "testNamespaces_wrapper.cpp", act_path + "testNamespaces_wrapper.cpp" )); } /* ************************************************************************* */ @@ -445,6 +451,7 @@ TEST( wrap, matlab_code_geometry ) { EXPECT(files_equal(epath + "Point3.m" , apath + "Point3.m" )); EXPECT(files_equal(epath + "Test.m" , apath + "Test.m" )); EXPECT(files_equal(epath + "aGlobalFunction.m" , apath + "aGlobalFunction.m" )); + EXPECT(files_equal(epath + "overloadedGlobalFunction.m" , apath + "overloadedGlobalFunction.m" )); } /* ************************************************************************* */ diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 1acc50db1..47f7d10a6 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -88,9 +88,11 @@ bool files_equal(const string& expected, const string& actual, bool skipheader) string actual_contents = file_contents(actual, skipheader); bool equal = actual_contents == expected_contents; if (!equal) { + cerr << "<<< DIFF OUTPUT (if none, white-space differences only):\n"; stringstream command; - command << "diff " << actual << " " << expected << endl; + command << "diff --ignore-all-space " << expected << " " << actual << endl; system(command.str().c_str()); + cerr << ">>>\n"; } return equal; }